amelia/pid auto

src/main/java/frc/robot/Constants.java

@@ -38,7 +38,23 @@ 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.065; // value accurate as of 3/9/24
+    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
+
+    // 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 final double ROTATION_KP = 0;
+    public static final double ROTATION_KI = 0;
+    public static final double ROTATION_KD = 0;
+
   }
 
   public static class IntakeConstants {
@@ -53,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
@@ -104,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)
@@ -120,7 +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 = -2; // FIX THIS !!!!!! (in m/s)
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -41,6 +41,8 @@
 // auto commands
 import frc.robot.commands.DistanceDriveCommand;
 import frc.robot.commands.RotationCommand;
+import frc.robot.commands.NavXRotationCommand;
+
 // subsystems
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.ExampleSubsystem;
@@ -105,14 +107,17 @@ 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("Rotate 90 degrees (new/navx rotation)", new NavXRotationCommand(m_drivetrain, 90));
+
+
     m_chooser.addOption("SFR ELIMS BLUE", Autos.elimsBlue(m_drivetrain, m_indexer, m_shooter));
     m_chooser.addOption("SFR ELIMS RED", Autos.elimsRed(m_drivetrain, m_indexer, m_shooter));
 
-    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("Rotate 90 degrees (old rotation)", new RotationCommand(m_drivetrain, 90));
 
     m_chooser.addOption("Just shoot", Autos.justShoot(m_indexer, m_shooter));
 

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;
@@ -64,9 +66,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, -40), // rotate
+      new NavXRotationCommand(drivetrain, 0), // rotate
       new DistanceDriveCommand(drivetrain, AutoConstants.TAXI_DISTANCE) // taxi forwards
     );
   }
@@ -83,9 +86,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, 40), // rotate
+      new RotationCommand(drivetrain, 0), // rotate
       new DistanceDriveCommand(drivetrain, AutoConstants.TAXI_DISTANCE) // taxi forwards
     );
   }
@@ -127,16 +131,17 @@ 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.3m gets center robot in line with note
-      new RotationCommand(drivetrain, -60), // 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
     );
@@ -156,16 +161,16 @@ 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 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
     );
@@ -184,14 +189,16 @@ 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 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
     );
@@ -210,14 +217,15 @@ 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 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
     );

src/main/java/frc/robot/commands/DistanceDriveCommand.java

@@ -7,6 +7,13 @@
 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 edu.wpi.first.wpilibj.Timer;
+
 //import java.lang.Math.*;
 
 import edu.wpi.first.wpilibj2.command.Command;
@@ -17,15 +24,44 @@ 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;
+
+  private Timer m_timer;
 
   /** 
    * 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 * 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 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 * Math.signum(m_targetDistance); // convert meters/sec to RPM
+
+    m_leftPIDController = m_drivetrain.m_leftPrimary.getPIDController();
+    m_rightPIDController = m_drivetrain.m_rightPrimary.getPIDController();
 
     addRequirements(drivetrain);
   }
@@ -35,26 +71,87 @@ public void initialize() {
     // resets positions of encoders
     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);
+    // 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
+    // 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.0004; //0.0003
+    double i = 0;
+    double d = 0;
+    double iz = 0;
+    double ff = 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());
+    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
   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));
   }
 }

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;
+  }
+}