diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 859b5df..af3a063 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,55 @@ */ public final class Constants { public class DriveConstants { + // The maximum linear velocity of the robot in meters per second. + // This represents the highest speed at which the robot can move forward or backward. public static double maxLinearVelocityMetersPerSec = 1.0; + + // The maximum angular velocity of the robot in radians per second. + // This represents the highest speed at which the robot can rotate in place. public static double maxAngularVelocityRadPerSec = 4 * 2 * Math.PI; + + // The track width of the robot in meters. + // This is the distance between the left and right wheels of the robot. + public static double trackWidthMeters = 0.155; + + // The diameter of the robot's wheels in meters (60 mm). + public static double wheelDiameterMeters = 0.060; + + public class speedPIDConstants { + // The proportional gain for the PID controller. + // This value determines how strongly the controller reacts to the current error. + // A higher kP value will result in a stronger response to the error, but may cause overshooting. + public static double kP = .5; + + // The integral gain for the PID controller. + // This value determines how strongly the controller reacts to the accumulation of past errors. + // A higher kI value will result in a stronger response to accumulated errors, but may cause instability. + public static double kI = 0.00; + + // The derivative gain for the PID controller. + // This value determines how strongly the controller reacts to the rate of change of the error. + // A higher kD value will result in a stronger response to the rate of change, helping to dampen oscillations. + public static double kD = .5; + } + + public class zRotationPIDConstants { + // The proportional gain for the PID controller. + // This value determines how strongly the controller reacts to the current error. + // A higher kP value will result in a stronger response to the error, but may cause overshooting. + public static double kP = 0.01; + + // The integral gain for the PID controller. + // This value determines how strongly the controller reacts to the accumulation of past errors. + // A higher kI value will result in a stronger response to accumulated errors, but may cause instability. + public static double kI = 0.0; + + // The derivative gain for the PID controller. + // This value determines how strongly the controller reacts to the rate of change of the error. + // A higher kD value will result in a stronger response to the rate of change, helping to dampen oscillations. + public static double kD = 0.01; + } + + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ed4564e..74be256 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.ArcadeDrive; +// This is neeeded if you are using TankDrive +// import frc.robot.commands.TankDrive; import frc.robot.commands.ArmCommand; +import frc.robot.commands.AutonomousCommand; import frc.robot.commands.IntakeCommand; import frc.robot.commands.ShooterCommand; import frc.robot.subsystems.arm.ArmIOXrp; @@ -19,6 +22,7 @@ import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterIOXrp; import frc.robot.subsystems.shooter.ShooterSubsystem; +import org.littletonrobotics.junction.Logger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -27,10 +31,11 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { + // The robot's subsystems and commands are defined here... private final DriveSubsystemXrp drive = new DriveSubsystemXrp(new DriveDifferentialIOXrp()); - private final ArmSubsystem arm = new ArmSubsystem(new ArmIOXrp(4)); - private final IntakeSubsystem intake = new IntakeSubsystem(new IntakeIOXrp(5)); + private final ArmSubsystem arm = new ArmSubsystem(new ArmIOXrp(4), new ArmIOXrp(5)); + //private final IntakeSubsystem intake = new IntakeSubsystem(new IntakeIOXrp(5)); private final ShooterSubsystem shooter = new ShooterSubsystem(new ShooterIOXrp(2)); private final CommandXboxController mainController = new CommandXboxController(0); @@ -48,11 +53,28 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + + // Configure the robot to be driven by the left stick for steering and for + // throttle drive.setDefaultCommand( new ArcadeDrive(drive, () -> -mainController.getLeftY(), () -> -mainController.getLeftX())); + // // Arcade drive but left stick for throttle and right stick controls steering + // drive.setDefaultCommand( + // new ArcadeDrive(drive, + // () -> -mainController.getLeftY(), + // () -> -mainController.getRightX())); + + // Both sticks control steering using TankDrive + // drive.setDefaultCommand( + // new TankDrive(drive, + // () -> -mainController.getLeftY(), + // () -> -mainController.getRightY())); + arm.setDefaultCommand(new ArmCommand(arm, () -> -mainController.getRightY())); - intake.setDefaultCommand(new IntakeCommand(intake, () -> -mainController.getRightX())); + + //intake.setDefaultCommand(new IntakeCommand(intake, () -> -mainController.getRightX())); + shooter.setDefaultCommand( new ShooterCommand( shooter, @@ -66,7 +88,9 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous - return null; + + Command myAuto = new AutonomousCommand(drive); + + return myAuto; } } diff --git a/src/main/java/frc/robot/commands/AutonomousCommand.java b/src/main/java/frc/robot/commands/AutonomousCommand.java new file mode 100644 index 0000000..87c561c --- /dev/null +++ b/src/main/java/frc/robot/commands/AutonomousCommand.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.drive.DriveSubsystemXrp; + +public class AutonomousCommand extends SequentialCommandGroup { + /** + * Creates a new Autonomous Drive based on distance. This will drive out for a specified distance, + * turn around and drive back. + * + * @param drivetrain The drivetrain subsystem on which this command will run + */ + public AutonomousCommand(DriveSubsystemXrp drivetrain) { + + // Creates a PID Controller for the wheels going in a straight line + PIDController wheelPIDController = + new PIDController(DriveConstants.speedPIDConstants.kP, + DriveConstants.speedPIDConstants.kI, + DriveConstants.speedPIDConstants.kD); + + PIDController zAxisPidController = + new PIDController(DriveConstants.zRotationPIDConstants.kP, + DriveConstants.zRotationPIDConstants.kI, + DriveConstants.zRotationPIDConstants.kD); + + addCommands( + new DriveDistance(1, 1, drivetrain, wheelPIDController, zAxisPidController), // Drive forward + new DriveDistance(-1, 1, drivetrain, wheelPIDController, zAxisPidController)); // Drive backward + } +} diff --git a/src/main/java/frc/robot/commands/DriveDistance.java b/src/main/java/frc/robot/commands/DriveDistance.java new file mode 100644 index 0000000..c20c983 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveDistance.java @@ -0,0 +1,129 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.drive.DriveSubsystemXrp; +import org.littletonrobotics.junction.Logger; + +public class DriveDistance extends Command { + private DriveSubsystemXrp drive; + private double distance; + private double speed; + private PIDController distancePIDController; + private PIDController zRotationPIDController; + private DifferentialDriveKinematics kinematics; + private double rightWheelStartPosition; + private double leftWheelStartPosition; + private double zRotationStartingPosition; + + /** + * Creates a new DriveDistance. This command will drive your your robot for a desired distance at + * a desired speed. + * + * @param speed The speed at which the robot will drive + * @param meters The number of meters the robot will drive + * @param drive The drivetrain subsystem on which this command will run + */ + + public DriveDistance( + double driveSpeed, + double meters, + DriveSubsystemXrp driveSubsystem, + PIDController dPIDController, + PIDController zPIDController + ) + { + + distance = meters; + speed = driveSpeed; + drive = driveSubsystem; + distancePIDController = dPIDController; + zRotationPIDController = zPIDController; // Initialize the PID controller for Z rotation + + // Create a DifferentialDriveKinematics object with the track width + kinematics = new DifferentialDriveKinematics(DriveConstants.trackWidthMeters); + + + + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drive.resetEncoders(); + drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + distancePIDController.reset(); + rightWheelStartPosition = drive.getRightPositionMeters(); + leftWheelStartPosition = drive.getLeftPositionMeters(); + zRotationStartingPosition = drive.getAngleZ(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double leftDistance = leftWheelStartPosition - drive.getLeftPositionMeters(); + double rightDistance = rightWheelStartPosition - drive.getRightPositionMeters(); + + double distanceError = leftDistance - rightDistance; + + double distanceCorrection = distancePIDController.calculate(distanceError); + + double zRotationDrift = zRotationStartingPosition - drive.getAngleZ(); + + // Calculate the Z rotation correction using the PID controller + double headingCorrection = zRotationPIDController.calculate(zRotationDrift); + + double leftSpeed = speed - distanceCorrection; + double rightSpeed = speed + distanceCorrection; + + // Create a DifferentialDriveWheelSpeeds object with the wheel speeds + DifferentialDriveWheelSpeeds wheelSpeeds = + new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed); + + // // Convert the wheel speeds to chassis speeds + ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds); + // Log the new value + Logger.recordOutput("leftWheelStartPosition", leftWheelStartPosition); + Logger.recordOutput("rightWheelStartPosition", rightWheelStartPosition); + Logger.recordOutput("LeftPositionMeters", drive.getLeftPositionMeters()); + Logger.recordOutput("RightPositionMeters", drive.getLeftPositionMeters()); + Logger.recordOutput("leftDistance", leftDistance); + Logger.recordOutput("rightDistance", leftDistance); + Logger.recordOutput("distanceError", distanceError); + Logger.recordOutput("distanceCorrection", distanceCorrection); + Logger.recordOutput("zRotationDrift", zRotationDrift); + Logger.recordOutput("headingCorrection", headingCorrection); + Logger.recordOutput("leftSpeed", leftSpeed); + Logger.recordOutput("rightSpeed", rightSpeed); + Logger.recordOutput("chassisSpeeds", chassisSpeeds); + Logger.recordOutput("zRotationStartingPosition", zRotationStartingPosition); + + drive.setChassisSpeeds(chassisSpeeds); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + double leftDistanceTraveled = leftWheelStartPosition - drive.getLeftPositionMeters(); + double rightDistanceTraveled = rightWheelStartPosition - drive.getRightPositionMeters(); + + double distanceTraveled = (leftDistanceTraveled + rightDistanceTraveled) / 2.0; + return (Math.abs(distanceTraveled) >= distance); + } +} diff --git a/src/main/java/frc/robot/commands/TankDrive.java b/src/main/java/frc/robot/commands/TankDrive.java new file mode 100644 index 0000000..b911c4d --- /dev/null +++ b/src/main/java/frc/robot/commands/TankDrive.java @@ -0,0 +1,76 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.drive.Drive; +import java.util.function.Supplier; + +public class TankDrive extends Command { + private final Drive drive; + private final Supplier rightWheelSpeedSupplier; + private final Supplier leftWheelSpeedSupplier; + + // Create a DifferentialDriveKinematics object with the track width + private DifferentialDriveKinematics kinematics = + new DifferentialDriveKinematics(DriveConstants.trackWidthMeters); + + /** + * Creates a new TankDrive. This command will drive your robot according to the speed supplier + * lambdas. This command does not terminate. + * + * @param drive The drivetrain subsystem on which this command will run + * @param leftWheelSpeedSupplier Lambda supplier of forward/backward speed + * @param rightWheelSpeedSupplier Lambda supplier of rotational speed + */ + public TankDrive( + Drive drive, + Supplier leftWheelSpeedSupplier, + Supplier rightWheelSpeedSupplier) { + this.drive = drive; + this.rightWheelSpeedSupplier = leftWheelSpeedSupplier; + this.leftWheelSpeedSupplier = rightWheelSpeedSupplier; + addRequirements((Subsystem) drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + double rightWheelSpeed = + rightWheelSpeedSupplier.get() * DriveConstants.maxLinearVelocityMetersPerSec; + double leftWheelSpeed = + leftWheelSpeedSupplier.get() * DriveConstants.maxLinearVelocityMetersPerSec; + + // Create a DifferentialDriveWheelSpeeds object with the wheel speeds + DifferentialDriveWheelSpeeds wheelSpeeds = + new DifferentialDriveWheelSpeeds(leftWheelSpeed, rightWheelSpeed); + + // Convert the wheel speeds to chassis speeds + ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds); + + // Pass the wheel speeds to the drive subsystem + drive.setChassisSpeeds(chassisSpeeds); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java new file mode 100644 index 0000000..a621df2 --- /dev/null +++ b/src/main/java/frc/robot/commands/TurnDegrees.java @@ -0,0 +1,67 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.DriveSubsystemXrp; + +public class TurnDegrees extends Command { + private final DriveSubsystemXrp m_drive; + private final double m_degrees; + private final double m_speed; + + /** + * Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in + * degrees) and rotational speed. + * + * @param speed The speed which the robot will drive. Negative is in reverse. + * @param degrees Degrees to turn. Leverages encoders to compare distance. + * @param drive The drive subsystem on which this command will run + */ + public TurnDegrees(double speed, double degrees, DriveSubsystemXrp drive) { + m_degrees = degrees; + m_speed = speed; + m_drive = drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // Set motors to stop, read encoder values for starting point + // m_drive.setChassisSpeeds(0, 0); + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // m_drive.setChassisSpeeds(0, m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + // m_drive.setChassisSpeeds(0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + /* Need to convert distance travelled to degrees. The Standard + + XRP Chassis has a track width of .155 m + */ + double meterPerDegree = Math.PI * .155 / 360; + // Compare distance travelled from start to distance based on degree turn + return getAverageTurningDistance() >= (meterPerDegree * m_degrees); + } + + private double getAverageTurningDistance() { + double leftDistance = Math.abs(m_drive.getLeftPositionMeters()); + double rightDistance = Math.abs(m_drive.getRightPositionMeters()); + return (leftDistance + rightDistance) / 2.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index e711e8c..06cb55a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -4,24 +4,30 @@ import org.littletonrobotics.junction.Logger; public class ArmSubsystem extends SubsystemBase implements Arm { - ArmIO io; - ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); + ArmIO ioOne; + ArmIO ioTwo; + ArmIOInputsAutoLogged inputsOne = new ArmIOInputsAutoLogged(); + ArmIOInputsAutoLogged inputsTwo = new ArmIOInputsAutoLogged(); - public ArmSubsystem(ArmIO io) { - this.io = io; + public ArmSubsystem(ArmIO ioOne, ArmIO ioTwo) { + this.ioOne = ioOne; + this.ioTwo = ioTwo; } @Override public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("arm", inputs); + ioOne.updateInputs(inputsOne); + ioTwo.updateInputs(inputsTwo); + Logger.processInputs("armOne", inputsOne); + Logger.processInputs("armTwo", inputsTwo); } public void setAngle(double angleDeg) { - io.setAngle(angleDeg); + ioOne.setAngle(angleDeg); + ioTwo.setAngle(angleDeg); } public double getAngle() { - return inputs.angleDeg; + return inputsOne.angleDeg; } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index be255b0..ffc1340 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -4,11 +4,60 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; public interface Drive { + /** + * Gets the current chassis speeds. + * + * @return The current chassis speeds. + */ public ChassisSpeeds getChassisSpeeds(); + /** + * Sets the chassis speeds. + * + * @param speeds The chassis speeds to set. + */ public void setChassisSpeeds(ChassisSpeeds speeds); + /** + * Gets the current pose of the robot. + * + * @return The current pose of the robot. + */ public Pose2d getPose(); + /** + * Sets the pose of the robot. The pose includes the position (x, y coordinates) + * and orientation + * (heading) of the robot. This method is typically used to reset the robot's + * position and + * orientation during initialization or after a significant event, such as a + * collision or manual + * repositioning. + * + * @param pose The pose to set, which includes the x and y coordinates and the + * orientation. + */ public void setPose(Pose2d pose); -} + + /** + * Gets the current angle around the X-axis. + * + * @return The angle around the X-axis in degrees. + */ + public double getAngleX(); + + /** + * Gets the current angle around the Y-axis. + * + * @return The angle around the Y-axis in degrees. + */ + public double getAngleY(); + + /** + * Gets the current angle around the Z-axis. + * + * @return The angle around the Z-axis in degrees. + */ + public double getAngleZ(); + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java index 7c34a6c..b1b2e85 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java @@ -7,9 +7,32 @@ import edu.wpi.first.wpilibj.xrp.XRPMotor; public class DriveDifferentialIOXrp implements DriveDifferentialIO { + + /** + * The gear ratio of the drive system. This is calculated as the product of the ratios of each + * stage in the gear train. The gear ratio determines how many times the motor shaft must rotate + * to achieve one full rotation of the output shaft. In this case, the gear ratio is calculated + * as: (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0) = 48.75 This means the motor + * shaft rotates 48.75 times for each rotation of the output shaft. + */ private static final double gearRatio = (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1 + + /** + * The number of encoder counts per revolution of the motor shaft. This value is specific to the + * encoder being used and represents how many pulses the encoder generates for one full rotation + * of the motor shaft. In this case, the encoder generates 12 counts per revolution of the motor + * shaft. + */ private static final double countsPerMotorShaftRev = 12.0; + + /** + * The number of encoder counts per revolution of the output shaft. This is calculated by + * multiplying the counts per motor shaft revolution by the gear ratio. It represents how many + * pulses the encoder generates for one full rotation of the output shaft. For this drive system, + * it is calculated as: countsPerMotorShaftRev * gearRatio = 12.0 * 48.75 = 585.0 This means the + * encoder generates 585 counts for each full rotation of the output shaft. + */ private static final double countsPerRevolution = countsPerMotorShaftRev * gearRatio; // 585.0 // The XRP has the left and right motors set to @@ -30,9 +53,26 @@ public class DriveDifferentialIOXrp implements DriveDifferentialIO { private final XRPGyro gyro = new XRPGyro(); public DriveDifferentialIOXrp() { - // Use meters as unit for encoder distances + // Set the distance per pulse for the left and right encoders using meters as + // the unit. The distance per pulse is calculated as the circumference of the + // wheel divided by the number of encoder counts per revolution. + // The circumference of the wheel is given by the formula 2 * π * radius. + // Assuming the radius of the wheel is 1 meter, the distance per pulse is + // calculated as (2 * Math.PI) / countsPerRevolution. + // This means each pulse from the encoder corresponds to a specific distance + // traveled by the wheel, allowing for accurate measurement of the robot's + // movement. leftEncoder.setDistancePerPulse((Math.PI * 2) / countsPerRevolution); rightEncoder.setDistancePerPulse((Math.PI * 2) / countsPerRevolution); + + /** + * Resets the encoders to their initial state. This method is typically called during the + * initialization of the drive system to ensure that the encoder readings start from zero. By + * resetting the encoders, any previous counts or distances recorded by the encoders are + * cleared. This is important for accurate tracking of the robot's movement, as it ensures that + * the encoder values reflect the current position and movement of the robot from a known + * starting point. + */ resetEncoders(); // Invert right side since motor is flipped diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index 600be4a..1b10352 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -16,25 +16,44 @@ import org.littletonrobotics.junction.Logger; public class DriveSubsystemXrp extends SubsystemBase implements Drive { + // The IO subsystem used to interface with the robot's differential drive + // hardware. private final DriveDifferentialIO io; + + // The inputs for the differential drive, automatically logged. private final DriveDifferentialIOInputsAutoLogged inputs = new DriveDifferentialIOInputsAutoLogged(); - private static final double wheelDiameterMeters = 0.060; // 60 mm - private static final double trackWidthMeters = 0.155; // 155 mm + // The diameter of the robot's wheels in meters (60 mm). + private static final double wheelDiameterMeters = DriveConstants.wheelDiameterMeters; + + // The track width of the robot in meters (155 mm). + private static final double trackWidthMeters = DriveConstants.trackWidthMeters; + // The kinematics object used to convert between chassis speeds and wheel + // speeds. private final DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(trackWidthMeters); - // START: Setup Odometry + // The odometry object used to track the robot's position on the field. private final DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(), 0.0, 0.0); + + // The field object used to represent the robot's position on the dashboard + // view. private Field2d field = new Field2d(); - // END: Setup Odometry + /** + * The current pose of the robot, including its position and orientation. This field is + * automatically logged. + */ @AutoLogOutput private Pose2d pose; - /** Creates a new DriveSubsystemXrp. */ + /** + * Constructs a new DriveSubsystemXrp. + * + * @param io The IO subsystem used to interface with the robot's differential drive hardware. + */ public DriveSubsystemXrp(DriveDifferentialIO io) { this.io = io; @@ -43,6 +62,12 @@ public DriveSubsystemXrp(DriveDifferentialIO io) { // END: Setup Odometry } + /** + * Gets the current chassis speeds of the robot. + * + * @return The current chassis speeds, represented as a ChassisSpeeds object. This includes the + * linear and angular velocities of the robot. + */ @Override public ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds( @@ -51,42 +76,123 @@ public ChassisSpeeds getChassisSpeeds() { Units.radiansToRotations(inputs.rightVelocityRadPerSec) * wheelDiameterMeters)); } + /** + * Sets the desired chassis speeds for the robot. This method converts the desired chassis speeds + * (linear and angular velocities) into individual wheel speeds and then drives the robot using + * those wheel speeds. + * + * @param speeds The desired chassis speeds, represented as a {@link ChassisSpeeds} object. This + * includes the linear and angular velocities of the robot. + */ @Override public void setChassisSpeeds(ChassisSpeeds speeds) { - double leftSpeed; - double rightSpeed; + DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(speeds); - leftSpeed = wheelSpeeds.leftMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; - rightSpeed = wheelSpeeds.rightMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; + // Normalize wheel speeds by the maximum linear velocity (highest speed at which + // the robot can move in a straight line) + double leftSpeed = + wheelSpeeds.leftMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; + double rightSpeed = + wheelSpeeds.rightMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; + // Drive the robot using the calculated wheel speeds io.tankDrive(leftSpeed, rightSpeed); } + /** + * Gets the current pose of the robot. The pose represents the robot's position and orientation on + * the field. It is typically used for tracking the robot's location during autonomous and + * teleoperated periods. + * + * @return The current pose of the robot as a {@link Pose2d} object. + */ public Pose2d getPose() { return pose; } + /** + * Sets a new pose for the robot and resets the odometry and encoders. This method is typically + * used to reset the robot's position and orientation on the field, such as at the start of a + * match or after a significant event that requires reinitialization. + * + * @param pose The new pose to set for the robot, represented as a {@link Pose2d} object. This + * includes the x and y coordinates and the orientation (heading) of the robot. + */ public void setPose(Pose2d pose) { + // Reset the odometry to the new pose with zeroed wheel positions odometry.resetPosition(inputs.zRotation, new DifferentialDriveWheelPositions(0, 0), pose); + + // Reset the encoders to ensure accurate tracking from the new pose io.resetEncoders(); + + // Update the internal pose state to the new pose this.pose = pose; } + public void resetEncoders() + { + io.resetEncoders(); + } + + /** + * This method is called periodically by the scheduler. It updates the robot's inputs, processes + * those inputs, updates the robot's pose using odometry, and updates the field with the robot's + * current pose. This ensures that the robot's state is consistently updated and accurately + * reflects its position and orientation on the field. + */ @Override public void periodic() { + // Update inputs from the IO subsystem io.updateInputs(inputs); Logger.processInputs("Drive", inputs); - // START: Setup Odometry + // Update the robot's pose using odometry pose = odometry.update( inputs.zRotation, new DifferentialDriveWheelPositions( Units.radiansToRotations(inputs.leftPositionRad) * wheelDiameterMeters, Units.radiansToRotations(inputs.rightPositionRad) * wheelDiameterMeters)); + + // Update the field with the robot's current pose field.setRobotPose(pose); - // END: Setup Odometry + } + + /** + * The current position of the right wheels in meters. + * + * @return the current position of the right wheels (in meters) + */ + public double getRightPositionMeters() { + return Units.radiansToRotations(inputs.rightPositionRad) * wheelDiameterMeters; + } + + /** + * The current position of the right wheels in meters. + * + * @return the current position of the right wheels (in meters) + */ + public double getLeftPositionMeters() { + return Units.radiansToRotations(inputs.leftPositionRad) * wheelDiameterMeters; + } + + @Override + public double getAngleX() + { + return inputs.xRotation.getDegrees(); + } + + @Override + public double getAngleY() + { + return inputs.yRotation.getDegrees(); + } + + @Override + public double getAngleZ() + { + return inputs.zRotation.getDegrees(); } @Override