From a88dbc27579a426419e29bcbe71341ee87a5aee0 Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Fri, 15 Mar 2024 17:59:21 -0700 Subject: [PATCH 01/11] switch pid controller to be created within the pid drive command --- src/main/java/frc/robot/Constants.java | 4 ++++ .../java/frc/robot/commands/PidDistanceDriveCommand.java | 5 +++-- src/main/java/frc/robot/subsystems/Drivetrain.java | 8 -------- 3 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7e36adf..d0db4f5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -35,6 +35,10 @@ public static class DrivetrainConstants { // 1 meter = 39.37 inches = 2.088 wheel rotations = 17.664 motor rotations (assuming gear ratio = 8.46) public static final double METERS_TO_ROTATIONS = 17.664; public static final double DEGREES_TO_ROTATIONS = 0.1; // value accurate as of 2/13/24 + + public static final double drive_P = 0; + public static final double drive_I = 0; + public static final double drive_D = 0; } public static class IntakeConstants { diff --git a/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java b/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java index d50d0f7..6303011 100644 --- a/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java +++ b/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java @@ -27,11 +27,12 @@ public class PidDistanceDriveCommand extends Command { * Takes in targetDistance in meters (can be positive or negative) */ public PidDistanceDriveCommand(Drivetrain drivetrain, double targetDistance) { + m_drivetrain = drivetrain; // the encoders measure distance in number of rotations, so we need to convert to that from meters - m_targetDistance = targetDistance * DrivetrainConstants.METERS_TO_ROTATIONS; - m_controller = m_drivetrain.drive_controller; + m_targetDistance = targetDistance * DrivetrainConstants.METERS_TO_ROTATIONS; + m_controller = new PIDController(DrivetrainConstants.drive_P, DrivetrainConstants.drive_I, DrivetrainConstants.drive_D); m_controller.disableContinuousInput(); m_controller.setTolerance(1.0); // TO DO: tune position tolerance diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 5722169..4da737a 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -14,8 +14,6 @@ import com.revrobotics.SparkRelativeEncoder; import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.math.controller.PIDController; - public class Drivetrain extends SubsystemBase { // instance variables @@ -44,12 +42,6 @@ public class Drivetrain extends SubsystemBase { .getEncoder(SparkRelativeEncoder.Type.kHallSensor, DrivetrainConstants.COUNTS_PER_REV); public static DifferentialDrive m_robotDrive; - - // pid - private final double drive_P = 0; - private final double drive_I = 0; - private final double drive_D = 0; - public PIDController drive_controller = new PIDController(drive_P, drive_I, drive_D); public Drivetrain(OI humanControl) { m_robotDrive = new DifferentialDrive(m_rightPrimary, m_leftPrimary); From 07b01773520c41d389774f6913ca9185448ecb52 Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Fri, 15 Mar 2024 18:27:11 -0700 Subject: [PATCH 02/11] add navx to code --- src/main/java/frc/robot/Constants.java | 7 +-- .../java/frc/robot/subsystems/Drivetrain.java | 43 ++++++++++++++++++- vendordeps/NavX.json | 40 +++++++++++++++++ 3 files changed, 85 insertions(+), 5 deletions(-) create mode 100644 vendordeps/NavX.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d0db4f5..36e72a6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -35,10 +35,11 @@ public static class DrivetrainConstants { // 1 meter = 39.37 inches = 2.088 wheel rotations = 17.664 motor rotations (assuming gear ratio = 8.46) public static final double METERS_TO_ROTATIONS = 17.664; public static final double DEGREES_TO_ROTATIONS = 0.1; // value accurate as of 2/13/24 + public static final double ENCODER_TICKS_TO_METERS = 0.0013; // calculated based on METERS_TO_ROTATIONS and COUNTS_PER_REV - public static final double drive_P = 0; - public static final double drive_I = 0; - public static final double drive_D = 0; + public static final double drive_P = 0; // TEST + public static final double drive_I = 0; // TEST + public static final double drive_D = 0; // TEST } public static class IntakeConstants { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 4da737a..fe121f7 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -12,8 +12,17 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.SparkRelativeEncoder; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.I2C.Port; + public class Drivetrain extends SubsystemBase { // instance variables @@ -41,8 +50,16 @@ public class Drivetrain extends SubsystemBase { public final SparkRelativeEncoder m_rightSecondaryEncoder = (SparkRelativeEncoder) m_rightSecondary .getEncoder(SparkRelativeEncoder.Type.kHallSensor, DrivetrainConstants.COUNTS_PER_REV); + // odometry public static DifferentialDrive m_robotDrive; - + // private final DifferentialDriveKinematics m_kinematics = new DifferentialDriveKinematics(DrivetrainConstants.TRACK_WIDTH); + public static AHRS m_gyro = new AHRS(Port.kMXP); + + public DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry( + m_gyro.getRotation2d(), + getLeftEncoderPosition(), getRightEncoderPosition(), + new Pose2d(0, 0, new Rotation2d())); + public Drivetrain(OI humanControl) { m_robotDrive = new DifferentialDrive(m_rightPrimary, m_leftPrimary); m_humanControl = humanControl; @@ -51,7 +68,12 @@ public Drivetrain(OI humanControl) { } @Override - public void periodic() {} + public void periodic() { + // Update the robot pose accordingly + m_odometry.update(m_gyro.getRotation2d(), + DrivetrainConstants.ENCODER_TICKS_TO_METERS * getLeftEncoderPosition(), + DrivetrainConstants.ENCODER_TICKS_TO_METERS * getRightEncoderPosition()); + } public void configDrivetrainMotors() { @@ -99,6 +121,23 @@ public void setleftEncoder(double position) { m_leftPrimaryEncoder.setPosition(position); } + // odometry methods + + // returns current robot pose in meters + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } + + // returns current robot rotation + public Rotation2d getRotation() { + return m_gyro.getRotation2d(); + } + + // resets robot pose to given pose + public void resetPose(Pose2d pose) { + m_odometry.resetPosition(m_gyro.getRotation2d(), new DifferentialDriveWheelPositions(getLeftEncoderPosition(), getRightEncoderPosition()), pose); + } + @Override public void simulationPeriodic() {} diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..e978a5f --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2024/" + ], + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2024.1.0", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file From c8ce662e104d7fdcfdc106343c8e585537ae8a0b Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Mon, 18 Mar 2024 18:05:54 -0700 Subject: [PATCH 03/11] add pid rotation command + button bindings --- src/main/java/frc/robot/RobotContainer.java | 9 ++- .../robot/commands/NavXRotationCommand.java | 71 +++++++++++++++++++ .../commands/PidDistanceDriveCommand.java | 18 ++--- .../java/frc/robot/subsystems/Drivetrain.java | 4 ++ 4 files changed, 91 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/robot/commands/NavXRotationCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 712ac65..2e37016 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,10 +14,12 @@ import frc.robot.commands.IndexerUpCommand; import frc.robot.commands.IndexerDownCommand; import frc.robot.commands.IntakeOutCommand; +import frc.robot.commands.NavXRotationCommand; import frc.robot.commands.IntakeInCommand; import frc.robot.commands.IntakeIndexCommandBackup; import frc.robot.commands.IntakeIndexCommand; import frc.robot.commands.OuttakeIndexCommand; +import frc.robot.commands.PidDistanceDriveCommand; import frc.robot.commands.PrintClimberEncoder; import frc.robot.commands.BeamBreakCommand; import frc.robot.commands.ClimberUpCommand; @@ -129,8 +131,11 @@ public RobotContainer() { */ private void configureBindings() { // temp for testing - OI.m_coPilotXbox.a().whileTrue(new ShooterHoodBackward(m_shooter, m_hood)); // emergency use button that should not be pressed in normal circumstances - OI.m_coPilotXbox.b().whileTrue(new OuttakeIndexCommand(m_indexer, m_intake)); // outtaking should not normally be necessary + // OI.m_coPilotXbox.a().whileTrue(new ShooterHoodBackward(m_shooter, m_hood)); // emergency use button that should not be pressed in normal circumstances + // OI.m_coPilotXbox.b().whileTrue(new OuttakeIndexCommand(m_indexer, m_intake)); // outtaking should not normally be necessary + + OI.m_coPilotXbox.a().onTrue(new PidDistanceDriveCommand(m_drivetrain, 1)); + OI.m_coPilotXbox.b().onTrue(new NavXRotationCommand(m_drivetrain, 90)); OI.m_coPilotXbox.x().whileTrue(new IntakeIndexCommand(m_indexer, m_intake)); OI.m_coPilotXbox.y().whileTrue(new IndexerUpCommand(m_indexer)); diff --git a/src/main/java/frc/robot/commands/NavXRotationCommand.java b/src/main/java/frc/robot/commands/NavXRotationCommand.java new file mode 100644 index 0000000..eb15fc0 --- /dev/null +++ b/src/main/java/frc/robot/commands/NavXRotationCommand.java @@ -0,0 +1,71 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.AutoConstants; +import frc.robot.subsystems.Drivetrain; + +public class NavXRotationCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Drivetrain m_drivetrain; + private double m_targetDegrees; + private double m_startDegrees; + private double m_thrust; + + /** + * A NavX based turn command. + * Takes in target angle RELATIVE TO FIELD. + * 0 = facing the field from the alliance wall + * Negative values turning left, positive values turning right + * All values between -180 and 180. + */ + public NavXRotationCommand(Drivetrain drivetrain, double targetDegrees) { + m_drivetrain = drivetrain; + m_targetDegrees = targetDegrees; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + // sets initial position to 0 + // m_drivetrain.setRightEncoder(0); + // m_drivetrain.setleftEncoder(0); + m_startDegrees = m_drivetrain.getDegrees(); // value in degrees between -180 and 180 + // m_rightEncoderStart = m_drivetrain.getRightEncoderPosition(); + // m_leftEncoderStart = m_drivetrain.getLeftEncoderPosition(); + + // get appropriate thrust sign + if (m_startDegrees < m_targetDegrees) { + if (m_targetDegrees - m_startDegrees < 180) { + m_thrust = AutoConstants.ROTATION_SPEED; + } else { + m_thrust = -AutoConstants.ROTATION_SPEED; + } + } else { + if (m_startDegrees - m_targetDegrees < 180) { + m_thrust = -AutoConstants.ROTATION_SPEED; + } else { + m_thrust = AutoConstants.ROTATION_SPEED; + } + } + } + + @Override + public void execute() { + System.out.println("Target degrees: " + m_targetDegrees + ", Current degrees: " + m_drivetrain.getDegrees()); + m_drivetrain.tankDrive(m_thrust, -m_thrust); // drives in two opposite directions so the robot spins + // System.out.println("Left encoder position: " + m_drivetrain.getLeftEncoderPosition() + ", Right encoder position: " + m_drivetrain.getRightEncoderPosition()); + } + + @Override + public void end(boolean interrupted) { + m_drivetrain.stopMotors(); + // m_drivetrain.setRightEncoder(0); + // m_drivetrain.setleftEncoder(0); + } + + @Override + public boolean isFinished() { + // calculates if either encoder has moved past the target number of rotations + return Math.abs(m_drivetrain.getDegrees() - m_targetDegrees) < 0.1; + } +} diff --git a/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java b/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java index 6303011..bb2acb9 100644 --- a/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java +++ b/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java @@ -20,7 +20,7 @@ public class PidDistanceDriveCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final Drivetrain m_drivetrain; private double m_targetDistance; - private PIDController m_controller; + private PIDController m_driveController; /* * Drives a given distance straight. @@ -32,9 +32,9 @@ public PidDistanceDriveCommand(Drivetrain drivetrain, double targetDistance) { // the encoders measure distance in number of rotations, so we need to convert to that from meters m_targetDistance = targetDistance * DrivetrainConstants.METERS_TO_ROTATIONS; - m_controller = new PIDController(DrivetrainConstants.drive_P, DrivetrainConstants.drive_I, DrivetrainConstants.drive_D); - m_controller.disableContinuousInput(); - m_controller.setTolerance(1.0); // TO DO: tune position tolerance + m_driveController = new PIDController(DrivetrainConstants.drive_P, DrivetrainConstants.drive_I, DrivetrainConstants.drive_D); + m_driveController.disableContinuousInput(); + m_driveController.setTolerance(1.0); // TO DO: tune position tolerance addRequirements(drivetrain); } @@ -46,14 +46,14 @@ public void initialize() { m_drivetrain.setRightEncoder(0); m_drivetrain.setleftEncoder(0); - m_controller.reset(); - m_controller.setSetpoint(m_targetDistance); + m_driveController.reset(); + m_driveController.setSetpoint(m_targetDistance); } @Override public void execute() { - // sets both motors to the thrust calculated using the left encoder values, is this bad? - double thrust = m_controller.calculate(m_drivetrain.getLeftEncoderPosition()); + // takes average of two encoders ??? to ensure straight-ness? + double thrust = m_driveController.calculate((m_drivetrain.getLeftEncoderPosition() + m_drivetrain.getRightEncoderPosition())/2); m_drivetrain.tankDrive(thrust, thrust); } @@ -66,6 +66,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return m_controller.atSetpoint(); + return m_driveController.atSetpoint(); } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index fe121f7..6d288f9 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -133,6 +133,10 @@ public Rotation2d getRotation() { return m_gyro.getRotation2d(); } + public double getDegrees() { + return m_gyro.getRotation2d().getDegrees(); + } + // resets robot pose to given pose public void resetPose(Pose2d pose) { m_odometry.resetPosition(m_gyro.getRotation2d(), new DifferentialDriveWheelPositions(getLeftEncoderPosition(), getRightEncoderPosition()), pose); From e81e7519f936704a5854c654fd7edb59c8e4b0f4 Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Tue, 26 Mar 2024 18:16:29 -0700 Subject: [PATCH 04/11] Revert "add pid rotation command + button bindings" This reverts commit c8ce662e104d7fdcfdc106343c8e585537ae8a0b. --- src/main/java/frc/robot/RobotContainer.java | 9 +-- .../robot/commands/NavXRotationCommand.java | 71 ------------------- .../commands/PidDistanceDriveCommand.java | 18 ++--- .../java/frc/robot/subsystems/Drivetrain.java | 4 -- 4 files changed, 11 insertions(+), 91 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/NavXRotationCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2e37016..712ac65 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,12 +14,10 @@ import frc.robot.commands.IndexerUpCommand; import frc.robot.commands.IndexerDownCommand; import frc.robot.commands.IntakeOutCommand; -import frc.robot.commands.NavXRotationCommand; import frc.robot.commands.IntakeInCommand; import frc.robot.commands.IntakeIndexCommandBackup; import frc.robot.commands.IntakeIndexCommand; import frc.robot.commands.OuttakeIndexCommand; -import frc.robot.commands.PidDistanceDriveCommand; import frc.robot.commands.PrintClimberEncoder; import frc.robot.commands.BeamBreakCommand; import frc.robot.commands.ClimberUpCommand; @@ -131,11 +129,8 @@ public RobotContainer() { */ private void configureBindings() { // temp for testing - // OI.m_coPilotXbox.a().whileTrue(new ShooterHoodBackward(m_shooter, m_hood)); // emergency use button that should not be pressed in normal circumstances - // OI.m_coPilotXbox.b().whileTrue(new OuttakeIndexCommand(m_indexer, m_intake)); // outtaking should not normally be necessary - - OI.m_coPilotXbox.a().onTrue(new PidDistanceDriveCommand(m_drivetrain, 1)); - OI.m_coPilotXbox.b().onTrue(new NavXRotationCommand(m_drivetrain, 90)); + OI.m_coPilotXbox.a().whileTrue(new ShooterHoodBackward(m_shooter, m_hood)); // emergency use button that should not be pressed in normal circumstances + OI.m_coPilotXbox.b().whileTrue(new OuttakeIndexCommand(m_indexer, m_intake)); // outtaking should not normally be necessary OI.m_coPilotXbox.x().whileTrue(new IntakeIndexCommand(m_indexer, m_intake)); OI.m_coPilotXbox.y().whileTrue(new IndexerUpCommand(m_indexer)); diff --git a/src/main/java/frc/robot/commands/NavXRotationCommand.java b/src/main/java/frc/robot/commands/NavXRotationCommand.java deleted file mode 100644 index eb15fc0..0000000 --- a/src/main/java/frc/robot/commands/NavXRotationCommand.java +++ /dev/null @@ -1,71 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.AutoConstants; -import frc.robot.subsystems.Drivetrain; - -public class NavXRotationCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final Drivetrain m_drivetrain; - private double m_targetDegrees; - private double m_startDegrees; - private double m_thrust; - - /** - * A NavX based turn command. - * Takes in target angle RELATIVE TO FIELD. - * 0 = facing the field from the alliance wall - * Negative values turning left, positive values turning right - * All values between -180 and 180. - */ - public NavXRotationCommand(Drivetrain drivetrain, double targetDegrees) { - m_drivetrain = drivetrain; - m_targetDegrees = targetDegrees; - addRequirements(drivetrain); - } - - @Override - public void initialize() { - // sets initial position to 0 - // m_drivetrain.setRightEncoder(0); - // m_drivetrain.setleftEncoder(0); - m_startDegrees = m_drivetrain.getDegrees(); // value in degrees between -180 and 180 - // m_rightEncoderStart = m_drivetrain.getRightEncoderPosition(); - // m_leftEncoderStart = m_drivetrain.getLeftEncoderPosition(); - - // get appropriate thrust sign - if (m_startDegrees < m_targetDegrees) { - if (m_targetDegrees - m_startDegrees < 180) { - m_thrust = AutoConstants.ROTATION_SPEED; - } else { - m_thrust = -AutoConstants.ROTATION_SPEED; - } - } else { - if (m_startDegrees - m_targetDegrees < 180) { - m_thrust = -AutoConstants.ROTATION_SPEED; - } else { - m_thrust = AutoConstants.ROTATION_SPEED; - } - } - } - - @Override - public void execute() { - System.out.println("Target degrees: " + m_targetDegrees + ", Current degrees: " + m_drivetrain.getDegrees()); - m_drivetrain.tankDrive(m_thrust, -m_thrust); // drives in two opposite directions so the robot spins - // System.out.println("Left encoder position: " + m_drivetrain.getLeftEncoderPosition() + ", Right encoder position: " + m_drivetrain.getRightEncoderPosition()); - } - - @Override - public void end(boolean interrupted) { - m_drivetrain.stopMotors(); - // m_drivetrain.setRightEncoder(0); - // m_drivetrain.setleftEncoder(0); - } - - @Override - public boolean isFinished() { - // calculates if either encoder has moved past the target number of rotations - return Math.abs(m_drivetrain.getDegrees() - m_targetDegrees) < 0.1; - } -} diff --git a/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java b/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java index bb2acb9..6303011 100644 --- a/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java +++ b/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java @@ -20,7 +20,7 @@ public class PidDistanceDriveCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final Drivetrain m_drivetrain; private double m_targetDistance; - private PIDController m_driveController; + private PIDController m_controller; /* * Drives a given distance straight. @@ -32,9 +32,9 @@ public PidDistanceDriveCommand(Drivetrain drivetrain, double targetDistance) { // the encoders measure distance in number of rotations, so we need to convert to that from meters m_targetDistance = targetDistance * DrivetrainConstants.METERS_TO_ROTATIONS; - m_driveController = new PIDController(DrivetrainConstants.drive_P, DrivetrainConstants.drive_I, DrivetrainConstants.drive_D); - m_driveController.disableContinuousInput(); - m_driveController.setTolerance(1.0); // TO DO: tune position tolerance + m_controller = new PIDController(DrivetrainConstants.drive_P, DrivetrainConstants.drive_I, DrivetrainConstants.drive_D); + m_controller.disableContinuousInput(); + m_controller.setTolerance(1.0); // TO DO: tune position tolerance addRequirements(drivetrain); } @@ -46,14 +46,14 @@ public void initialize() { m_drivetrain.setRightEncoder(0); m_drivetrain.setleftEncoder(0); - m_driveController.reset(); - m_driveController.setSetpoint(m_targetDistance); + m_controller.reset(); + m_controller.setSetpoint(m_targetDistance); } @Override public void execute() { - // takes average of two encoders ??? to ensure straight-ness? - double thrust = m_driveController.calculate((m_drivetrain.getLeftEncoderPosition() + m_drivetrain.getRightEncoderPosition())/2); + // sets both motors to the thrust calculated using the left encoder values, is this bad? + double thrust = m_controller.calculate(m_drivetrain.getLeftEncoderPosition()); m_drivetrain.tankDrive(thrust, thrust); } @@ -66,6 +66,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return m_driveController.atSetpoint(); + return m_controller.atSetpoint(); } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 6d288f9..fe121f7 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -133,10 +133,6 @@ public Rotation2d getRotation() { return m_gyro.getRotation2d(); } - public double getDegrees() { - return m_gyro.getRotation2d().getDegrees(); - } - // resets robot pose to given pose public void resetPose(Pose2d pose) { m_odometry.resetPosition(m_gyro.getRotation2d(), new DifferentialDriveWheelPositions(getLeftEncoderPosition(), getRightEncoderPosition()), pose); From efdcf9c47be3f9c6292ef04e5c6ff2e7efbbc86f Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Tue, 26 Mar 2024 20:42:02 -0700 Subject: [PATCH 05/11] add velocity pid to distance drive command MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit command is unfinished—doesn't have the motor inputs actually implemented --- src/main/java/frc/robot/Constants.java | 15 +++- .../robot/commands/DistanceDriveCommand.java | 80 ++++++++++++++++++- .../commands/PidDistanceDriveCommand.java | 71 ---------------- .../java/frc/robot/subsystems/Drivetrain.java | 5 +- 4 files changed, 93 insertions(+), 78 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/PidDistanceDriveCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 36e72a6..9083d67 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -37,9 +37,16 @@ public static class DrivetrainConstants { public static final double DEGREES_TO_ROTATIONS = 0.1; // value accurate as of 2/13/24 public static final double ENCODER_TICKS_TO_METERS = 0.0013; // calculated based on METERS_TO_ROTATIONS and COUNTS_PER_REV - public static final double drive_P = 0; // TEST - public static final double drive_I = 0; // TEST - public static final double drive_D = 0; // TEST + // PID testing: https://www.chiefdelphi.com/t/how-do-you-tune-your-pid-gains/367425/8 + public static final double K_P = 0; // TO DO: TEST + public static final double K_I = 0; // TO DO: TEST + public static final double K_D = 0; // TO DO: TEST + public static final double K_MAX_OUTPUT = 1; + public static final double K_MIN_OUTPUT = -1; + public static final double MAX_RPM = 5676; + public static final double K_IZ = 0; // tune + public static final double K_FF = 0; // tune + } public static class IntakeConstants { @@ -122,5 +129,7 @@ public static class AutoConstants { public static final double TAXI_AUTO_SPEED = 0.5; // (temp) speed of robot during taxi auto public static final double ROTATION_SPEED = 0.5; + + public static final double DEFAULT_TARGET_VELOCITY = 100; // FIX THIS !!!!!! (in m/s) } } diff --git a/src/main/java/frc/robot/commands/DistanceDriveCommand.java b/src/main/java/frc/robot/commands/DistanceDriveCommand.java index 7a78d7f..e51d114 100644 --- a/src/main/java/frc/robot/commands/DistanceDriveCommand.java +++ b/src/main/java/frc/robot/commands/DistanceDriveCommand.java @@ -7,6 +7,11 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivetrainConstants; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkPIDController; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + //import java.lang.Math.*; import edu.wpi.first.wpilibj2.command.Command; @@ -17,15 +22,40 @@ public class DistanceDriveCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final Drivetrain m_drivetrain; private double m_targetDistance; + private double m_targetVelocity; + private SparkPIDController m_leftPIDController; + private SparkPIDController m_rightPIDController; /** * A dead reckoning drive command. * Takes in target distance in meters and drives straight that amount. + * Uses default target velocity of ??? m/s. */ public DistanceDriveCommand(Drivetrain drivetrain, double targetDistance) { m_drivetrain = drivetrain; m_targetDistance = targetDistance * DrivetrainConstants.METERS_TO_ROTATIONS; // convert meters to motor rotations // System.out.println("Target meters: " + targetDistance + ", target rotations: " + m_targetDistance); + m_targetVelocity = AutoConstants.DEFAULT_TARGET_VELOCITY * DrivetrainConstants.METERS_TO_ROTATIONS * 60; + + m_leftPIDController = m_drivetrain.m_leftPrimary.getPIDController(); + m_rightPIDController = m_drivetrain.m_rightPrimary.getPIDController(); + + addRequirements(drivetrain); + } + + /** + * A dead reckoning drive command. + * Takes in target distance in meters and drives straight that amount. + * Takes in target velocity in meters per second. + */ + public DistanceDriveCommand(Drivetrain drivetrain, double targetDistance, double targetVelocity) { + m_drivetrain = drivetrain; + m_targetDistance = targetDistance * DrivetrainConstants.METERS_TO_ROTATIONS; // convert meters to motor rotations + // System.out.println("Target meters: " + targetDistance + ", target rotations: " + m_targetDistance); + m_targetVelocity = targetVelocity * DrivetrainConstants.METERS_TO_ROTATIONS * 60; // convert meters/sec to RPM + + m_leftPIDController = m_drivetrain.m_leftPrimary.getPIDController(); + m_rightPIDController = m_drivetrain.m_rightPrimary.getPIDController(); addRequirements(drivetrain); } @@ -35,13 +65,57 @@ public void initialize() { // resets positions of encoders m_drivetrain.setRightEncoder(0); m_drivetrain.setleftEncoder(0); + + // // configure PID controllers (with constants) + // m_leftPIDController.setP(DrivetrainConstants.K_P); + // m_leftPIDController.setI(DrivetrainConstants.K_I); + // m_leftPIDController.setD(DrivetrainConstants.K_D); + // m_leftPIDController.setIZone(DrivetrainConstants.K_IZ); + // m_leftPIDController.setFF(DrivetrainConstants.K_FF); + // m_leftPIDController.setOutputRange(DrivetrainConstants.K_MIN_OUTPUT, DrivetrainConstants.K_MAX_OUTPUT); + + // m_rightPIDController.setP(DrivetrainConstants.K_P); + // m_rightPIDController.setI(DrivetrainConstants.K_I); + // m_rightPIDController.setD(DrivetrainConstants.K_D); + // m_rightPIDController.setIZone(DrivetrainConstants.K_IZ); + // m_rightPIDController.setFF(DrivetrainConstants.K_FF); + // m_rightPIDController.setOutputRange(DrivetrainConstants.K_MIN_OUTPUT, DrivetrainConstants.K_MAX_OUTPUT); + + // read PID coefficients from SmartDashboard + double p = SmartDashboard.getNumber("P Gain", 0); + double i = SmartDashboard.getNumber("I Gain", 0); + double d = SmartDashboard.getNumber("D Gain", 0); + double iz = SmartDashboard.getNumber("I Zone", 0); + double ff = SmartDashboard.getNumber("Feed Forward", 0); + + m_leftPIDController.setP(p); + m_leftPIDController.setI(i); + m_leftPIDController.setD(d); + m_leftPIDController.setIZone(iz); + m_leftPIDController.setFF(ff); + m_leftPIDController.setOutputRange(DrivetrainConstants.K_MIN_OUTPUT, DrivetrainConstants.K_MAX_OUTPUT); + + m_rightPIDController.setP(p); + m_rightPIDController.setI(i); + m_rightPIDController.setD(d); + m_rightPIDController.setIZone(iz); + m_rightPIDController.setFF(ff); + m_rightPIDController.setOutputRange(DrivetrainConstants.K_MIN_OUTPUT, DrivetrainConstants.K_MAX_OUTPUT); + } @Override public void execute() { - double thrust = AutoConstants.TAXI_AUTO_SPEED*Math.signum(m_targetDistance); // drives in the direction of targetDistance - m_drivetrain.tankDrive(-thrust, -thrust); - // System.out.println("Left encoder position: " + m_drivetrain.getLeftEncoderPosition() + ", Right encoder position: " + m_drivetrain.getRightEncoderPosition()); + // OLD CODE + // double thrust = AutoConstants.TAXI_AUTO_SPEED*Math.signum(m_targetDistance); // drives in the direction of targetDistance + // m_drivetrain.tankDrive(-thrust, -thrust); + + // we are unsure if this actually sends power to the motors...? + m_leftPIDController.setReference(m_targetVelocity, CANSparkMax.ControlType.kVelocity); + m_rightPIDController.setReference(m_targetVelocity, CANSparkMax.ControlType.kVelocity); + + System.out.println("Encoder velocities in RPM: " + m_drivetrain.getLeftEncoderVelocity() + ", " + m_drivetrain.getRightEncoderVelocity()); + } @Override diff --git a/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java b/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java deleted file mode 100644 index 6303011..0000000 --- a/src/main/java/frc/robot/commands/PidDistanceDriveCommand.java +++ /dev/null @@ -1,71 +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 frc.robot.subsystems.Drivetrain; -// import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DrivetrainConstants; - -//import java.lang.Math.*; - -import edu.wpi.first.wpilibj2.command.Command; - -import edu.wpi.first.math.controller.PIDController; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -/** A PID controlled taxi command that uses the drivetrain subsystem, UNTESTED DO NOT USE. */ -public class PidDistanceDriveCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final Drivetrain m_drivetrain; - private double m_targetDistance; - private PIDController m_controller; - - /* - * Drives a given distance straight. - * Takes in targetDistance in meters (can be positive or negative) - */ - public PidDistanceDriveCommand(Drivetrain drivetrain, double targetDistance) { - - m_drivetrain = drivetrain; - // the encoders measure distance in number of rotations, so we need to convert to that from meters - m_targetDistance = targetDistance * DrivetrainConstants.METERS_TO_ROTATIONS; - - m_controller = new PIDController(DrivetrainConstants.drive_P, DrivetrainConstants.drive_I, DrivetrainConstants.drive_D); - m_controller.disableContinuousInput(); - m_controller.setTolerance(1.0); // TO DO: tune position tolerance - - addRequirements(drivetrain); - } - - @Override - public void initialize() { - - // reset positions of encoders - m_drivetrain.setRightEncoder(0); - m_drivetrain.setleftEncoder(0); - - m_controller.reset(); - m_controller.setSetpoint(m_targetDistance); - } - - @Override - public void execute() { - // sets both motors to the thrust calculated using the left encoder values, is this bad? - double thrust = m_controller.calculate(m_drivetrain.getLeftEncoderPosition()); - m_drivetrain.tankDrive(thrust, thrust); - } - - @Override - public void end(boolean interrupted) { - m_drivetrain.stopMotors(); - m_drivetrain.setRightEncoder(0); - m_drivetrain.setleftEncoder(0); - } - - @Override - public boolean isFinished() { - return m_controller.atSetpoint(); - } -} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index fe121f7..9f3ee82 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -10,6 +10,7 @@ import frc.robot.OI; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.SparkRelativeEncoder; @@ -35,7 +36,7 @@ public class Drivetrain extends SubsystemBase { MotorType.kBrushless); private final CANSparkMax m_leftSecondary = new CANSparkMax(DrivetrainConstants.LEFT_MOTOR_2_PORT, MotorType.kBrushless); - private final CANSparkMax m_rightPrimary = new CANSparkMax(DrivetrainConstants.RIGHT_MOTOR_1_PORT, + public final CANSparkMax m_rightPrimary = new CANSparkMax(DrivetrainConstants.RIGHT_MOTOR_1_PORT, MotorType.kBrushless); private final CANSparkMax m_rightSecondary = new CANSparkMax(DrivetrainConstants.RIGHT_MOTOR_2_PORT, MotorType.kBrushless); @@ -55,6 +56,8 @@ public class Drivetrain extends SubsystemBase { // private final DifferentialDriveKinematics m_kinematics = new DifferentialDriveKinematics(DrivetrainConstants.TRACK_WIDTH); public static AHRS m_gyro = new AHRS(Port.kMXP); + public SparkPIDController m_pidController; + public DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry( m_gyro.getRotation2d(), getLeftEncoderPosition(), getRightEncoderPosition(), From e81a5a7ac4e01ff9b7ad0320ab088fcfd9cd8cd8 Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Thu, 28 Mar 2024 18:38:03 -0700 Subject: [PATCH 06/11] write navx based rotation command --- .../robot/commands/NavXRotationCommand.java | 66 +++++++++++++++++++ .../java/frc/robot/subsystems/Drivetrain.java | 8 ++- 2 files changed, 71 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/commands/NavXRotationCommand.java diff --git a/src/main/java/frc/robot/commands/NavXRotationCommand.java b/src/main/java/frc/robot/commands/NavXRotationCommand.java new file mode 100644 index 0000000..73a40df --- /dev/null +++ b/src/main/java/frc/robot/commands/NavXRotationCommand.java @@ -0,0 +1,66 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.DrivetrainConstants; +import frc.robot.subsystems.Drivetrain; + +import edu.wpi.first.math.MathUtil; + +public class NavXRotationCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Drivetrain m_drivetrain; + // private double m_rightEncoderStart; + // private double m_leftEncoderStart; + private double m_targetDegrees; + private int m_direction; + + /** + * A dead reckoning turn command. + * Takes in target angle in degrees (between -180 and 180) and turns to that angle relative to the field. + */ + public NavXRotationCommand(Drivetrain drivetrain, double targetDegrees) { + // System.out.println("targetDistance: " + targetDegrees); + m_drivetrain = drivetrain; + m_targetDegrees = MathUtil.inputModulus(targetDegrees, -180, 180); // make sure angle is correct format + + addRequirements(drivetrain); + + // find direction of rotation + double currentDegrees = m_drivetrain.getRotation(); + if (currentDegrees >= 0) { + if (currentDegrees > targetDegrees && targetDegrees >= currentDegrees - 180) { + m_direction = -1; + } else { + m_direction = 1; + } + } else { + if (targetDegrees > currentDegrees && targetDegrees <= currentDegrees + 180) { + m_direction = 1; + } else { + m_direction = -1; + } + } + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + double thrust = AutoConstants.ROTATION_SPEED * m_direction; // takes the sign of targetDegrees + m_drivetrain.tankDrive(thrust, -thrust); // drives in two opposite directions so the robot spins + } + + @Override + public void end(boolean interrupted) { + m_drivetrain.stopMotors(); + } + + @Override + public boolean isFinished() { + // returns if the current degrees is within a certain range of target degrees + return (Math.abs(m_drivetrain.getRotation() - m_targetDegrees) < 0.5); + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 9f3ee82..df36b7a 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -16,6 +16,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.MathUtil; + import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; @@ -131,9 +133,9 @@ public Pose2d getPose() { return m_odometry.getPoseMeters(); } - // returns current robot rotation - public Rotation2d getRotation() { - return m_gyro.getRotation2d(); + // returns current robot rotation in degrees relative to the field, from -180 to 180 + public double getRotation() { + return MathUtil.inputModulus(m_gyro.getRotation2d().getDegrees(), -180, 180); } // resets robot pose to given pose From 35f2c6e245509b212ea20f6f72f38e72912eaf29 Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Thu, 28 Mar 2024 18:57:25 -0700 Subject: [PATCH 07/11] write initialize angle command for beginning of auto --- .../frc/robot/commands/InitializeNavX.java | 42 +++++++++++++++++++ .../java/frc/robot/subsystems/Drivetrain.java | 7 +++- 2 files changed, 48 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/InitializeNavX.java diff --git a/src/main/java/frc/robot/commands/InitializeNavX.java b/src/main/java/frc/robot/commands/InitializeNavX.java new file mode 100644 index 0000000..24c2a6e --- /dev/null +++ b/src/main/java/frc/robot/commands/InitializeNavX.java @@ -0,0 +1,42 @@ +// 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.Drivetrain; + +/** + * Initializes the gyro to the current robot angle. + * Angle is measured as follows: + * 0 degrees is when the robot's intake side is facing the field, against OUR alliance wall + * Positive degrees = clockwise turning, negative degrees = counterclockwise + */ +public class InitializeNavX extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Drivetrain m_drivetrain; + private double m_degrees; + + public InitializeNavX(Drivetrain drivetrain, double degrees) { + m_drivetrain = drivetrain; + m_degrees = degrees; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + m_drivetrain.rotationOffset(m_degrees); + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index df36b7a..f2afe87 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -135,7 +135,12 @@ public Pose2d getPose() { // returns current robot rotation in degrees relative to the field, from -180 to 180 public double getRotation() { - return MathUtil.inputModulus(m_gyro.getRotation2d().getDegrees(), -180, 180); + return MathUtil.inputModulus(m_gyro.getAngle(), -180, 180); + } + + // offsets getAngle degrees by given amount, between -180 and 180 + public void rotationOffset(double degrees) { + m_gyro.setAngleAdjustment(MathUtil.inputModulus(degrees, -180, 180)); } // resets robot pose to given pose From d4eaf5bdf7c4b8925189deef158e962bce40383b Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Thu, 28 Mar 2024 19:08:35 -0700 Subject: [PATCH 08/11] change autos to use new rotation --- src/main/java/frc/robot/commands/Autos.java | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index dec724a..0b45e1e 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -7,6 +7,8 @@ import frc.robot.subsystems.*; import frc.robot.Constants.AutoConstants; +import javax.print.attribute.standard.MediaSize.NA; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -57,9 +59,10 @@ public static SequentialCommandGroup shootTaxiFront(Drivetrain drivetrain, Index */ public static SequentialCommandGroup shootTaxiLeft(Drivetrain drivetrain, Indexer indexer, Shooter shooter) { return new SequentialCommandGroup( + new InitializeNavX(drivetrain, 60), new AutoSpeakerShootCommand(indexer, shooter), new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_DIAGONAL_DISTANCE), // drive a little - new RotationCommand(drivetrain, -60), // rotate + new NavXRotationCommand(drivetrain, 0), // rotate new DistanceDriveCommand(drivetrain, AutoConstants.TAXI_DISTANCE) // taxi forwards ); } @@ -76,9 +79,10 @@ public static SequentialCommandGroup shootTaxiLeft(Drivetrain drivetrain, Indexe */ public static SequentialCommandGroup shootTaxiRight(Drivetrain drivetrain, Indexer indexer, Shooter shooter) { return new SequentialCommandGroup( + new InitializeNavX(drivetrain, -60), new AutoSpeakerShootCommand(indexer, shooter), new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_DIAGONAL_DISTANCE), // drive a little - new RotationCommand(drivetrain, 60), // rotate + new RotationCommand(drivetrain, 0), // rotate new DistanceDriveCommand(drivetrain, AutoConstants.TAXI_DISTANCE) // taxi forwards ); } @@ -120,9 +124,10 @@ public static SequentialCommandGroup frontSpeaker2pc(Drivetrain drivetrain, Inta */ public static SequentialCommandGroup leftSpeaker2pc(Drivetrain drivetrain, Intake intake, Indexer indexer, Shooter shooter) { return new SequentialCommandGroup( + new InitializeNavX(drivetrain, 60), new AutoSpeakerShootCommand(indexer, shooter), // shoot preloaded new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_DIAGONAL_DISTANCE), // 0.33m gets center robot in line with note - new RotationCommand(drivetrain, -60), // turns robot to face note + new RotationCommand(drivetrain, 0), // turns robot to face note new ParallelCommandGroup( // drives towards note while intaking for 4 seconds new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_HORIZONTAL_DISTANCE), // 1.75 is the distance needed to get to the note new IntakeIndexCommand(indexer, intake).withTimeout(AutoConstants.CLOSE_INTAKE_TIMEOUT) @@ -148,9 +153,10 @@ public static SequentialCommandGroup leftSpeaker2pc(Drivetrain drivetrain, Intak */ public static SequentialCommandGroup rightSpeaker2pc(Drivetrain drivetrain, Intake intake, Indexer indexer, Shooter shooter) { return new SequentialCommandGroup( + new InitializeNavX(drivetrain, -60), new AutoSpeakerShootCommand(indexer, shooter), // shoot preloaded new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_DIAGONAL_DISTANCE), // 0.33m gets center robot in line with note - new RotationCommand(drivetrain, 60), // turns robot to face note + new RotationCommand(drivetrain, 0), // turns robot to face note new ParallelCommandGroup( // drives towards note while intaking for 4 seconds new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_HORIZONTAL_DISTANCE), new IntakeIndexCommand(indexer, intake).withTimeout(AutoConstants.CLOSE_INTAKE_TIMEOUT) @@ -175,8 +181,10 @@ public static SequentialCommandGroup rightSpeaker2pc(Drivetrain drivetrain, Inta */ public static SequentialCommandGroup farIntakeLeftRed(Drivetrain drivetrain, Intake intake, Indexer indexer, Shooter shooter) { return new SequentialCommandGroup( + new InitializeNavX(drivetrain, 60), + new AutoSpeakerShootCommand(indexer, shooter), new DistanceDriveCommand(drivetrain, AutoConstants.FAR_DIAGONAL_DISTANCE), // drive a little - new RotationCommand(drivetrain, -60), // rotate + new RotationCommand(drivetrain, 0), // rotate new ParallelCommandGroup( // drives towards note while intaking for 4 seconds new DistanceDriveCommand(drivetrain, AutoConstants.FAR_HORIZONTAL_DISTANCE), new IntakeIndexCommand(indexer, intake).withTimeout(AutoConstants.FAR_INTAKE_TIMEOUT) @@ -201,8 +209,9 @@ public static SequentialCommandGroup farIntakeLeftRed(Drivetrain drivetrain, Int */ public static SequentialCommandGroup farIntakeRightBlue(Drivetrain drivetrain, Intake intake, Indexer indexer, Shooter shooter) { return new SequentialCommandGroup( + new InitializeNavX(drivetrain, -60), new DistanceDriveCommand(drivetrain, AutoConstants.FAR_DIAGONAL_DISTANCE), // drive a little - new RotationCommand(drivetrain, 60), // rotate + new RotationCommand(drivetrain, 0), // rotate new ParallelCommandGroup( // drives towards note while intaking for 4 seconds new DistanceDriveCommand(drivetrain, AutoConstants.FAR_HORIZONTAL_DISTANCE), new IntakeIndexCommand(indexer, intake).withTimeout(AutoConstants.FAR_INTAKE_TIMEOUT) From 0479b926f8355a8c7ffcb33f21d44f960582fb65 Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Fri, 29 Mar 2024 20:38:52 -0700 Subject: [PATCH 09/11] add (unused) pid and tune isfinished --- src/main/java/frc/robot/Constants.java | 4 ++++ src/main/java/frc/robot/commands/NavXRotationCommand.java | 7 ++++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9083d67..e65a181 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,6 +47,10 @@ public static class DrivetrainConstants { public static final double K_IZ = 0; // tune public static final double K_FF = 0; // tune + public static final double ROTATION_KP = 0; + public static final double ROTATION_KI = 0; + public static final double ROTATION_KD = 0; + } public static class IntakeConstants { diff --git a/src/main/java/frc/robot/commands/NavXRotationCommand.java b/src/main/java/frc/robot/commands/NavXRotationCommand.java index 73a40df..6015877 100644 --- a/src/main/java/frc/robot/commands/NavXRotationCommand.java +++ b/src/main/java/frc/robot/commands/NavXRotationCommand.java @@ -5,6 +5,8 @@ import frc.robot.Constants.DrivetrainConstants; import frc.robot.subsystems.Drivetrain; +import edu.wpi.first.math.controller.PIDController; + import edu.wpi.first.math.MathUtil; public class NavXRotationCommand extends Command { @@ -14,6 +16,7 @@ public class NavXRotationCommand extends Command { // private double m_leftEncoderStart; private double m_targetDegrees; private int m_direction; + private PIDController m_controller; /** * A dead reckoning turn command. @@ -24,6 +27,8 @@ public NavXRotationCommand(Drivetrain drivetrain, double targetDegrees) { m_drivetrain = drivetrain; m_targetDegrees = MathUtil.inputModulus(targetDegrees, -180, 180); // make sure angle is correct format + // m_controller = new PIDController(DrivetrainConstants.ROTATION_KP, DrivetrainConstants.ROTATION_KI, DrivetrainConstants.ROTATION_KD); + addRequirements(drivetrain); // find direction of rotation @@ -61,6 +66,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { // returns if the current degrees is within a certain range of target degrees - return (Math.abs(m_drivetrain.getRotation() - m_targetDegrees) < 0.5); + return (Math.abs(m_drivetrain.getRotation() - m_targetDegrees) < 5); } } From 25d27c979ecb03386282e4326ccdcce6d746f228 Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Fri, 29 Mar 2024 20:39:29 -0700 Subject: [PATCH 10/11] testing from pid (it "works" but kinda badly) --- src/main/java/frc/robot/Constants.java | 4 +-- src/main/java/frc/robot/RobotContainer.java | 7 ++--- .../robot/commands/DistanceDriveCommand.java | 26 ++++++++++++++----- .../java/frc/robot/subsystems/Drivetrain.java | 5 ++++ 4 files changed, 30 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e65a181..02bb57f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -120,7 +120,7 @@ public static class AutoConstants { public static final double CLOSE_INTAKE_TIMEOUT = 5; // in seconds public static final double FAR_INTAKE_TIMEOUT = 10; // in seconds (untested for 2024) FIX!!!!!! - public static final double TAXI_DISTANCE = 2; // distance in meters to cross taxi line (untested for 2024) + public static final double TAXI_DISTANCE = 1; // distance in meters to cross taxi line (untested for 2024) public static final double FRONT_SPEAKER_TO_CENTER_NOTE = 1.30; // TO DO: test public static final double CLOSE_DIAGONAL_DISTANCE = 0.33; // distance in meters from speaker // side to align with close note (untested for 2024) @@ -134,6 +134,6 @@ public static class AutoConstants { public static final double TAXI_AUTO_SPEED = 0.5; // (temp) speed of robot during taxi auto public static final double ROTATION_SPEED = 0.5; - public static final double DEFAULT_TARGET_VELOCITY = 100; // FIX THIS !!!!!! (in m/s) + public static final double DEFAULT_TARGET_VELOCITY = -6; // FIX THIS !!!!!! (in m/s) } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 712ac65..b93e155 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,6 +40,7 @@ // auto commands import frc.robot.commands.DistanceDriveCommand; import frc.robot.commands.RotationCommand; +import frc.robot.commands.NavXRotationCommand; // subsystems import frc.robot.subsystems.Drivetrain; @@ -96,12 +97,12 @@ public RobotContainer() { // set default commands m_drivetrain.setDefaultCommand(new ArcadeDriveCommand(m_drivetrain, m_humanControl)); - m_hood.setDefaultCommand(new PrintHoodEncoder(m_hood)); + // m_hood.setDefaultCommand(new PrintHoodEncoder(m_hood)); // m_indexer.setDefaultCommand(new BeamBreakCommand(m_indexer)); // for testing // m_climber.setDefaultCommand(new PrintClimberEncoder(m_climber)); - m_chooser.addOption("Drive 2 meters (testing)", new DistanceDriveCommand(m_drivetrain, 2)); - m_chooser.addOption("Rotate 90 degrees (testing)", new RotationCommand(m_drivetrain, 90)); + m_chooser.addOption("Drive 3 meter (testing)", new DistanceDriveCommand(m_drivetrain, 3)); + m_chooser.addOption("Rotate 90 degrees (testing)", new NavXRotationCommand(m_drivetrain, 90)); m_chooser.addOption("Taxi forward", Autos.taxiAuto(m_drivetrain)); m_chooser.addOption("Shoot and taxi from front", Autos.shootTaxiFront(m_drivetrain, m_indexer, m_shooter)); diff --git a/src/main/java/frc/robot/commands/DistanceDriveCommand.java b/src/main/java/frc/robot/commands/DistanceDriveCommand.java index e51d114..9fedf93 100644 --- a/src/main/java/frc/robot/commands/DistanceDriveCommand.java +++ b/src/main/java/frc/robot/commands/DistanceDriveCommand.java @@ -82,12 +82,24 @@ public void initialize() { // m_rightPIDController.setOutputRange(DrivetrainConstants.K_MIN_OUTPUT, DrivetrainConstants.K_MAX_OUTPUT); // read PID coefficients from SmartDashboard - double p = SmartDashboard.getNumber("P Gain", 0); - double i = SmartDashboard.getNumber("I Gain", 0); - double d = SmartDashboard.getNumber("D Gain", 0); - double iz = SmartDashboard.getNumber("I Zone", 0); - double ff = SmartDashboard.getNumber("Feed Forward", 0); + // SmartDashboard.putNumber("P Gain", 0); + // SmartDashboard.putNumber("I Gain", 0); + // SmartDashboard.putNumber("D Gain", 0); + // SmartDashboard.putNumber("I Zone", 0); + // SmartDashboard.putNumber("Feed Forward", 0); + + // // double p = SmartDashboard.getNumber("P Gain", 0); + // // double i = SmartDashboard.getNumber("I Gain", 0); + // double d = SmartDashboard.getNumber("D Gain", 0); + // double iz = SmartDashboard.getNumber("I Zone", 0); + // double ff = SmartDashboard.getNumber("Feed Forward", 0); + double p = 0.000003; //0.0003 + double i = 0.000000001; + double d = 0; + double iz = 0; + double ff = 0; + m_leftPIDController.setP(p); m_leftPIDController.setI(i); m_leftPIDController.setD(d); @@ -110,11 +122,11 @@ public void execute() { // double thrust = AutoConstants.TAXI_AUTO_SPEED*Math.signum(m_targetDistance); // drives in the direction of targetDistance // m_drivetrain.tankDrive(-thrust, -thrust); - // we are unsure if this actually sends power to the motors...? m_leftPIDController.setReference(m_targetVelocity, CANSparkMax.ControlType.kVelocity); m_rightPIDController.setReference(m_targetVelocity, CANSparkMax.ControlType.kVelocity); - System.out.println("Encoder velocities in RPM: " + m_drivetrain.getLeftEncoderVelocity() + ", " + m_drivetrain.getRightEncoderVelocity()); + System.out.println("Encoder velocities in m/s: " + m_drivetrain.getLeftEncoderVelocity() / (DrivetrainConstants.METERS_TO_ROTATIONS * 60) + + ", " + m_drivetrain.getRightEncoderVelocity() / (DrivetrainConstants.METERS_TO_ROTATIONS * 60)); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index f2afe87..618f9ad 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -78,6 +78,7 @@ public void periodic() { m_odometry.update(m_gyro.getRotation2d(), DrivetrainConstants.ENCODER_TICKS_TO_METERS * getLeftEncoderPosition(), DrivetrainConstants.ENCODER_TICKS_TO_METERS * getRightEncoderPosition()); + printRotation(); } public void configDrivetrainMotors() { @@ -138,6 +139,10 @@ public double getRotation() { return MathUtil.inputModulus(m_gyro.getAngle(), -180, 180); } + public void printRotation() { + System.out.println(getRotation()); + } + // offsets getAngle degrees by given amount, between -180 and 180 public void rotationOffset(double degrees) { m_gyro.setAngleAdjustment(MathUtil.inputModulus(degrees, -180, 180)); From b7f5032e8213993ecda1a8abbd3ade2c391a7db5 Mon Sep 17 00:00:00 2001 From: amelia-chen Date: Thu, 4 Apr 2024 23:48:49 -0700 Subject: [PATCH 11/11] auto tuning --- src/main/java/frc/robot/Constants.java | 8 ++-- src/main/java/frc/robot/RobotContainer.java | 4 +- src/main/java/frc/robot/commands/Autos.java | 21 +++++---- .../robot/commands/DistanceDriveCommand.java | 43 ++++++++++++------- .../robot/commands/NavXRotationCommand.java | 4 +- 5 files changed, 46 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 68542ef..78c1d00 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,7 +69,7 @@ public static class IntakeConstants { public static class IndexerConstants { public static final int INDEXER_MOTOR_PORT = 6; - public static final double INDEXER_MOTOR_SPEED_DOWN = -0.55; // TO DO: replace with values from beam break branch + public static final double INDEXER_MOTOR_SPEED_DOWN = -0.3; // TO DO: replace with values from beam break branch public static final double INDEXER_MOTOR_SPEED_UP = 0.4; // TO DO: replace with values from beam break branch public static final double INDEXER_MOTOR_SPEED_DOWN_BACKUP = -0.2; // for testing/backup, finalized 02/23/2024 @@ -120,7 +120,7 @@ public static class ClimberConstants { public static class AutoConstants { public static final double SPEAKER_SHOOT_TIMEOUT = 4; // unit: seconds - public static final double CLOSE_INTAKE_TIMEOUT = 5; // in seconds + public static final double CLOSE_INTAKE_TIMEOUT = 2; // in seconds public static final double FAR_INTAKE_TIMEOUT = 10; // in seconds (untested for 2024) FIX!!!!!! public static final double TAXI_DISTANCE = 2; // distance in meters to cross taxi line (untested for 2024) @@ -136,9 +136,9 @@ public static class AutoConstants { public static final double SFR_ELIMS_DISTANCE = 5; // distance in meters to far note // after alignment (untested for 2024) - public static final double TAXI_AUTO_SPEED = 0.6; // (temp) speed of robot during taxi auto + public static final double TAXI_AUTO_SPEED = 0.4; // (temp) speed of robot during taxi auto public static final double ROTATION_SPEED = 0.5; - public static final double DEFAULT_TARGET_VELOCITY = -6; // FIX THIS !!!!!! (in m/s) + public static final double DEFAULT_TARGET_VELOCITY = -2; // FIX THIS !!!!!! (in m/s) } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b0d8ab5..64b62a2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,10 +107,10 @@ public RobotContainer() { // set default commands m_drivetrain.setDefaultCommand(new ArcadeDriveCommand(m_drivetrain, m_humanControl)); // m_hood.setDefaultCommand(new PrintHoodEncoder(m_hood)); - // m_indexer.setDefaultCommand(new BeamBreakCommand(m_indexer)); // for testing + m_indexer.setDefaultCommand(new BeamBreakCommand(m_indexer)); // for testing // m_climber.setDefaultCommand(new PrintClimberEncoder(m_climber)); - m_chooser.addOption("Drive 3 meter (testing)", new DistanceDriveCommand(m_drivetrain, 3)); + m_chooser.addOption("Drive 3 meter (testing)", new DistanceDriveCommand(m_drivetrain, -3)); m_chooser.addOption("Rotate 90 degrees (new/navx rotation)", new NavXRotationCommand(m_drivetrain, 90)); diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 9f75e9d..5b448fd 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -133,15 +133,15 @@ public static SequentialCommandGroup leftSpeaker2pc(Drivetrain drivetrain, Intak return new SequentialCommandGroup( new InitializeNavX(drivetrain, 60), new AutoSpeakerShootCommand(indexer, shooter), // shoot preloaded - new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_DIAGONAL_DISTANCE), // 0.33m gets center robot in line with note - new RotationCommand(drivetrain, 0), // turns robot to face note + new NavXRotationCommand(drivetrain, 0), // turns robot to face note new ParallelCommandGroup( // drives towards note while intaking for 4 seconds new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_HORIZONTAL_DISTANCE), // 1.75 is the distance needed to get to the note new AutoIntakeCommand(indexer, intake).withTimeout(AutoConstants.CLOSE_INTAKE_TIMEOUT) ), - new DistanceDriveCommand(drivetrain, -AutoConstants.CLOSE_HORIZONTAL_DISTANCE), // back up same distance as before + new IndexerDownCommand(indexer).withTimeout(0.05), + new DistanceDriveCommand(drivetrain, -AutoConstants.CLOSE_HORIZONTAL_DISTANCE + 0.1), // back up same distance as before new WaitCommand(1), - new RotationCommand(drivetrain, 60), // turn the other way 60 degrees (facing speaker) + new NavXRotationCommand(drivetrain, 60), // turn the other way 60 degrees (facing speaker) // new DistanceDriveCommand(drivetrain, -AutoConstants.CLOSE_DIAGONAL_DISTANCE), // back into speaker new AutoSpeakerShootCommand(indexer, shooter) // shoot again ); @@ -163,15 +163,14 @@ public static SequentialCommandGroup rightSpeaker2pc(Drivetrain drivetrain, Inta return new SequentialCommandGroup( new InitializeNavX(drivetrain, -60), new AutoSpeakerShootCommand(indexer, shooter), // shoot preloaded - new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_DIAGONAL_DISTANCE), // 0.33m gets center robot in line with note - new RotationCommand(drivetrain, 0), // turns robot to face note + new NavXRotationCommand(drivetrain, 0), // turns robot to face note new ParallelCommandGroup( // drives towards note while intaking for 4 seconds new DistanceDriveCommand(drivetrain, AutoConstants.CLOSE_HORIZONTAL_DISTANCE), new AutoIntakeCommand(indexer, intake).withTimeout(AutoConstants.CLOSE_INTAKE_TIMEOUT) ), new DistanceDriveCommand(drivetrain, -AutoConstants.CLOSE_HORIZONTAL_DISTANCE), // back up same distance as before new WaitCommand(1), - new RotationCommand(drivetrain, -60), // turn the other way 60 degrees (facing speaker) + new NavXRotationCommand(drivetrain, -60), // turn the other way 60 degrees (facing speaker) // new DistanceDriveCommand(drivetrain, -AutoConstants.CLOSE_DIAGONAL_DISTANCE), // back into speaker new AutoSpeakerShootCommand(indexer, shooter) // shoot again ); @@ -193,13 +192,13 @@ public static SequentialCommandGroup farIntakeLeftRed(Drivetrain drivetrain, Int new InitializeNavX(drivetrain, 60), new AutoSpeakerShootCommand(indexer, shooter), new DistanceDriveCommand(drivetrain, AutoConstants.FAR_DIAGONAL_DISTANCE), // drive a little - new RotationCommand(drivetrain, 0), // rotate + new NavXRotationCommand(drivetrain, 0), // rotate new ParallelCommandGroup( // drives towards note while intaking for 4 seconds new DistanceDriveCommand(drivetrain, AutoConstants.FAR_HORIZONTAL_DISTANCE), new AutoIntakeCommand(indexer, intake).withTimeout(AutoConstants.FAR_INTAKE_TIMEOUT) ), new DistanceDriveCommand(drivetrain, -AutoConstants.FAR_HORIZONTAL_DISTANCE), // back up same distance as before - new RotationCommand(drivetrain, 60), // turn the other way 60 degrees (facing speaker) + new NavXRotationCommand(drivetrain, 60), // turn the other way 60 degrees (facing speaker) new DistanceDriveCommand(drivetrain, -AutoConstants.FAR_DIAGONAL_DISTANCE), // back into speaker new AutoSpeakerShootCommand(indexer, shooter) // shoot again ); @@ -220,13 +219,13 @@ public static SequentialCommandGroup farIntakeRightBlue(Drivetrain drivetrain, I return new SequentialCommandGroup( new InitializeNavX(drivetrain, -60), new DistanceDriveCommand(drivetrain, AutoConstants.FAR_DIAGONAL_DISTANCE), // drive a little - new RotationCommand(drivetrain, 0), // rotate + new NavXRotationCommand(drivetrain, 0), // rotate new ParallelCommandGroup( // drives towards note while intaking for 4 seconds new DistanceDriveCommand(drivetrain, AutoConstants.FAR_HORIZONTAL_DISTANCE), new AutoIntakeCommand(indexer, intake).withTimeout(AutoConstants.FAR_INTAKE_TIMEOUT) ), new DistanceDriveCommand(drivetrain, -AutoConstants.FAR_HORIZONTAL_DISTANCE), // back up same distance as before - new RotationCommand(drivetrain, -60), // turn the other way 60 degrees (facing speaker) + new NavXRotationCommand(drivetrain, -60), // turn the other way 60 degrees (facing speaker) new DistanceDriveCommand(drivetrain, -AutoConstants.FAR_DIAGONAL_DISTANCE), // back into speaker new AutoSpeakerShootCommand(indexer, shooter) // shoot again ); diff --git a/src/main/java/frc/robot/commands/DistanceDriveCommand.java b/src/main/java/frc/robot/commands/DistanceDriveCommand.java index 9fedf93..962c5ab 100644 --- a/src/main/java/frc/robot/commands/DistanceDriveCommand.java +++ b/src/main/java/frc/robot/commands/DistanceDriveCommand.java @@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.Timer; + //import java.lang.Math.*; import edu.wpi.first.wpilibj2.command.Command; @@ -26,6 +28,8 @@ public class DistanceDriveCommand extends Command { private SparkPIDController m_leftPIDController; private SparkPIDController m_rightPIDController; + private Timer m_timer; + /** * A dead reckoning drive command. * Takes in target distance in meters and drives straight that amount. @@ -35,24 +39,26 @@ public DistanceDriveCommand(Drivetrain drivetrain, double targetDistance) { m_drivetrain = drivetrain; m_targetDistance = targetDistance * DrivetrainConstants.METERS_TO_ROTATIONS; // convert meters to motor rotations // System.out.println("Target meters: " + targetDistance + ", target rotations: " + m_targetDistance); - m_targetVelocity = AutoConstants.DEFAULT_TARGET_VELOCITY * DrivetrainConstants.METERS_TO_ROTATIONS * 60; + m_targetVelocity = AutoConstants.DEFAULT_TARGET_VELOCITY * DrivetrainConstants.METERS_TO_ROTATIONS * 60 * Math.signum(m_targetDistance); m_leftPIDController = m_drivetrain.m_leftPrimary.getPIDController(); m_rightPIDController = m_drivetrain.m_rightPrimary.getPIDController(); + m_timer = new Timer(); + addRequirements(drivetrain); } /** * A dead reckoning drive command. * Takes in target distance in meters and drives straight that amount. - * Takes in target velocity in meters per second. + * Takes in target speed in meters per second (always positive). */ public DistanceDriveCommand(Drivetrain drivetrain, double targetDistance, double targetVelocity) { m_drivetrain = drivetrain; m_targetDistance = targetDistance * DrivetrainConstants.METERS_TO_ROTATIONS; // convert meters to motor rotations // System.out.println("Target meters: " + targetDistance + ", target rotations: " + m_targetDistance); - m_targetVelocity = targetVelocity * DrivetrainConstants.METERS_TO_ROTATIONS * 60; // convert meters/sec to RPM + m_targetVelocity = targetVelocity * DrivetrainConstants.METERS_TO_ROTATIONS * 60 * Math.signum(m_targetDistance); // convert meters/sec to RPM m_leftPIDController = m_drivetrain.m_leftPrimary.getPIDController(); m_rightPIDController = m_drivetrain.m_rightPrimary.getPIDController(); @@ -66,6 +72,9 @@ public void initialize() { m_drivetrain.setRightEncoder(0); m_drivetrain.setleftEncoder(0); + m_timer.reset(); + m_timer.start(); + // // configure PID controllers (with constants) // m_leftPIDController.setP(DrivetrainConstants.K_P); // m_leftPIDController.setI(DrivetrainConstants.K_I); @@ -94,8 +103,8 @@ public void initialize() { // double iz = SmartDashboard.getNumber("I Zone", 0); // double ff = SmartDashboard.getNumber("Feed Forward", 0); - double p = 0.000003; //0.0003 - double i = 0.000000001; + double p = 0.0004; //0.0003 + double i = 0; double d = 0; double iz = 0; double ff = 0; @@ -118,16 +127,16 @@ public void initialize() { @Override public void execute() { - // OLD CODE - // double thrust = AutoConstants.TAXI_AUTO_SPEED*Math.signum(m_targetDistance); // drives in the direction of targetDistance - // m_drivetrain.tankDrive(-thrust, -thrust); - - m_leftPIDController.setReference(m_targetVelocity, CANSparkMax.ControlType.kVelocity); - m_rightPIDController.setReference(m_targetVelocity, CANSparkMax.ControlType.kVelocity); - - System.out.println("Encoder velocities in m/s: " + m_drivetrain.getLeftEncoderVelocity() / (DrivetrainConstants.METERS_TO_ROTATIONS * 60) - + ", " + m_drivetrain.getRightEncoderVelocity() / (DrivetrainConstants.METERS_TO_ROTATIONS * 60)); - + if (m_timer.get() < 0.5) { + double thrust = AutoConstants.TAXI_AUTO_SPEED*Math.signum(m_targetDistance); // drives in the direction of targetDistance + m_drivetrain.tankDrive(-thrust, -thrust*1.17); + } else { + m_leftPIDController.setReference(m_targetVelocity, CANSparkMax.ControlType.kVelocity); + m_rightPIDController.setReference(m_targetVelocity, CANSparkMax.ControlType.kVelocity); + + System.out.println("Encoder velocities in m/s: " + m_drivetrain.getLeftEncoderVelocity() / (DrivetrainConstants.METERS_TO_ROTATIONS * 60) + + ", " + m_drivetrain.getRightEncoderVelocity() / (DrivetrainConstants.METERS_TO_ROTATIONS * 60)); + } } @Override @@ -135,12 +144,14 @@ public void end(boolean interrupted) { m_drivetrain.stopMotors(); m_drivetrain.setRightEncoder(0); m_drivetrain.setleftEncoder(0); + m_timer.stop(); + m_timer.reset(); } @Override public boolean isFinished() { // calculates if either encoder has moved enough to reach the target distance return (Math.abs(m_drivetrain.getRightEncoderPosition()) > Math.abs(m_targetDistance) - || Math.abs(m_drivetrain.getLeftEncoderPosition()) > Math.abs(m_targetDistance)); + || Math.abs(m_drivetrain.getLeftEncoderPosition()) > Math.abs(m_targetDistance)); } } diff --git a/src/main/java/frc/robot/commands/NavXRotationCommand.java b/src/main/java/frc/robot/commands/NavXRotationCommand.java index 6015877..269d449 100644 --- a/src/main/java/frc/robot/commands/NavXRotationCommand.java +++ b/src/main/java/frc/robot/commands/NavXRotationCommand.java @@ -66,6 +66,8 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { // returns if the current degrees is within a certain range of target degrees - return (Math.abs(m_drivetrain.getRotation() - m_targetDegrees) < 5); + return (Math.abs(m_drivetrain.getRotation() - m_targetDegrees) < 10); } + + }