From 95109b702d9278f37376b96cee5b61ca5fa20ab4 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Fri, 15 Nov 2024 15:58:31 -0500 Subject: [PATCH 01/16] Added capability to switch how the robot is controlled when the 'A' button is pressed on the XBox controller --- build.gradle | 3 +- src/main/java/frc/robot/RobotContainer.java | 50 +++++++++++++++---- .../subsystems/drive/DriveSubsystemXrp.java | 5 +- 3 files changed, 47 insertions(+), 11 deletions(-) diff --git a/build.gradle b/build.gradle index 35a348c..dae90fb 100644 --- a/build.gradle +++ b/build.gradle @@ -43,7 +43,8 @@ wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() //Sets the XRP Client Host -wpi.sim.envVar("HALSIMXRP_HOST", "192.168.42.1") +//wpi.sim.envVar("HALSIMXRP_HOST", "192.168.50.238") // BHS +wpi.sim.envVar("HALSIMXRP_HOST", "192.168.2.212") // HOME wpi.sim.addXRPClient().defaultEnabled = true // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ed4564e..e930de9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,8 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.ArcadeDrive; import frc.robot.commands.ArmCommand; @@ -19,6 +21,9 @@ import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterIOXrp; import frc.robot.subsystems.shooter.ShooterSubsystem; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -35,10 +40,16 @@ public class RobotContainer { private final CommandXboxController mainController = new CommandXboxController(0); + // Stores the number of times the "B" button has been pressed + private int bPressCount = 0; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the button bindings configureButtonBindings(); + + // drive.setDefaultCommand( + // new ArcadeDrive(drive, () -> -mainController.getLeftY(), () -> -mainController.getRightX())); } /** @@ -48,17 +59,38 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - drive.setDefaultCommand( - new ArcadeDrive(drive, () -> -mainController.getLeftY(), () -> -mainController.getLeftX())); - arm.setDefaultCommand(new ArmCommand(arm, () -> -mainController.getRightY())); - intake.setDefaultCommand(new IntakeCommand(intake, () -> -mainController.getRightX())); - shooter.setDefaultCommand( - new ShooterCommand( - shooter, - () -> mainController.getLeftTriggerAxis(), - () -> mainController.getRightTriggerAxis())); + // toggles the drive mode between left stick controlling steering and right stick controlling steering + new Trigger(mainController.b().onTrue(new InstantCommand(this::handleBButtonPress))); } + + // toggles the drive mode between left stick controlling steering and right stick controlling steering + private void handleBButtonPress(){ + + bPressCount++; + if (bPressCount % 2 == 1) + { + new ArcadeDrive( + drive, + () -> -mainController.getLeftY(), + () -> -mainController.getLeftX() + ).schedule(); + } else { + new ArcadeDrive( + drive, + () -> -mainController.getLeftY(), + () -> -mainController.getRightX() + ).schedule(); + } + }; + + //arm.setDefaultCommand(new ArmCommand(arm, () -> -mainController.())); + //intake.setDefaultCommand(new IntakeCommand(intake, () -> -mainController.getRightX())); + //shooter.setDefaultCommand( + // new ShooterCommand( + // shooter, + // () -> mainController.getLeftTriggerAxis(), + /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index 600be4a..97b479d 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -12,6 +12,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriveConstants; + +import java.nio.channels.WritableByteChannel; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -59,7 +62,7 @@ public void setChassisSpeeds(ChassisSpeeds speeds) { leftSpeed = wheelSpeeds.leftMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; rightSpeed = wheelSpeeds.rightMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; - + io.tankDrive(leftSpeed, rightSpeed); } From f569bac8a0e6ac029b8c05ce393e8977200e0641 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Fri, 15 Nov 2024 17:08:10 -0500 Subject: [PATCH 02/16] Adding documentation to RobotContainer.java and DriveSybsystemXrp.java --- src/main/java/frc/robot/RobotContainer.java | 7 ++- .../subsystems/drive/DriveSubsystemXrp.java | 54 ++++++++++++++----- 2 files changed, 45 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e930de9..b059adc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -48,8 +48,9 @@ public RobotContainer() { // Configure the button bindings configureButtonBindings(); - // drive.setDefaultCommand( - // new ArcadeDrive(drive, () -> -mainController.getLeftY(), () -> -mainController.getRightX())); + // Configure the robot to be driven by the left stick for steering and for throttle + drive.setDefaultCommand( + new ArcadeDrive(drive, () -> -mainController.getLeftY(), () -> -mainController.getRightX())); } /** @@ -70,12 +71,14 @@ private void handleBButtonPress(){ bPressCount++; if (bPressCount % 2 == 1) { + // Left stick controlls steering new ArcadeDrive( drive, () -> -mainController.getLeftY(), () -> -mainController.getLeftX() ).schedule(); } else { + // Right stick controlls steering new ArcadeDrive( drive, () -> -mainController.getLeftY(), diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index 97b479d..666990b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -19,33 +19,52 @@ 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; - 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 inputs for the differential drive, automatically logged. + private final DriveDifferentialIOInputsAutoLogged inputs = new DriveDifferentialIOInputsAutoLogged(); - private final DifferentialDriveKinematics kinematics = - new DifferentialDriveKinematics(trackWidthMeters); + // The diameter of the robot's wheels in meters (60 mm). + private static final double wheelDiameterMeters = 0.060; - // START: Setup Odometry - private final DifferentialDriveOdometry odometry = - new DifferentialDriveOdometry(new Rotation2d(), 0.0, 0.0); + // The track width of the robot in meters (155 mm). + private static final double trackWidthMeters = 0.155; + + // The kinematics object used to convert between chassis speeds and wheel speeds. + private final DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(trackWidthMeters); + + // 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; - // START: Setup Odometry + // START: Setup Odometry for dashboard display. SmartDashboard.putData("Field", field); // 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( @@ -56,13 +75,16 @@ public ChassisSpeeds getChassisSpeeds() { @Override public void setChassisSpeeds(ChassisSpeeds speeds) { + // Declare variables for left and right wheel speeds double leftSpeed; double rightSpeed; DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(speeds); + // Normalize wheel speeds by the maximum linear velocity (highest speed at which the robot can move in a straight line) leftSpeed = wheelSpeeds.leftMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; rightSpeed = wheelSpeeds.rightMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; + // Drive the robot using the calculated wheel speeds io.tankDrive(leftSpeed, rightSpeed); } @@ -70,6 +92,7 @@ public Pose2d getPose() { return pose; } + // Set a new pose for the robot and reset odometry and encoders public void setPose(Pose2d pose) { odometry.resetPosition(inputs.zRotation, new DifferentialDriveWheelPositions(0, 0), pose); io.resetEncoders(); @@ -78,18 +101,21 @@ public void setPose(Pose2d pose) { @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 + } @Override From accb41ae7d7088ebb604306a80f1bb366c5fee49 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Wed, 20 Nov 2024 18:45:26 -0500 Subject: [PATCH 03/16] Adding logic to toggle between three driving modes, Arcade Left only, Arcade Left and right, and tank mode. --- src/main/java/frc/robot/RobotContainer.java | 50 +++++++++++------ .../java/frc/robot/commands/TankDrive.java | 56 +++++++++++++++++++ .../frc/robot/subsystems/drive/Drive.java | 1 + .../subsystems/drive/DriveSubsystemXrp.java | 18 ++++++ 4 files changed, 109 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc/robot/commands/TankDrive.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b059adc..7980711 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.ArcadeDrive; +import frc.robot.commands.TankDrive; import frc.robot.commands.ArmCommand; import frc.robot.commands.IntakeCommand; import frc.robot.commands.ShooterCommand; @@ -50,7 +51,7 @@ public RobotContainer() { // Configure the robot to be driven by the left stick for steering and for throttle drive.setDefaultCommand( - new ArcadeDrive(drive, () -> -mainController.getLeftY(), () -> -mainController.getRightX())); + new ArcadeDrive(drive, () -> -mainController.getLeftY(), () -> -mainController.getLeftX())); } /** @@ -68,22 +69,39 @@ private void configureButtonBindings() { // toggles the drive mode between left stick controlling steering and right stick controlling steering private void handleBButtonPress(){ + // Increment the button B press count bPressCount++; - if (bPressCount % 2 == 1) - { - // Left stick controlls steering - new ArcadeDrive( - drive, - () -> -mainController.getLeftY(), - () -> -mainController.getLeftX() - ).schedule(); - } else { - // Right stick controlls steering - new ArcadeDrive( - drive, - () -> -mainController.getLeftY(), - () -> -mainController.getRightX() - ).schedule(); + + // Use modulo to cycle through control schemes + int controlScheme = bPressCount % 3; + + switch (controlScheme) { + case 0: + // Left stick controls steering + new ArcadeDrive(drive, + () -> -mainController.getLeftY(), + () -> -mainController.getLeftX() + ).schedule(); + break; + case 1: + // Right stick controls steering + new ArcadeDrive( + drive, + () -> -mainController.getLeftY(), + () -> -mainController.getRightX() + ).schedule(); + break; + case 2: + // Both sticks control steering (example of a third scheme) + new TankDrive( + drive, + () -> -mainController.getLeftY(), + () -> -mainController.getRightY() + ).schedule(); + break; + default: + // Default case (should not be reached) + break; } }; 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..fc04a58 --- /dev/null +++ b/src/main/java/frc/robot/commands/TankDrive.java @@ -0,0 +1,56 @@ +// 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.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; + + /** + * 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() { + drive.setChassisSpeeds( + rightWheelSpeedSupplier.get() * DriveConstants.maxLinearVelocityMetersPerSec, + leftWheelSpeedSupplier.get() * DriveConstants.maxAngularVelocityRadPerSec); + } + + // 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/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index be255b0..effe160 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -7,6 +7,7 @@ public interface Drive { public ChassisSpeeds getChassisSpeeds(); public void setChassisSpeeds(ChassisSpeeds speeds); + public void setChassisSpeeds(double leftSpeed, double rightSpeed); public Pose2d getPose(); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index 666990b..496daff 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -88,6 +88,24 @@ public void setChassisSpeeds(ChassisSpeeds speeds) { io.tankDrive(leftSpeed, rightSpeed); } + // Other class members and methods + + /** + * Sets the chassis speeds for the left and right wheels. + * + * @param leftspeed The speed for the left wheel. + * @param rightspeed The speed for the right wheel. + */ + @Override + public void setChassisSpeeds(double leftSpeed, double rightSpeed) { + + leftSpeed = leftSpeed / DriveConstants.maxLinearVelocityMetersPerSec; + rightSpeed = rightSpeed / DriveConstants.maxLinearVelocityMetersPerSec; + + // Drive the robot using the calculated wheel speeds + io.tankDrive(leftSpeed, rightSpeed); + } + public Pose2d getPose() { return pose; } From 47a2aa44c40dcbadd69322394cc2621b154e3821 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Wed, 20 Nov 2024 18:53:38 -0500 Subject: [PATCH 04/16] Putting gradle back to default settings --- build.gradle | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index dae90fb..35a348c 100644 --- a/build.gradle +++ b/build.gradle @@ -43,8 +43,7 @@ wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() //Sets the XRP Client Host -//wpi.sim.envVar("HALSIMXRP_HOST", "192.168.50.238") // BHS -wpi.sim.envVar("HALSIMXRP_HOST", "192.168.2.212") // HOME +wpi.sim.envVar("HALSIMXRP_HOST", "192.168.42.1") wpi.sim.addXRPClient().defaultEnabled = true // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') From 650140b1963567e02b1d870bfffebfd4eda2bb5f Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Wed, 20 Nov 2024 18:55:17 -0500 Subject: [PATCH 05/16] cleaning up code in robotContainer.java --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7980711..6df2e79 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -48,10 +48,7 @@ public class RobotContainer { public RobotContainer() { // Configure the button bindings 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())); + } /** @@ -61,6 +58,11 @@ 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())); // toggles the drive mode between left stick controlling steering and right stick controlling steering new Trigger(mainController.b().onTrue(new InstantCommand(this::handleBButtonPress))); From 73e9280b93fc5db9b5ff2446b28c73642c19ebb0 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Sat, 23 Nov 2024 10:18:16 -0500 Subject: [PATCH 06/16] documenation cleanup --- src/main/java/frc/robot/RobotContainer.java | 118 ++++++++++-------- .../java/frc/robot/commands/TankDrive.java | 18 +-- .../frc/robot/subsystems/drive/Drive.java | 33 +++++ .../drive/DriveDifferentialIOXrp.java | 59 ++++++++- 4 files changed, 166 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6df2e79..8907d6a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,9 +27,12 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; /** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * This class is where the bulk of the robot should be declared. Since + * Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in + * the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of + * the robot (including * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { @@ -44,76 +47,91 @@ public class RobotContainer { // Stores the number of times the "B" button has been pressed private int bPressCount = 0; - /** The container for the robot. Contains subsystems, OI devices, and commands. */ + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ public RobotContainer() { // Configure the button bindings configureButtonBindings(); - + } /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * Use this method to define your button->command mappings. Buttons can be + * created by + * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its + * subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing + * it to a {@link * 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 + // Configure the robot to be driven by the left stick for steering and for + // throttle drive.setDefaultCommand( - new ArcadeDrive(drive, - () -> -mainController.getLeftY(), - () -> -mainController.getLeftX())); + new ArcadeDrive(drive, + () -> -mainController.getLeftY(), + () -> -mainController.getLeftX())); - // toggles the drive mode between left stick controlling steering and right stick controlling steering + // toggles the drive mode between left stick controlling steering and right + // stick controlling steering new Trigger(mainController.b().onTrue(new InstantCommand(this::handleBButtonPress))); } - - // toggles the drive mode between left stick controlling steering and right stick controlling steering - private void handleBButtonPress(){ - + + // toggles the drive mode between left stick controlling steering and right + // stick controlling steering + private void handleBButtonPress() { + // Increment the button B press count bPressCount++; // Use modulo to cycle through control schemes int controlScheme = bPressCount % 3; + /** + * Switches between different control schemes for the robot's steering based on + * the value of controlScheme. + * Each case represents a different control scheme: + * - Case 0: Left stick controls steering using ArcadeDrive. + * - Case 1: Right stick controls steering using ArcadeDrive. + * - Case 2: Both sticks control steering using TankDrive. + * + * @param controlScheme The current control scheme to use. + */ switch (controlScheme) { - case 0: - // Left stick controls steering - new ArcadeDrive(drive, - () -> -mainController.getLeftY(), - () -> -mainController.getLeftX() - ).schedule(); - break; - case 1: - // Right stick controls steering - new ArcadeDrive( - drive, - () -> -mainController.getLeftY(), - () -> -mainController.getRightX() - ).schedule(); - break; - case 2: - // Both sticks control steering (example of a third scheme) - new TankDrive( - drive, - () -> -mainController.getLeftY(), - () -> -mainController.getRightY() - ).schedule(); - break; - default: - // Default case (should not be reached) - break; + case 0: + // Left stick controls steering + new ArcadeDrive(drive, + () -> -mainController.getLeftY(), + () -> -mainController.getLeftX()).schedule(); + break; + case 1: + // Right stick controls steering + new ArcadeDrive( + drive, + () -> -mainController.getLeftY(), + () -> -mainController.getRightX()).schedule(); + break; + case 2: + // Both sticks control steering using TankDrive + new TankDrive( + drive, + () -> -mainController.getLeftY(), + () -> -mainController.getRightY()).schedule(); + break; + default: + // Default case (should not be reached) + break; } }; - - //arm.setDefaultCommand(new ArmCommand(arm, () -> -mainController.())); - //intake.setDefaultCommand(new IntakeCommand(intake, () -> -mainController.getRightX())); - //shooter.setDefaultCommand( - // new ShooterCommand( - // shooter, - // () -> mainController.getLeftTriggerAxis(), + // arm.setDefaultCommand(new ArmCommand(arm, () -> -mainController.())); + // intake.setDefaultCommand(new IntakeCommand(intake, () -> + // -mainController.getRightX())); + // shooter.setDefaultCommand( + // new ShooterCommand( + // shooter, + // () -> mainController.getLeftTriggerAxis(), /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/commands/TankDrive.java b/src/main/java/frc/robot/commands/TankDrive.java index fc04a58..22e8e07 100644 --- a/src/main/java/frc/robot/commands/TankDrive.java +++ b/src/main/java/frc/robot/commands/TankDrive.java @@ -17,11 +17,13 @@ public class TankDrive extends Command { private final Supplier leftWheelSpeedSupplier; /** - * Creates a new TankDrive. This command will drive your robot according to the speed supplier + * 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 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( @@ -34,19 +36,21 @@ public TankDrive( // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { drive.setChassisSpeeds( - rightWheelSpeedSupplier.get() * DriveConstants.maxLinearVelocityMetersPerSec, - leftWheelSpeedSupplier.get() * DriveConstants.maxAngularVelocityRadPerSec); + rightWheelSpeedSupplier.get() * DriveConstants.maxLinearVelocityMetersPerSec, + leftWheelSpeedSupplier.get() * DriveConstants.maxAngularVelocityRadPerSec); } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index effe160..7f97659 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -4,12 +4,45 @@ 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); + + /** + * Sets the chassis speeds for the left and right wheels. + * + * @param leftSpeed The speed for the left wheel. + * @param rightSpeed The speed for the right wheel. + */ public void setChassisSpeeds(double leftSpeed, double rightSpeed); + /** + * 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); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java index 7c34a6c..06f1946 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java @@ -7,9 +7,40 @@ import edu.wpi.first.wpilibj.xrp.XRPMotor; public class DriveDifferentialIOXrp implements DriveDifferentialIO { - 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 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 @@ -23,16 +54,34 @@ public class DriveDifferentialIOXrp implements DriveDifferentialIO { private final Encoder rightEncoder = new Encoder(6, 7); // Set up the differential drive controller - private final DifferentialDrive differentialDrive = - new DifferentialDrive(leftMotor::set, rightMotor::set); + private final DifferentialDrive differentialDrive = new DifferentialDrive(leftMotor::set, rightMotor::set); // Set up the XRPGyro 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 From d411605ab41fa7a53cb86dcea263266b939fa498 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Sat, 23 Nov 2024 10:38:58 -0500 Subject: [PATCH 07/16] update comments --- src/main/java/frc/robot/RobotContainer.java | 28 ++--- .../subsystems/drive/DriveSubsystemXrp.java | 109 ++++++++++++------ 2 files changed, 90 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8907d6a..46a0e70 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,12 +28,12 @@ /** * This class is where the bulk of the robot should be declared. Since - * Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in - * the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of - * the robot (including - * subsystems, commands, and button mappings) should be declared here. + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in + * the {@link Robot} periodic methods (other than the scheduler calls). Instead, + * the structure of + * the robot (including subsystems, commands, and button mappings) should be + * declared here. */ public class RobotContainer { // The robot's subsystems and commands are defined here... @@ -73,6 +73,14 @@ private void configureButtonBindings() { () -> -mainController.getLeftY(), () -> -mainController.getLeftX())); + arm.setDefaultCommand(new ArmCommand(arm, () -> -mainController.getRightY())); + + intake.setDefaultCommand(new IntakeCommand(intake, () -> -mainController.getRightX())); + + shooter.setDefaultCommand(new ShooterCommand(shooter, + () -> mainController.getLeftTriggerAxis(), + () -> mainController.getRightTriggerAxis())); + // toggles the drive mode between left stick controlling steering and right // stick controlling steering new Trigger(mainController.b().onTrue(new InstantCommand(this::handleBButtonPress))); @@ -125,14 +133,6 @@ private void handleBButtonPress() { } }; - // arm.setDefaultCommand(new ArmCommand(arm, () -> -mainController.())); - // intake.setDefaultCommand(new IntakeCommand(intake, () -> - // -mainController.getRightX())); - // shooter.setDefaultCommand( - // new ShooterCommand( - // shooter, - // () -> mainController.getLeftTriggerAxis(), - /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index 496daff..af39ec0 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -19,7 +19,8 @@ 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. + // 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. @@ -31,26 +32,30 @@ public class DriveSubsystemXrp extends SubsystemBase implements Drive { // The track width of the robot in meters (155 mm). private static final double trackWidthMeters = 0.155; - // The kinematics object used to convert between chassis speeds and wheel speeds. + // The kinematics object used to convert between chassis speeds and wheel + // speeds. private final DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 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. + // The field object used to represent the robot's position on the dashboard + // view. private Field2d field = new Field2d(); /** - * The current pose of the robot, including its position and orientation. - * This field is automatically logged. - */ - @AutoLogOutput private Pose2d pose; + * The current pose of the robot, including its position and orientation. + * This field is automatically logged. + */ + @AutoLogOutput + private Pose2d pose; /** - * Constructs a new DriveSubsystemXrp. - * - * @param io The IO subsystem used to interface with the robot's differential drive hardware. - */ + * 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; @@ -61,10 +66,10 @@ public DriveSubsystemXrp(DriveDifferentialIO io) { /** * 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. - */ + * + * @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( @@ -73,6 +78,16 @@ 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) { // Declare variables for left and right wheel speeds @@ -80,43 +95,72 @@ public void setChassisSpeeds(ChassisSpeeds speeds) { double rightSpeed; DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(speeds); - // Normalize wheel speeds by the maximum linear velocity (highest speed at which the robot can move in a straight line) + // Normalize wheel speeds by the maximum linear velocity (highest speed at which + // the robot can move in a straight line) leftSpeed = wheelSpeeds.leftMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; rightSpeed = wheelSpeeds.rightMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; - + // Drive the robot using the calculated wheel speeds io.tankDrive(leftSpeed, rightSpeed); } - // Other class members and methods - /** - * Sets the chassis speeds for the left and right wheels. - * - * @param leftspeed The speed for the left wheel. - * @param rightspeed The speed for the right wheel. - */ + * Sets the chassis speeds for the left and right wheels. + * + * @param leftspeed The speed for the left wheel. + * @param rightspeed The speed for the right wheel. + */ @Override public void setChassisSpeeds(double leftSpeed, double rightSpeed) { leftSpeed = leftSpeed / DriveConstants.maxLinearVelocityMetersPerSec; rightSpeed = rightSpeed / 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; } - // Set a new pose for the robot and reset odometry and encoders + /** + * 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; } + /** + * 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 @@ -124,16 +168,15 @@ public void periodic() { Logger.processInputs("Drive", inputs); // Update the robot's pose using odometry - pose = - odometry.update( - inputs.zRotation, - new DifferentialDriveWheelPositions( - Units.radiansToRotations(inputs.leftPositionRad) * wheelDiameterMeters, - Units.radiansToRotations(inputs.rightPositionRad) * wheelDiameterMeters)); + 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); - + } @Override From 744df25fd27173caa60dc7683c3be8331682c247 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Wed, 4 Dec 2024 18:22:07 -0500 Subject: [PATCH 08/16] added autonomous drive logic --- .../robot/commands/AutonomousDistance.java | 24 +++++++ .../frc/robot/commands/DriveDistance.java | 55 +++++++++++++++ .../java/frc/robot/commands/TurnDegrees.java | 67 +++++++++++++++++++ 3 files changed, 146 insertions(+) create mode 100644 src/main/java/frc/robot/commands/AutonomousDistance.java create mode 100644 src/main/java/frc/robot/commands/DriveDistance.java create mode 100644 src/main/java/frc/robot/commands/TurnDegrees.java diff --git a/src/main/java/frc/robot/commands/AutonomousDistance.java b/src/main/java/frc/robot/commands/AutonomousDistance.java new file mode 100644 index 0000000..0382f87 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutonomousDistance.java @@ -0,0 +1,24 @@ +// 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.SequentialCommandGroup; +import frc.robot.subsystems.drive.DriveSubsystemXrp; + +public class AutonomousDistance 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 AutonomousDistance(DriveSubsystemXrp drivetrain) { + addCommands( + new DriveDistance(-0.5, 1, drivetrain)); + // new TurnDegrees(-0.5, 180, drivetrain), + // new DriveDistance(-0.5, 1, drivetrain), + // new TurnDegrees(0.5, 180, drivetrain)); + } +} 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..8b3023c --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveDistance.java @@ -0,0 +1,55 @@ +// 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 DriveDistance extends Command { + private final DriveSubsystemXrp m_drive; + private final double m_distance; + private final double m_speed; + + /** + * 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 speed, double meters, DriveSubsystemXrp drive) { + m_distance = meters; + m_speed = speed; + m_drive = drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_drive.setChassisSpeeds(0,0); + m_drive.resetEncoders(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.setChassisSpeeds(m_speed, 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() { + // Compare distance travelled from start to desired distance + return Math.abs(m_drive.getAverageDistanceMeters()) >= m_distance; + } +} \ No newline at end of file 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..cd73312 --- /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; + } +} From 96c828f9539e5b3180a5ebc46a993eff26f06048 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Wed, 4 Dec 2024 19:21:16 -0500 Subject: [PATCH 09/16] Added PID logic to DriveDistance.java --- src/main/java/frc/robot/RobotContainer.java | 13 +++++++ .../robot/commands/AutonomousDistance.java | 2 +- .../frc/robot/commands/DriveDistance.java | 17 +++++++-- .../subsystems/drive/DriveSubsystemXrp.java | 35 +++++++++++++++++++ 4 files changed, 64 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 46a0e70..4136e38 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -22,10 +23,12 @@ import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterIOXrp; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.commands.AutonomousDistance; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; + /** * This class is where the bulk of the robot should be declared. Since * Command-based is a "declarative" paradigm, very little robot logic should @@ -56,6 +59,10 @@ public RobotContainer() { } + // Create SmartDashboard chooser for autonomous routines + private final SendableChooser m_chooser = new SendableChooser<>(); + + /** * Use this method to define your button->command mappings. Buttons can be * created by @@ -84,6 +91,12 @@ private void configureButtonBindings() { // toggles the drive mode between left stick controlling steering and right // stick controlling steering new Trigger(mainController.b().onTrue(new InstantCommand(this::handleBButtonPress))); + + mainController.a().toggleOnTrue(new AutonomousDistance(drive)); + // Setup SmartDashboard options for autonomous routines + m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(drive)); + + } // toggles the drive mode between left stick controlling steering and right diff --git a/src/main/java/frc/robot/commands/AutonomousDistance.java b/src/main/java/frc/robot/commands/AutonomousDistance.java index 0382f87..691d709 100644 --- a/src/main/java/frc/robot/commands/AutonomousDistance.java +++ b/src/main/java/frc/robot/commands/AutonomousDistance.java @@ -16,7 +16,7 @@ public class AutonomousDistance extends SequentialCommandGroup { */ public AutonomousDistance(DriveSubsystemXrp drivetrain) { addCommands( - new DriveDistance(-0.5, 1, drivetrain)); + new DriveDistance(-1, .1, drivetrain)); // new TurnDegrees(-0.5, 180, drivetrain), // new DriveDistance(-0.5, 1, drivetrain), // new TurnDegrees(0.5, 180, drivetrain)); diff --git a/src/main/java/frc/robot/commands/DriveDistance.java b/src/main/java/frc/robot/commands/DriveDistance.java index 8b3023c..34dc299 100644 --- a/src/main/java/frc/robot/commands/DriveDistance.java +++ b/src/main/java/frc/robot/commands/DriveDistance.java @@ -4,13 +4,17 @@ package frc.robot.commands; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.DriveSubsystemXrp; +import edu.wpi.first.math.controller.PIDController; public class DriveDistance extends Command { private final DriveSubsystemXrp m_drive; private final double m_distance; private final double m_speed; + private final PIDController m_pidController; + /** * Creates a new DriveDistance. This command will drive your your robot for a desired distance at @@ -24,20 +28,29 @@ public DriveDistance(double speed, double meters, DriveSubsystemXrp drive) { m_distance = meters; m_speed = speed; m_drive = drive; + m_pidController = new PIDController(1.0, 0.0, 0.0); // Adjust PID constants as needed addRequirements(drive); } // Called when the command is initially scheduled. @Override public void initialize() { - m_drive.setChassisSpeeds(0,0); m_drive.resetEncoders(); + m_pidController.reset(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.setChassisSpeeds(m_speed, m_speed); + double leftDistance = m_drive.getLeftPositionMeters(); + double rightDistance = m_drive.getLeftPositionMeters(); + double error = leftDistance - rightDistance; + double correction = m_pidController.calculate(error); + + double leftSpeed = m_speed - correction; + double rightSpeed = m_speed + correction; + + m_drive.setChassisSpeeds(leftSpeed, rightSpeed); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index af39ec0..eefe0e5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -120,6 +120,10 @@ public void setChassisSpeeds(double leftSpeed, double rightSpeed) { io.tankDrive(leftSpeed, rightSpeed); } + public void resetEncoders(){ + io.resetEncoders(); + } + /** * Gets the current pose of the robot. * The pose represents the robot's position and orientation on the field. @@ -179,6 +183,37 @@ public void periodic() { } + /** + * The current position of the right wheels in meters. + * + * @return the current position of the right wheels (in meters) + */ + public double getRightPositionMeters() { + double position = Units.radiansToRotations(inputs.rightPositionRad) * wheelDiameterMeters; + 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() { + double position = Units.radiansToRotations(inputs.leftPositionRad) * wheelDiameterMeters; + return Units.radiansToRotations(inputs.leftPositionRad) * wheelDiameterMeters; + } + + /** + * The average wheel distance in meters + * + * @return the average wheel distance (in meters) + */ + public double getAverageDistanceMeters() { + double distanceTraveled = (getLeftPositionMeters() + getRightPositionMeters()) / 2.0; + System.out.println("Distance Traveled: " + distanceTraveled); + return (getLeftPositionMeters() + getRightPositionMeters()) / 2.0; + } + @Override public void simulationPeriodic() { // This method will be called once per scheduler run during simulation From f86d925cedc8d9c7971d816e9004005d571ac16b Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Wed, 4 Dec 2024 19:35:32 -0500 Subject: [PATCH 10/16] Adding logic to drive using the wheel encoders. --- .../robot/commands/AutonomousDistance.java | 4 +- .../frc/robot/commands/DriveDistance.java | 1 + .../java/frc/robot/commands/DriveTime.java | 54 +++++++++++++++++++ 3 files changed, 57 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveTime.java diff --git a/src/main/java/frc/robot/commands/AutonomousDistance.java b/src/main/java/frc/robot/commands/AutonomousDistance.java index 691d709..22236df 100644 --- a/src/main/java/frc/robot/commands/AutonomousDistance.java +++ b/src/main/java/frc/robot/commands/AutonomousDistance.java @@ -16,9 +16,9 @@ public class AutonomousDistance extends SequentialCommandGroup { */ public AutonomousDistance(DriveSubsystemXrp drivetrain) { addCommands( - new DriveDistance(-1, .1, drivetrain)); + new DriveDistance(-1, .1, drivetrain), // new TurnDegrees(-0.5, 180, drivetrain), - // new DriveDistance(-0.5, 1, drivetrain), + new DriveDistance(1, .1, drivetrain)); // new TurnDegrees(0.5, 180, drivetrain)); } } diff --git a/src/main/java/frc/robot/commands/DriveDistance.java b/src/main/java/frc/robot/commands/DriveDistance.java index 34dc299..d618638 100644 --- a/src/main/java/frc/robot/commands/DriveDistance.java +++ b/src/main/java/frc/robot/commands/DriveDistance.java @@ -35,6 +35,7 @@ public DriveDistance(double speed, double meters, DriveSubsystemXrp drive) { // Called when the command is initially scheduled. @Override public void initialize() { + m_drive.setChassisSpeeds(0, 0); m_drive.resetEncoders(); m_pidController.reset(); } diff --git a/src/main/java/frc/robot/commands/DriveTime.java b/src/main/java/frc/robot/commands/DriveTime.java new file mode 100644 index 0000000..aab3cff --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveTime.java @@ -0,0 +1,54 @@ +// 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 DriveTime extends Command { + private final double m_duration; + private final double m_speed; + private final DriveSubsystemXrp m_drive; + private long m_startTime; + + /** + * Creates a new DriveTime. This command will drive your robot for a desired speed and time. + * + * @param speed The speed which the robot will drive. Negative is in reverse. + * @param time How much time to drive in seconds + * @param drive The drivetrain subsystem on which this command will run + */ + public DriveTime(double speed, double time, DriveSubsystemXrp drive) { + m_speed = speed; + m_duration = time * 1000; + m_drive = drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_startTime = System.currentTimeMillis(); + m_drive.setChassisSpeeds(0, 0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.setChassisSpeeds(m_speed, 0); + } + + // 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() { + return (System.currentTimeMillis() - m_startTime) >= m_duration; + } +} \ No newline at end of file From 19329e672693cced1a5051a64a945ad43657943a Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Sat, 7 Dec 2024 18:46:35 -0500 Subject: [PATCH 11/16] updated logic for tank drive to use chassisppeed --- src/main/java/frc/robot/Constants.java | 13 +++- src/main/java/frc/robot/RobotContainer.java | 76 +++++-------------- .../frc/robot/commands/DriveDistance.java | 27 ++++++- .../java/frc/robot/commands/DriveTime.java | 7 +- .../java/frc/robot/commands/TankDrive.java | 23 +++++- .../java/frc/robot/commands/TurnDegrees.java | 7 +- .../frc/robot/subsystems/drive/Drive.java | 8 -- .../subsystems/drive/DriveSubsystemXrp.java | 22 +----- 8 files changed, 80 insertions(+), 103 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 859b5df..cf6ae17 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,16 @@ */ public final class Constants { public class DriveConstants { - public static double maxLinearVelocityMetersPerSec = 1.0; - public static double maxAngularVelocityRadPerSec = 4 * 2 * Math.PI; +// 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.06; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4136e38..31f9aa3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,14 +65,13 @@ public RobotContainer() { /** * Use this method to define your button->command mappings. Buttons can be - * created by - * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its - * subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing - * it to a {@link + * created by instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one + * of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or {@link + * XboxController}), and then passing it to a {@link * 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( @@ -80,6 +79,18 @@ private void configureButtonBindings() { () -> -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())); @@ -88,63 +99,10 @@ private void configureButtonBindings() { () -> mainController.getLeftTriggerAxis(), () -> mainController.getRightTriggerAxis())); - // toggles the drive mode between left stick controlling steering and right - // stick controlling steering - new Trigger(mainController.b().onTrue(new InstantCommand(this::handleBButtonPress))); - mainController.a().toggleOnTrue(new AutonomousDistance(drive)); - // Setup SmartDashboard options for autonomous routines - m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(drive)); - - + } - // toggles the drive mode between left stick controlling steering and right - // stick controlling steering - private void handleBButtonPress() { - - // Increment the button B press count - bPressCount++; - - // Use modulo to cycle through control schemes - int controlScheme = bPressCount % 3; - - /** - * Switches between different control schemes for the robot's steering based on - * the value of controlScheme. - * Each case represents a different control scheme: - * - Case 0: Left stick controls steering using ArcadeDrive. - * - Case 1: Right stick controls steering using ArcadeDrive. - * - Case 2: Both sticks control steering using TankDrive. - * - * @param controlScheme The current control scheme to use. - */ - switch (controlScheme) { - case 0: - // Left stick controls steering - new ArcadeDrive(drive, - () -> -mainController.getLeftY(), - () -> -mainController.getLeftX()).schedule(); - break; - case 1: - // Right stick controls steering - new ArcadeDrive( - drive, - () -> -mainController.getLeftY(), - () -> -mainController.getRightX()).schedule(); - break; - case 2: - // Both sticks control steering using TankDrive - new TankDrive( - drive, - () -> -mainController.getLeftY(), - () -> -mainController.getRightY()).schedule(); - break; - default: - // Default case (should not be reached) - break; - } - }; /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/commands/DriveDistance.java b/src/main/java/frc/robot/commands/DriveDistance.java index d618638..c471fa1 100644 --- a/src/main/java/frc/robot/commands/DriveDistance.java +++ b/src/main/java/frc/robot/commands/DriveDistance.java @@ -5,16 +5,22 @@ 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.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.DriveSubsystemXrp; 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; public class DriveDistance extends Command { private final DriveSubsystemXrp m_drive; private final double m_distance; private final double m_speed; private final PIDController m_pidController; - + private final DifferentialDriveKinematics m_kinematics; /** * Creates a new DriveDistance. This command will drive your your robot for a desired distance at @@ -29,13 +35,19 @@ public DriveDistance(double speed, double meters, DriveSubsystemXrp drive) { m_speed = speed; m_drive = drive; m_pidController = new PIDController(1.0, 0.0, 0.0); // Adjust PID constants as needed + + // Create a DifferentialDriveKinematics object with the track width + m_kinematics = + new DifferentialDriveKinematics(DriveConstants.trackWidthMeters); + addRequirements(drive); } // Called when the command is initially scheduled. @Override public void initialize() { - m_drive.setChassisSpeeds(0, 0); + + m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); m_drive.resetEncoders(); m_pidController.reset(); } @@ -51,13 +63,20 @@ public void execute() { double leftSpeed = m_speed - correction; double rightSpeed = m_speed + correction; - m_drive.setChassisSpeeds(leftSpeed, rightSpeed); + // Create a DifferentialDriveWheelSpeeds object with the wheel speeds + DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed); + + // Convert the wheel speeds to chassis speeds + ChassisSpeeds chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); + + + m_drive.setChassisSpeeds(chassisSpeeds); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_drive.setChassisSpeeds(0, 0); + m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/DriveTime.java b/src/main/java/frc/robot/commands/DriveTime.java index aab3cff..115bfa4 100644 --- a/src/main/java/frc/robot/commands/DriveTime.java +++ b/src/main/java/frc/robot/commands/DriveTime.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.DriveSubsystemXrp;; @@ -31,19 +32,19 @@ public DriveTime(double speed, double time, DriveSubsystemXrp drive) { @Override public void initialize() { m_startTime = System.currentTimeMillis(); - m_drive.setChassisSpeeds(0, 0); + m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.setChassisSpeeds(m_speed, 0); + m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_drive.setChassisSpeeds(0, 0); + m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/TankDrive.java b/src/main/java/frc/robot/commands/TankDrive.java index 22e8e07..5163b58 100644 --- a/src/main/java/frc/robot/commands/TankDrive.java +++ b/src/main/java/frc/robot/commands/TankDrive.java @@ -5,6 +5,8 @@ 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; @@ -16,6 +18,11 @@ public class TankDrive extends Command { 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 @@ -42,9 +49,19 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - drive.setChassisSpeeds( - rightWheelSpeedSupplier.get() * DriveConstants.maxLinearVelocityMetersPerSec, - leftWheelSpeedSupplier.get() * DriveConstants.maxAngularVelocityRadPerSec); + + 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. diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java index cd73312..d623365 100644 --- a/src/main/java/frc/robot/commands/TurnDegrees.java +++ b/src/main/java/frc/robot/commands/TurnDegrees.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.DriveSubsystemXrp; @@ -31,20 +32,20 @@ public TurnDegrees(double speed, double degrees, DriveSubsystemXrp drive) { @Override public void initialize() { // Set motors to stop, read encoder values for starting point - m_drive.setChassisSpeeds(0, 0); + m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.setChassisSpeeds(0, m_speed); + m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_drive.setChassisSpeeds(0, 0); + m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 7f97659..e00d20c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -18,14 +18,6 @@ public interface Drive { */ public void setChassisSpeeds(ChassisSpeeds speeds); - /** - * Sets the chassis speeds for the left and right wheels. - * - * @param leftSpeed The speed for the left wheel. - * @param rightSpeed The speed for the right wheel. - */ - public void setChassisSpeeds(double leftSpeed, double rightSpeed); - /** * Gets the current pose of the robot. * diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index eefe0e5..5cdee08 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -13,8 +13,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriveConstants; -import java.nio.channels.WritableByteChannel; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -59,9 +57,6 @@ public class DriveSubsystemXrp extends SubsystemBase implements Drive { public DriveSubsystemXrp(DriveDifferentialIO io) { this.io = io; - // START: Setup Odometry for dashboard display. - SmartDashboard.putData("Field", field); - // END: Setup Odometry } /** @@ -99,26 +94,11 @@ public void setChassisSpeeds(ChassisSpeeds speeds) { // the robot can move in a straight line) leftSpeed = wheelSpeeds.leftMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; rightSpeed = wheelSpeeds.rightMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; - + System.out.print("setChassisSpeeds leftSpeed: " + leftSpeed + " rightSpeed: " + rightSpeed + "\n"); // Drive the robot using the calculated wheel speeds io.tankDrive(leftSpeed, rightSpeed); } - /** - * Sets the chassis speeds for the left and right wheels. - * - * @param leftspeed The speed for the left wheel. - * @param rightspeed The speed for the right wheel. - */ - @Override - public void setChassisSpeeds(double leftSpeed, double rightSpeed) { - - leftSpeed = leftSpeed / DriveConstants.maxLinearVelocityMetersPerSec; - rightSpeed = rightSpeed / DriveConstants.maxLinearVelocityMetersPerSec; - - // Drive the robot using the calculated wheel speeds - io.tankDrive(leftSpeed, rightSpeed); - } public void resetEncoders(){ io.resetEncoders(); From 9fc33d081c9278273163ea71e9a096efc97dc04c Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Sun, 8 Dec 2024 10:51:13 -0500 Subject: [PATCH 12/16] Cleaned up logic. --- src/main/java/frc/robot/Constants.java | 18 +-- src/main/java/frc/robot/RobotContainer.java | 60 ++++------ .../robot/commands/AutonomousDistance.java | 6 +- .../frc/robot/commands/DriveDistance.java | 74 ++++++------ .../java/frc/robot/commands/DriveTime.java | 30 ++--- .../java/frc/robot/commands/TankDrive.java | 37 +++--- .../java/frc/robot/commands/TurnDegrees.java | 9 +- .../frc/robot/subsystems/drive/Drive.java | 13 +-- .../drive/DriveDifferentialIOXrp.java | 55 ++++----- .../subsystems/drive/DriveSubsystemXrp.java | 105 +++++++----------- 10 files changed, 174 insertions(+), 233 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cf6ae17..7b95d22 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,16 +14,16 @@ */ 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 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 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.06; + // 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.06; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 31f9aa3..2c2406e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,14 +5,13 @@ package frc.robot; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.ArcadeDrive; -import frc.robot.commands.TankDrive; +// This is neeeded if you are using TankDrive +// import frc.robot.commands.TankDrive; import frc.robot.commands.ArmCommand; +import frc.robot.commands.AutonomousDistance; import frc.robot.commands.IntakeCommand; import frc.robot.commands.ShooterCommand; import frc.robot.subsystems.arm.ArmIOXrp; @@ -23,20 +22,12 @@ import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterIOXrp; import frc.robot.subsystems.shooter.ShooterSubsystem; -import frc.robot.commands.AutonomousDistance; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; - /** - * This class is where the bulk of the robot should be declared. Since - * Command-based is a "declarative" paradigm, very little robot logic should - * actually be handled in - * the {@link Robot} periodic methods (other than the scheduler calls). Instead, - * the structure of - * the robot (including subsystems, commands, and button mappings) should be - * declared here. + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { // The robot's subsystems and commands are defined here... @@ -47,27 +38,16 @@ public class RobotContainer { private final CommandXboxController mainController = new CommandXboxController(0); - // Stores the number of times the "B" button has been pressed - private int bPressCount = 0; - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the button bindings configureButtonBindings(); - } - // Create SmartDashboard chooser for autonomous routines - private final SendableChooser m_chooser = new SendableChooser<>(); - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one - * of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or {@link - * XboxController}), and then passing it to a {@link + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { @@ -75,9 +55,7 @@ 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())); + new ArcadeDrive(drive, () -> -mainController.getLeftY(), () -> -mainController.getLeftX())); // // Arcade drive but left stick for throttle and right stick controls steering // drive.setDefaultCommand( @@ -85,25 +63,25 @@ private void configureButtonBindings() { // () -> -mainController.getLeftY(), // () -> -mainController.getRightX())); - //Both sticks control steering using TankDrive + // 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())); - shooter.setDefaultCommand(new ShooterCommand(shooter, - () -> mainController.getLeftTriggerAxis(), - () -> mainController.getRightTriggerAxis())); + shooter.setDefaultCommand( + new ShooterCommand( + shooter, + () -> mainController.getLeftTriggerAxis(), + () -> mainController.getRightTriggerAxis())); mainController.a().toggleOnTrue(new AutonomousDistance(drive)); - } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/AutonomousDistance.java b/src/main/java/frc/robot/commands/AutonomousDistance.java index 22236df..ea097ec 100644 --- a/src/main/java/frc/robot/commands/AutonomousDistance.java +++ b/src/main/java/frc/robot/commands/AutonomousDistance.java @@ -16,9 +16,7 @@ public class AutonomousDistance extends SequentialCommandGroup { */ public AutonomousDistance(DriveSubsystemXrp drivetrain) { addCommands( - new DriveDistance(-1, .1, drivetrain), - // new TurnDegrees(-0.5, 180, drivetrain), - new DriveDistance(1, .1, drivetrain)); - // new TurnDegrees(0.5, 180, drivetrain)); + new DriveDistance(1, .1, drivetrain), + new DriveDistance(-1, .1, drivetrain)); } } diff --git a/src/main/java/frc/robot/commands/DriveDistance.java b/src/main/java/frc/robot/commands/DriveDistance.java index c471fa1..3efc8e1 100644 --- a/src/main/java/frc/robot/commands/DriveDistance.java +++ b/src/main/java/frc/robot/commands/DriveDistance.java @@ -7,20 +7,19 @@ 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 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; public class DriveDistance extends Command { - private final DriveSubsystemXrp m_drive; - private final double m_distance; - private final double m_speed; - private final PIDController m_pidController; - private final DifferentialDriveKinematics m_kinematics; + private final DriveSubsystemXrp drive; + private final double distance; + private final double speed; + private final PIDController pidController; + private final DifferentialDriveKinematics kinematics; + private double rightWheelStartPosition; + private double leftWheelStartPosition; /** * Creates a new DriveDistance. This command will drive your your robot for a desired distance at @@ -30,15 +29,15 @@ public class DriveDistance extends Command { * @param meters The number of meters the robot will drive * @param drive The drivetrain subsystem on which this command will run */ - public DriveDistance(double speed, double meters, DriveSubsystemXrp drive) { - m_distance = meters; - m_speed = speed; - m_drive = drive; - m_pidController = new PIDController(1.0, 0.0, 0.0); // Adjust PID constants as needed - + public DriveDistance(double driveSpeed, double meters, DriveSubsystemXrp driveSubsystem) { + distance = meters; + speed = driveSpeed; + drive = driveSubsystem; + + pidController = new PIDController(1.0, 0.0, 0.0); // Adjust PID constants as needed + // Create a DifferentialDriveKinematics object with the track width - m_kinematics = - new DifferentialDriveKinematics(DriveConstants.trackWidthMeters); + kinematics = new DifferentialDriveKinematics(DriveConstants.trackWidthMeters); addRequirements(drive); } @@ -46,43 +45,50 @@ public DriveDistance(double speed, double meters, DriveSubsystemXrp drive) { // Called when the command is initially scheduled. @Override public void initialize() { - - m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); - m_drive.resetEncoders(); - m_pidController.reset(); + + drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + pidController.reset(); + rightWheelStartPosition = drive.getRightPositionMeters(); + leftWheelStartPosition = drive.getLeftPositionMeters(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double leftDistance = m_drive.getLeftPositionMeters(); - double rightDistance = m_drive.getLeftPositionMeters(); + double leftDistance = leftWheelStartPosition - drive.getLeftPositionMeters(); + double rightDistance = rightWheelStartPosition - drive.getRightPositionMeters(); + System.out.println("Left: " + leftDistance + " Right: " + rightDistance); + double error = leftDistance - rightDistance; - double correction = m_pidController.calculate(error); + double correction = pidController.calculate(error); - double leftSpeed = m_speed - correction; - double rightSpeed = m_speed + correction; + double leftSpeed = speed - correction; + double rightSpeed = speed + correction; // Create a DifferentialDriveWheelSpeeds object with the wheel speeds - DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed); + DifferentialDriveWheelSpeeds wheelSpeeds = + new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed); // Convert the wheel speeds to chassis speeds - ChassisSpeeds chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); + ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds); - - m_drive.setChassisSpeeds(chassisSpeeds); + drive.setChassisSpeeds(chassisSpeeds); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); + drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); } // Returns true when the command should end. @Override public boolean isFinished() { - // Compare distance travelled from start to desired distance - return Math.abs(m_drive.getAverageDistanceMeters()) >= m_distance; + double leftDistanceTraveled = leftWheelStartPosition - drive.getLeftPositionMeters(); + double rightDistanceTraveled = rightWheelStartPosition - drive.getRightPositionMeters(); + + double distanceTraveled = (leftDistanceTraveled + rightDistanceTraveled) / 2.0; + System.out.println("Distance Traveled: " + distanceTraveled); + return (Math.abs(distanceTraveled) >= distance); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/DriveTime.java b/src/main/java/frc/robot/commands/DriveTime.java index 115bfa4..abc9655 100644 --- a/src/main/java/frc/robot/commands/DriveTime.java +++ b/src/main/java/frc/robot/commands/DriveTime.java @@ -6,13 +6,13 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.drive.DriveSubsystemXrp;; +import frc.robot.subsystems.drive.DriveSubsystemXrp; public class DriveTime extends Command { - private final double m_duration; - private final double m_speed; - private final DriveSubsystemXrp m_drive; - private long m_startTime; + private final double duration; + private final double speed; + private final DriveSubsystemXrp drive; + private long startTime; /** * Creates a new DriveTime. This command will drive your robot for a desired speed and time. @@ -21,35 +21,35 @@ public class DriveTime extends Command { * @param time How much time to drive in seconds * @param drive The drivetrain subsystem on which this command will run */ - public DriveTime(double speed, double time, DriveSubsystemXrp drive) { - m_speed = speed; - m_duration = time * 1000; - m_drive = drive; + public DriveTime(double robotSpeed, double time, DriveSubsystemXrp driveSubsystem) { + speed = robotSpeed; + duration = time * 1000; + drive = driveSubsystem; addRequirements(drive); } // Called when the command is initially scheduled. @Override public void initialize() { - m_startTime = System.currentTimeMillis(); - m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); + startTime = System.currentTimeMillis(); + drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); + drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); + drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); } // Returns true when the command should end. @Override public boolean isFinished() { - return (System.currentTimeMillis() - m_startTime) >= m_duration; + return (System.currentTimeMillis() - startTime) >= duration; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/TankDrive.java b/src/main/java/frc/robot/commands/TankDrive.java index 5163b58..b911c4d 100644 --- a/src/main/java/frc/robot/commands/TankDrive.java +++ b/src/main/java/frc/robot/commands/TankDrive.java @@ -18,23 +18,22 @@ public class TankDrive extends Command { private final Supplier rightWheelSpeedSupplier; private final Supplier leftWheelSpeedSupplier; - // Create a DifferentialDriveKinematics object with the track width - private DifferentialDriveKinematics kinematics = - new DifferentialDriveKinematics(DriveConstants.trackWidthMeters); + private DifferentialDriveKinematics kinematics = + new DifferentialDriveKinematics(DriveConstants.trackWidthMeters); /** - * Creates a new TankDrive. This command will drive your robot according to the - * speed supplier + * 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 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) { + Drive drive, + Supplier leftWheelSpeedSupplier, + Supplier rightWheelSpeedSupplier) { this.drive = drive; this.rightWheelSpeedSupplier = leftWheelSpeedSupplier; this.leftWheelSpeedSupplier = rightWheelSpeedSupplier; @@ -43,31 +42,31 @@ public TankDrive( // Called when the command is initially scheduled. @Override - public void initialize() { - } + 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; + + 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); - + 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) { - } + public void end(boolean interrupted) {} // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java index d623365..cddf0b7 100644 --- a/src/main/java/frc/robot/commands/TurnDegrees.java +++ b/src/main/java/frc/robot/commands/TurnDegrees.java @@ -32,27 +32,26 @@ public TurnDegrees(double speed, double degrees, DriveSubsystemXrp drive) { @Override public void initialize() { // Set motors to stop, read encoder values for starting point - m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); - + m_drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); + m_drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_drive.setChassisSpeeds(new ChassisSpeeds(0,0,0)); + m_drive.setChassisSpeeds(new ChassisSpeeds(0, 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; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e00d20c..3364c37 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -26,15 +26,12 @@ public interface Drive { 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. + * 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. + * @param pose The pose to set, which includes the x and y coordinates and the orientation. */ public void setPose(Pose2d pose); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java index 06f1946..b1b2e85 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveDifferentialIOXrp.java @@ -9,37 +9,29 @@ 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. + * 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 + 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 + * 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. + * 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 @@ -54,7 +46,8 @@ public class DriveDifferentialIOXrp implements DriveDifferentialIO { private final Encoder rightEncoder = new Encoder(6, 7); // Set up the differential drive controller - private final DifferentialDrive differentialDrive = new DifferentialDrive(leftMotor::set, rightMotor::set); + private final DifferentialDrive differentialDrive = + new DifferentialDrive(leftMotor::set, rightMotor::set); // Set up the XRPGyro private final XRPGyro gyro = new XRPGyro(); @@ -73,14 +66,12 @@ public DriveDifferentialIOXrp() { 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. + * 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(); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index 5cdee08..d07f2af 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -9,10 +9,8 @@ import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriveConstants; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -22,7 +20,8 @@ public class DriveSubsystemXrp extends SubsystemBase implements Drive { private final DriveDifferentialIO io; // The inputs for the differential drive, automatically logged. - private final DriveDifferentialIOInputsAutoLogged inputs = new DriveDifferentialIOInputsAutoLogged(); + private final DriveDifferentialIOInputsAutoLogged inputs = + new DriveDifferentialIOInputsAutoLogged(); // The diameter of the robot's wheels in meters (60 mm). private static final double wheelDiameterMeters = 0.060; @@ -32,38 +31,37 @@ public class DriveSubsystemXrp extends SubsystemBase implements Drive { // The kinematics object used to convert between chassis speeds and wheel // speeds. - private final DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(trackWidthMeters); + private final DifferentialDriveKinematics kinematics = + new DifferentialDriveKinematics(trackWidthMeters); // 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); + 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(); /** - * The current pose of the robot, including its position and orientation. - * This field is automatically logged. + * The current pose of the robot, including its position and orientation. This field is + * automatically logged. */ - @AutoLogOutput - private Pose2d pose; + @AutoLogOutput private Pose2d pose; /** * Constructs a new DriveSubsystemXrp. * - * @param io The IO subsystem used to interface with the robot's differential - * drive hardware. + * @param io The IO subsystem used to interface with the robot's differential drive hardware. */ public DriveSubsystemXrp(DriveDifferentialIO io) { this.io = io; - } /** * 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. + * @return The current chassis speeds, represented as a ChassisSpeeds object. This includes the + * linear and angular velocities of the robot. */ @Override public ChassisSpeeds getChassisSpeeds() { @@ -74,40 +72,32 @@ public ChassisSpeeds getChassisSpeeds() { } /** - * 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 + * 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. + * @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) { - // Declare variables for left and right wheel speeds - double leftSpeed; - double rightSpeed; + DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(speeds); // Normalize wheel speeds by the maximum linear velocity (highest speed at which // the robot can move in a straight line) - leftSpeed = wheelSpeeds.leftMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; - rightSpeed = wheelSpeeds.rightMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; - System.out.print("setChassisSpeeds leftSpeed: " + leftSpeed + " rightSpeed: " + rightSpeed + "\n"); + double leftSpeed = + wheelSpeeds.leftMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; + double rightSpeed = + wheelSpeeds.rightMetersPerSecond / DriveConstants.maxLinearVelocityMetersPerSec; + // Drive the robot using the calculated wheel speeds io.tankDrive(leftSpeed, rightSpeed); } - - public void resetEncoders(){ - io.resetEncoders(); - } - /** - * 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 + * 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. @@ -117,15 +107,12 @@ public Pose2d getPose() { } /** - * 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. + * 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. + * @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 @@ -139,11 +126,10 @@ public void setPose(Pose2d pose) { } /** - * 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. + * 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() { @@ -152,15 +138,15 @@ public void periodic() { Logger.processInputs("Drive", inputs); // Update the robot's pose using odometry - pose = odometry.update( - inputs.zRotation, - new DifferentialDriveWheelPositions( - Units.radiansToRotations(inputs.leftPositionRad) * wheelDiameterMeters, - Units.radiansToRotations(inputs.rightPositionRad) * wheelDiameterMeters)); + 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); - } /** @@ -169,7 +155,6 @@ public void periodic() { * @return the current position of the right wheels (in meters) */ public double getRightPositionMeters() { - double position = Units.radiansToRotations(inputs.rightPositionRad) * wheelDiameterMeters; return Units.radiansToRotations(inputs.rightPositionRad) * wheelDiameterMeters; } @@ -179,21 +164,9 @@ public double getRightPositionMeters() { * @return the current position of the right wheels (in meters) */ public double getLeftPositionMeters() { - double position = Units.radiansToRotations(inputs.leftPositionRad) * wheelDiameterMeters; return Units.radiansToRotations(inputs.leftPositionRad) * wheelDiameterMeters; } - /** - * The average wheel distance in meters - * - * @return the average wheel distance (in meters) - */ - public double getAverageDistanceMeters() { - double distanceTraveled = (getLeftPositionMeters() + getRightPositionMeters()) / 2.0; - System.out.println("Distance Traveled: " + distanceTraveled); - return (getLeftPositionMeters() + getRightPositionMeters()) / 2.0; - } - @Override public void simulationPeriodic() { // This method will be called once per scheduler run during simulation From 4397cc0a3c6f299d67bb6502146bd38abb14934f Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Sun, 8 Dec 2024 15:01:43 -0500 Subject: [PATCH 13/16] updating per pull request comments. --- src/main/java/frc/robot/Constants.java | 4 ++ src/main/java/frc/robot/RobotContainer.java | 10 ++-- ...usDistance.java => AutonomousCommand.java} | 15 +++-- .../frc/robot/commands/DriveDistance.java | 11 +++- .../java/frc/robot/commands/DriveTime.java | 55 ------------------- .../java/frc/robot/commands/TurnDegrees.java | 8 +-- 6 files changed, 32 insertions(+), 71 deletions(-) rename src/main/java/frc/robot/commands/{AutonomousDistance.java => AutonomousCommand.java} (50%) delete mode 100644 src/main/java/frc/robot/commands/DriveTime.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7b95d22..49617ae 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,5 +25,9 @@ public class DriveConstants { // 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.06; + + public static double wheelkP = 0.1; + public static double wheelkI = 0.0; + public static double wheelkD = 0.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c2406e..0f7b019 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,7 @@ // This is neeeded if you are using TankDrive // import frc.robot.commands.TankDrive; import frc.robot.commands.ArmCommand; -import frc.robot.commands.AutonomousDistance; +import frc.robot.commands.AutonomousCommand; import frc.robot.commands.IntakeCommand; import frc.robot.commands.ShooterCommand; import frc.robot.subsystems.arm.ArmIOXrp; @@ -78,8 +78,6 @@ private void configureButtonBindings() { shooter, () -> mainController.getLeftTriggerAxis(), () -> mainController.getRightTriggerAxis())); - - mainController.a().toggleOnTrue(new AutonomousDistance(drive)); } /** @@ -88,7 +86,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/AutonomousDistance.java b/src/main/java/frc/robot/commands/AutonomousCommand.java similarity index 50% rename from src/main/java/frc/robot/commands/AutonomousDistance.java rename to src/main/java/frc/robot/commands/AutonomousCommand.java index ea097ec..29217f1 100644 --- a/src/main/java/frc/robot/commands/AutonomousDistance.java +++ b/src/main/java/frc/robot/commands/AutonomousCommand.java @@ -4,19 +4,26 @@ 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 AutonomousDistance extends SequentialCommandGroup { +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 AutonomousDistance(DriveSubsystemXrp drivetrain) { + public AutonomousCommand(DriveSubsystemXrp drivetrain) { + + // Creates a PID Controller for the wheels going in a straight line + PIDController wheelPIDController = + new PIDController(DriveConstants.wheelkP, DriveConstants.wheelkI, DriveConstants.wheelkD); + addCommands( - new DriveDistance(1, .1, drivetrain), - new DriveDistance(-1, .1, drivetrain)); + new DriveDistance(1, .1, drivetrain, wheelPIDController), // Drive forward + new DriveDistance(-1, .1, drivetrain, wheelPIDController)); // Drive backward } } diff --git a/src/main/java/frc/robot/commands/DriveDistance.java b/src/main/java/frc/robot/commands/DriveDistance.java index 3efc8e1..e77275d 100644 --- a/src/main/java/frc/robot/commands/DriveDistance.java +++ b/src/main/java/frc/robot/commands/DriveDistance.java @@ -29,12 +29,16 @@ public class DriveDistance extends Command { * @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) { + public DriveDistance( + double driveSpeed, + double meters, + DriveSubsystemXrp driveSubsystem, + PIDController wheelPIDController) { distance = meters; speed = driveSpeed; drive = driveSubsystem; - pidController = new PIDController(1.0, 0.0, 0.0); // Adjust PID constants as needed + pidController = wheelPIDController; // Adjust PID constants as needed // Create a DifferentialDriveKinematics object with the track width kinematics = new DifferentialDriveKinematics(DriveConstants.trackWidthMeters); @@ -50,6 +54,8 @@ public void initialize() { pidController.reset(); rightWheelStartPosition = drive.getRightPositionMeters(); leftWheelStartPosition = drive.getLeftPositionMeters(); + + return; } // Called every time the scheduler runs while the command is scheduled. @@ -57,7 +63,6 @@ public void initialize() { public void execute() { double leftDistance = leftWheelStartPosition - drive.getLeftPositionMeters(); double rightDistance = rightWheelStartPosition - drive.getRightPositionMeters(); - System.out.println("Left: " + leftDistance + " Right: " + rightDistance); double error = leftDistance - rightDistance; double correction = pidController.calculate(error); diff --git a/src/main/java/frc/robot/commands/DriveTime.java b/src/main/java/frc/robot/commands/DriveTime.java deleted file mode 100644 index abc9655..0000000 --- a/src/main/java/frc/robot/commands/DriveTime.java +++ /dev/null @@ -1,55 +0,0 @@ -// 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.wpilibj2.command.Command; -import frc.robot.subsystems.drive.DriveSubsystemXrp; - -public class DriveTime extends Command { - private final double duration; - private final double speed; - private final DriveSubsystemXrp drive; - private long startTime; - - /** - * Creates a new DriveTime. This command will drive your robot for a desired speed and time. - * - * @param speed The speed which the robot will drive. Negative is in reverse. - * @param time How much time to drive in seconds - * @param drive The drivetrain subsystem on which this command will run - */ - public DriveTime(double robotSpeed, double time, DriveSubsystemXrp driveSubsystem) { - speed = robotSpeed; - duration = time * 1000; - drive = driveSubsystem; - addRequirements(drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - startTime = System.currentTimeMillis(); - drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); - } - - // 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() { - return (System.currentTimeMillis() - startTime) >= duration; - } -} diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java index cddf0b7..a621df2 100644 --- a/src/main/java/frc/robot/commands/TurnDegrees.java +++ b/src/main/java/frc/robot/commands/TurnDegrees.java @@ -4,7 +4,6 @@ package frc.robot.commands; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.DriveSubsystemXrp; @@ -32,19 +31,20 @@ public TurnDegrees(double speed, double degrees, DriveSubsystemXrp drive) { @Override public void initialize() { // Set motors to stop, read encoder values for starting point - m_drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + // m_drive.setChassisSpeeds(0, 0); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + // m_drive.setChassisSpeeds(0, m_speed); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + // m_drive.setChassisSpeeds(0, 0); } // Returns true when the command should end. From 2d1e03b2de14a0ea69d8f3e1b1fec7e19810ef94 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Sun, 8 Dec 2024 15:11:34 -0500 Subject: [PATCH 14/16] Putting field back into DriveSubSystemXRP.java --- .../java/frc/robot/subsystems/drive/DriveSubsystemXrp.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index d07f2af..36d1c09 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriveConstants; import org.littletonrobotics.junction.AutoLogOutput; @@ -55,6 +56,10 @@ public class DriveSubsystemXrp extends SubsystemBase implements Drive { */ public DriveSubsystemXrp(DriveDifferentialIO io) { this.io = io; + + // START: Setup Odometry + SmartDashboard.putData("Field", field); + // END: Setup Odometry } /** From 9df19e550acbb70a13a22e97ecf554128c6bf19b Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Wed, 11 Dec 2024 18:55:11 -0500 Subject: [PATCH 15/16] adding more autonous stuff --- src/main/java/frc/robot/Constants.java | 43 ++++++++++-- src/main/java/frc/robot/RobotContainer.java | 5 ++ .../frc/robot/commands/AutonomousCommand.java | 13 +++- .../frc/robot/commands/DriveDistance.java | 70 +++++++++++++------ .../frc/robot/subsystems/drive/Drive.java | 36 ++++++++-- .../subsystems/drive/DriveSubsystemXrp.java | 27 ++++++- 6 files changed, 160 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 49617ae..af3a063 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,10 +24,45 @@ public class DriveConstants { // 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.06; + 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; + } + - public static double wheelkP = 0.1; - public static double wheelkI = 0.0; - public static double wheelkD = 0.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f7b019..029e16b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,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 @@ -30,6 +31,10 @@ * 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)); diff --git a/src/main/java/frc/robot/commands/AutonomousCommand.java b/src/main/java/frc/robot/commands/AutonomousCommand.java index 29217f1..87c561c 100644 --- a/src/main/java/frc/robot/commands/AutonomousCommand.java +++ b/src/main/java/frc/robot/commands/AutonomousCommand.java @@ -20,10 +20,17 @@ public AutonomousCommand(DriveSubsystemXrp drivetrain) { // Creates a PID Controller for the wheels going in a straight line PIDController wheelPIDController = - new PIDController(DriveConstants.wheelkP, DriveConstants.wheelkI, DriveConstants.wheelkD); + 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), // Drive forward - new DriveDistance(-1, .1, drivetrain, wheelPIDController)); // Drive backward + 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 index e77275d..c20c983 100644 --- a/src/main/java/frc/robot/commands/DriveDistance.java +++ b/src/main/java/frc/robot/commands/DriveDistance.java @@ -11,15 +11,18 @@ 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 final DriveSubsystemXrp drive; - private final double distance; - private final double speed; - private final PIDController pidController; - private final DifferentialDriveKinematics kinematics; + 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 @@ -29,19 +32,26 @@ public class DriveDistance extends Command { * @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 wheelPIDController) { + PIDController dPIDController, + PIDController zPIDController + ) + { + distance = meters; speed = driveSpeed; drive = driveSubsystem; - - pidController = wheelPIDController; // Adjust PID constants as needed - + 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); } @@ -49,13 +59,12 @@ public DriveDistance( // Called when the command is initially scheduled. @Override public void initialize() { - + drive.resetEncoders(); drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); - pidController.reset(); + distancePIDController.reset(); rightWheelStartPosition = drive.getRightPositionMeters(); leftWheelStartPosition = drive.getLeftPositionMeters(); - - return; + zRotationStartingPosition = drive.getAngleZ(); } // Called every time the scheduler runs while the command is scheduled. @@ -64,20 +73,42 @@ public void execute() { double leftDistance = leftWheelStartPosition - drive.getLeftPositionMeters(); double rightDistance = rightWheelStartPosition - drive.getRightPositionMeters(); - double error = leftDistance - rightDistance; - double correction = pidController.calculate(error); + double distanceError = leftDistance - rightDistance; - double leftSpeed = speed - correction; - double rightSpeed = speed + correction; + 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); + new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed); - // Convert the wheel speeds to chassis speeds + // // 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. @@ -93,7 +124,6 @@ public boolean isFinished() { double rightDistanceTraveled = rightWheelStartPosition - drive.getRightPositionMeters(); double distanceTraveled = (leftDistanceTraveled + rightDistanceTraveled) / 2.0; - System.out.println("Distance Traveled: " + distanceTraveled); return (Math.abs(distanceTraveled) >= distance); } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3364c37..ffc1340 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -26,12 +26,38 @@ public interface Drive { 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 + * 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. + * @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/DriveSubsystemXrp.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java index 36d1c09..1b10352 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystemXrp.java @@ -25,10 +25,10 @@ public class DriveSubsystemXrp extends SubsystemBase implements Drive { new DriveDifferentialIOInputsAutoLogged(); // The diameter of the robot's wheels in meters (60 mm). - private static final double wheelDiameterMeters = 0.060; + private static final double wheelDiameterMeters = DriveConstants.wheelDiameterMeters; // The track width of the robot in meters (155 mm). - private static final double trackWidthMeters = 0.155; + private static final double trackWidthMeters = DriveConstants.trackWidthMeters; // The kinematics object used to convert between chassis speeds and wheel // speeds. @@ -130,6 +130,11 @@ public void setPose(Pose2d 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 @@ -172,6 +177,24 @@ 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 public void simulationPeriodic() { // This method will be called once per scheduler run during simulation From 3159d50c8055be241299511058971070920314a5 Mon Sep 17 00:00:00 2001 From: Eric Wojtaszek Date: Wed, 11 Dec 2024 19:27:21 -0500 Subject: [PATCH 16/16] adding logic to get both servos to move at the same time: --- src/main/java/frc/robot/RobotContainer.java | 9 +++----- .../robot/subsystems/arm/ArmSubsystem.java | 22 ++++++++++++------- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 029e16b..74be256 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,13 +32,10 @@ */ 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); @@ -76,7 +73,7 @@ private void configureButtonBindings() { 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( 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; } }