Add a lot of auton stuff

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

@@ -8,6 +8,12 @@ public class Constants {
     public static final int DRIVE_RIGHT_PARENT_ID = 11;
     public static final int DRIVE_RIGHT_CHILD_ID = 3;
 
+    /*SHOOTER CONFIG*/
+    public static final double REV_TIME = 1.0;
+
+    /*DRIVE TRAIN CONFIG */
+    public static final double CONTROLLER_DEADZONE = 0.06;
+
     /*CONTROLLER PORT IDs*/
     public static final int DRIVER_CONTROLLER_ID = 0;
 
@@ -21,10 +27,13 @@ public class Constants {
 
     /*PID GAINS *///0.00001, 100
     public static final Gains ROTATION_GAINS = new Gains(0.011, 0.0001, 0.000935, 0, 0, 0.3);
-    public static final Gains NORMAL_ROBOT_GAINS = new Gains(0.75, 0, 0, 0, 0, 1);
+    public static final Gains NORMAL_ROBOT_GAINS = new Gains(0.0005, 0.0, 0, 0, 0, 1.);
 
     /*PHYSICAL ROBOT CONSTANTS */
-    public static final double WHEEL_CIRCUMFERENCE_INCHES = 0;
+    public static final double WHEEL_CIRCUMFERENCE_INCHES = 8 * Math.PI;
+
+    //8.46
+    public static final double DRIVE_GEARBOX_RATIO = 10.7;
 
     /* Jack's helper class that should probably have been retired */
     /** Which PID slot to pull gains from */

src/main/java/frc/robot/Robot.java

@@ -49,21 +49,26 @@ public static AHRS getGyroscope() {
      */
     @Override
     public void robotInit() {
-        aprilTagHighlighter = new AprilTagHighlighter();
+        initializeSmartMotion(driveLeftParent, Constants.NORMAL_ROBOT_GAINS);
+        initializeSmartMotion(driveRightParent, Constants.NORMAL_ROBOT_GAINS);
+
+        // aprilTagHighlighter = new AprilTagHighlighter();
         autonRouteChooser.addOption("move forward", "move forward");
         autonRouteChooser.addOption("move backwards", "move backwards");
         SmartDashboard.putData(autonRouteChooser);
 
+        driveLeftChild.follow(driveLeftParent);
+        driveRightChild.follow(driveRightParent);
+
         driveLeftParent.setInverted(true);
         driveRightParent.setInverted(false);
 
-        driveLeftChild.follow(driveLeftParent);
-        driveRightChild.follow(driveRightParent);
+        System.out.println("is drive righjt parent inverted? " + driveRightParent.getInverted());
 
-        driveLeftChild.setBrakeMode(true);
-        driveLeftParent.setBrakeMode(true);
-        driveRightChild.setBrakeMode(true);
-        driveRightParent.setBrakeMode(true);
+        driveLeftChild.setBrakeMode(false);
+        driveLeftParent.setBrakeMode(false);
+        driveRightChild.setBrakeMode(false);
+        driveRightParent.setBrakeMode(false);
 
         QuickActions.setDriveMotors(driveLeftParent, driveRightParent);
         getGyroscope().reset();
@@ -74,8 +79,7 @@ public void robotInit() {
 
         SmartDashboard.putNumber("PIDTARGET", 90);
 
-        initializeSmartMotion(driveLeftParent, Constants.NORMAL_ROBOT_GAINS);
-        initializeSmartMotion(driveRightParent, Constants.NORMAL_ROBOT_GAINS);
+        System.out.println("is drive righjt parent inverted at end?? " + driveRightParent.getInverted());
     }
 
     /**
@@ -87,13 +91,15 @@ public void robotInit() {
      */
     @Override
     public void robotPeriodic() {
-        aprilTagHighlighter.doEveryFrame();
+        // aprilTagHighlighter.doEveryFrame();
         SmartDashboard.putNumber("Gyro Reading", getGyroscope().getAngle());
-        SmartDashboard.putNumber("Left motor controller encoder", driveLeftParent.getEncoderPosition());
-        SmartDashboard.putNumber("right motor controller encoder", driveRightParent.getEncoderPosition());
-        Constants.ROTATION_GAINS.P = SmartDashboard.getNumber("rotationGainsP", kDefaultPeriod);
-        Constants.ROTATION_GAINS.I = SmartDashboard.getNumber("rotationGainsI", kDefaultPeriod);
-        Constants.ROTATION_GAINS.D = SmartDashboard.getNumber("rotationGainsD", kDefaultPeriod);
+        SmartDashboard.putNumber("left encoder", driveLeftParent.getEncoderPosition());
+        SmartDashboard.putNumber("right encoder", driveRightParent.getEncoderPosition());
+        // SmartDashboard.putNumber("Left motor controller encoder", driveLeftParent.getEncoderPosition());
+        // SmartDashboard.putNumber("right motor controller encoder", driveRightParent.getEncoderPosition());
+        // Constants.ROTATION_GAINS.P = SmartDashboard.getNumber("rotationGainsP", kDefaultPeriod);
+        // Constants.ROTATION_GAINS.I = SmartDashboard.getNumber("rotationGainsI", kDefaultPeriod);
+        // Constants.ROTATION_GAINS.D = SmartDashboard.getNumber("rotationGainsD", kDefaultPeriod);
     }
 
     /**
@@ -110,28 +116,16 @@ public void robotPeriodic() {
     public void autonomousInit() {
         navX.resetDisplacement();
         navX.reset();
-        QuickActions.setAll(0.3);
 
         ArrayDeque<AutonAction> route =
             switch (autonRouteChooser.getSelected()) {
-                case "move forward" -> AutonRoutes.RUN_INTO_WALL_AND_BREAK_ROBOT;
-                case "move backwards" -> AutonRoutes.RUN_INTO_OTHER_WALL_AND_BREAK_ROBOT_AGAIN;
+                case "move forward" -> new ArrayDeque<>(AutonRoutes.RUN_INTO_WALL_AND_BREAK_ROBOT);
+                case "move backwards" -> new ArrayDeque<>(AutonRoutes.RUN_INTO_OTHER_WALL_AND_BREAK_ROBOT_AGAIN);
                 default -> new ArrayDeque<AutonAction>();
             };
+        System.out.println("selected auton: " + route);
         auton = new AutonActionRunner(route);
         auton.initiateAuton();
-        // QuickActions.turn(TurnDirection.RIGHT, 0.3);
-
-        // Thread auto = new Thread(() -> {
-        //     try {
-        //         QuickActions.setAll(-0.3);
-        //         Thread.sleep(2000);
-        //         QuickActions.turn(TurnDirection.RIGHT, 0.3);
-        //     } catch (InterruptedException e) {
-        //         e.printStackTrace();
-        //     }
-        // });
-        // auto.start();
     }
 
     /**Things Auton needs to do:
@@ -168,25 +162,27 @@ public void autonomousPeriodic() {
     /** This function is called once when teleop is enabled. */
     @Override
     public void teleopInit() {
-        aprilTagHighlighter.sequenceInitiated = false;
+        // aprilTagHighlighter.sequenceInitiated = false;
+        System.out.println("FLP inverted??????:" + driveLeftParent.getInverted());
+        System.out.println("FrP inverted??????:" + driveRightParent.getInverted());
     }
 
     /** This function is called periodically during operator control. */
     @Override
     public void teleopPeriodic() {
+        // TODO: make slow mode
         double leftY = driverController.getLeftY();
         double rightY = driverController.getRightY();
         // TODO the left wheels were moving despite the controller output being 0, why?
         QuickActions.stopAll();
-        if (Math.abs(leftY) > .03) {
-            driveLeftParent.set(-leftY);
+        if (Math.abs(leftY) > Constants.CONTROLLER_DEADZONE) {
+            driveLeftParent.set(leftY);
         }
-        if (Math.abs(rightY) > .03) {
+        if (Math.abs(rightY) > Constants.CONTROLLER_DEADZONE) {
             driveRightParent.set(rightY);
         }
-        driverController.setRumble(RumbleType.kBothRumble, 0.1);
-
-        aprilTagHighlighter.doEveryTeleopFrame(driverController);
+        // driverController.setRumble(RumbleType.kBothRumble, 0.1);
+        // aprilTagHighlighter.doEveryTeleopFrame(driverController);
     }
 
     /** This function is called once when the robot is disabled. */
@@ -243,6 +239,11 @@ public void simulationInit() {}
     @Override
     public void simulationPeriodic() {}
 
+    /**
+     * WARNING: THIS METHOD WILL RESET MOTOR CONFIG. Do all configuring after this method is called.
+     * @param motorController
+     * @param gains
+     */
     public void initializeSmartMotion(MotorController motorController, Gains gains) {
         /* Factory default hardware to prevent unexpected behavior */
         motorController.reset();
@@ -253,7 +254,7 @@ public void initializeSmartMotion(MotorController motorController, Gains gains)
         /*
          * set deadband to super small 0.001 (0.1 %). The default deadband is 0.04 (4 %)
          */
-        motorController.setNeutralDeadband(0.001);
+        motorController.setNeutralDeadband(0.05);
 
         /**
          * Configure Talon SRX Output and Sensor direction accordingly Invert Motor to have green LEDs
@@ -265,13 +266,13 @@ public void initializeSmartMotion(MotorController motorController, Gains gains)
         motorController.setStatusFramePeriod(10);
 
         /* Set the peak and nominal outputs */
-        motorController.setOutputLimits(0, 0, gains.PEAK_OUTPUT, -gains.PEAK_OUTPUT);
+        // motorController.setOutputLimits(0, 0, gains.PEAK_OUTPUT, -gains.PEAK_OUTPUT);
 
         /* Set Motion Magic gains in slot0 - see documentation */
         motorController.setPID(gains);
 
         /* Set acceleration and vcruise velocity - see documentation */
-        motorController.setMotionSpeed(15000, 400);
+        motorController.setMotionSpeed(15000 * Constants.DRIVE_GEARBOX_RATIO, 400 * Constants.DRIVE_GEARBOX_RATIO);
         /* Zero the sensor once on robot boot up */
         motorController.setEncoderPosition(0);
     }

src/main/java/frc/robot/auton/AutonActionRunner.java

@@ -22,11 +22,16 @@ public void initiateAuton() {
     }
 
     public void onEveryFrame() {
+        if (queue.isEmpty()) {
+            return;
+        }
         AutonAction action = queue.getFirst();
         if (action.isDone()) {
             queue.removeFirst();
-            // // Maybe remove if causes problems
-            // onEveryFrame();
+            if (queue.isEmpty()) {
+                System.out.println("We have finished Auton!");
+                return;
+            }
             queue.getFirst().initiate();
         }
     }

src/main/java/frc/robot/auton/AutonMoveForward.java

@@ -1,18 +1,27 @@
 package frc.robot.auton;
 
 import frc.robot.QuickActions;
-import frc.robot.Robot;
 
 public class AutonMoveForward extends AutonAction {
 
     double distance;
+    double isDoneDebounceTime = 0;
 
     @Override
     public boolean isDone() {
-        double distanceTravelled = Robot.getGyroscope().getDisplacementY();
+        QuickActions.leftParent.setDistance(distance);
+        QuickActions.rightParent.setDistance(distance);
+        System.out.println(
+            QuickActions.leftParent.get() + " veL: " + QuickActions.leftParent.getActiveTrajectoryVelocity()
+        );
+        if (getMaxTrajectoryVelocity() < 0.05) {
+            isDoneDebounceTime += 0.02;
+        } else {
+            isDoneDebounceTime = 0;
+        }
 
-        if (distanceTravelled >= distance) {
-            QuickActions.stopAll();
+        if (isDoneDebounceTime > 3) {
+            System.out.println("We moved the correct amount of inches!");
             return true;
         }
 
@@ -22,8 +31,13 @@ public boolean isDone() {
     @Override
     public void initiate() {
         QuickActions.resetDriveTrainEncoders();
-        QuickActions.leftParent.setDistance(distance);
-        QuickActions.rightParent.setDistance(distance);
+    }
+
+    private double getMaxTrajectoryVelocity() {
+        return Math.max(
+            QuickActions.leftParent.getActiveTrajectoryVelocity(),
+            QuickActions.rightParent.getActiveTrajectoryVelocity()
+        );
     }
 
     /**

src/main/java/frc/robot/auton/AutonRoutes.java

@@ -6,9 +6,9 @@
 public class AutonRoutes {
 
     public static ArrayDeque<AutonAction> RUN_INTO_WALL_AND_BREAK_ROBOT = new ArrayDeque<>(
-        List.of(new AutonMoveForward(100))
+        List.of(new AutonMoveForward(36))
     );
     public static ArrayDeque<AutonAction> RUN_INTO_OTHER_WALL_AND_BREAK_ROBOT_AGAIN = new ArrayDeque<>(
-        List.of(new AutonMoveForward(-100))
+        List.of(new AutonMoveForward(-36))
     );
 }

src/main/java/frc/robot/auton/AutonShoot.java

@@ -1,13 +1,32 @@
 package frc.robot.auton;
 
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.motor.MotorController;
+
 //TODO : this
 public class AutonShoot extends AutonAction {
 
+    private MotorController leftShooterMotor;
+    private MotorController rightShooterMotor;
+
+    private MotorController shooterFeederMotor;
+
+    private double revFinishedTime;
+
     @Override
     public boolean isDone() {
+        if (Timer.getFPGATimestamp() >= revFinishedTime) {
+            shooterFeederMotor.set(0.2);
+        }
+
         return true;
     }
 
     @Override
-    public void initiate() {}
+    public void initiate() {
+        leftShooterMotor.set(1.0);
+        rightShooterMotor.set(1.0);
+        revFinishedTime = Timer.getFPGATimestamp() + Constants.REV_TIME;
+    }
 }

src/main/java/frc/robot/motor/MotorController.java

@@ -86,6 +86,12 @@ public interface MotorController extends edu.wpi.first.wpilibj.motorcontrol.Moto
      */
     public void setStatusFramePeriod(int period);
 
+    /**
+     * @return The voltage fed into the motor controller. (In volts?)
+     */
+
+    public double getBusVoltage();
+
     /**
      * Set the output limits for the controller. All numbers range from -1.0 to 1.0.
      *

src/main/java/frc/robot/motor/SparkMotorController.java

@@ -19,6 +19,8 @@ public class SparkMotorController implements MotorController {
     SparkMotorController(int canID) {
         controller = new CANSparkMax(canID, MotorType.kBrushless);
         encoder = controller.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42);
+        encoder.setPositionConversionFactor(Constants.DRIVE_GEARBOX_RATIO);
+        encoder.setVelocityConversionFactor(Constants.DRIVE_GEARBOX_RATIO);
 
         // number
         // of ticks
@@ -37,14 +39,22 @@ public void setPercentOutput(double speed) {
         controller.set(speed);
     }
 
+    // idk
+    public SparkPIDController getPID() {
+        return pid;
+    }
+
     @Override
     public void setVelocity(double speed) {
         pid.setReference(speed, CANSparkMax.ControlType.kSmartVelocity);
     }
 
     @Override
     public void setDistance(double inches) {
-        pid.setReference(inches / Constants.WHEEL_CIRCUMFERENCE_INCHES, CANSparkMax.ControlType.kSmartMotion);
+        pid.setReference(
+            (inches / Constants.WHEEL_CIRCUMFERENCE_INCHES) * Constants.DRIVE_GEARBOX_RATIO,
+            CANSparkMax.ControlType.kSmartMotion
+        );
     }
 
     @Override
@@ -65,14 +75,19 @@ public void follow(MotorController leader) {
         controller.follow(((SparkMotorController) leader).controller);
     }
 
+    @Override
+    public double getBusVoltage() {
+        return controller.getBusVoltage();
+    }
+
     @Override
     public void setSensorSource() {
         // Do nothing
     }
 
     @Override
     public void setNeutralDeadband(double percent) {
-        // Only used for PWM (?)
+        pid.setSmartMotionAllowedClosedLoopError(percent, 0);
     }
 
     @Override

src/main/java/frc/robot/motor/TalonMotorController.java

@@ -50,6 +50,11 @@ public void setInverted(boolean inverted) {
         controller.setInverted(inverted);
     }
 
+    @Override
+    public double getBusVoltage() {
+        return controller.getBusVoltage();
+    }
+
     @Override
     public void follow(MotorController leader) {
         if (!(leader instanceof TalonMotorController)) throw new IllegalArgumentException(