ru happy arnav

diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
index 4cea990d9ca6d3bb548acc53a3705a4fbca27eba..b0865dfcadae873d369e87a90984b77185d8dc74 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
+++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
@@ -9,25 +9,15 @@ import frc.robot.subsystems.spindexer.Spindexer;
 import lib.controllers.PS5Controller.PS5Button;
 
 public class ReverseMotors extends Command {
-    // Reverse motors (intake, spindexer, shooter: shoot out constant speed)
-
-        // if (intake != null && spindexer != null && shooter != null){
-        //     driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() ->{
-        //         intake.spinReverse();
-        //         // reverse spindexer
-        //         shooter.
-        //     }))
-        // }
     private Intake intake;
     private Spindexer spindexer;
-    private Shooter shooter;
 
 
-    public ReverseMotors(Intake intake, Spindexer spindexer, Shooter shooter){
+    public ReverseMotors(Intake intake, Spindexer spindexer){
         this.intake = intake;
         this.spindexer = spindexer;
 
-        addRequirements(intake, spindexer, shooter);
+        addRequirements(intake, spindexer);
     }
 
     @Override
@@ -39,7 +29,6 @@ public class ReverseMotors extends Command {
     public void execute(){
         intake.spinReverse();
         spindexer.reverseSpindexer();
-        shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 65dc6fd9a4cd17993d85db4b2fd00710cb008baf..2a7ca77c504e54efc928f3a186857008dc6c2ae9 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -36,9 +36,8 @@ public class Superstructure extends Command {
     private Shooter shooter;
     private Spindexer spindexer;
 
-    // TODO: find maximum interpolation
-    private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767,
-            HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
+    //TODO: find maximum interpolation
+    private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
 
     private double turretSetpoint;
     private double hoodSetpoint;
@@ -47,7 +46,7 @@ public class Superstructure extends Command {
 
     private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
     private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
-
+    
     private Rotation2d lastTurretAngle;
     private Rotation2d turretAngle;
     private double turretVelocity;
@@ -70,66 +69,59 @@ public class Superstructure extends Command {
         this.hood = hood;
         this.shooter = shooter;
         this.spindexer = spindexer;
-        drivepose = drivetrain.getPose();
+        drivepose  = drivetrain.getPose();
 
         goalState = ShooterPhysics.getShotParams(
-                FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
-                8.0); // Random initial goalState to prevent it being null
-
+                               FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
+                               8.0); // Random initial goalState to prevent it being null
+        
         addRequirements(turret);
     }
 
     public void updateSetpoints(Pose2d drivepose) {
-        Pose2d turretPosition = drivepose
-                .transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),
-                        TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d()));
-
+        Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d()));
+        
         // If the robot is moving, adjust the target position based on velocity
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
         ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
 
         // Rotational adjustment is not being used, since turret is in center of robot
-        double turretVelocityX = fieldRelVel.vxMetersPerSecond
+        double turretVelocityX =
+            fieldRelVel.vxMetersPerSecond
                 + fieldRelVel.omegaRadiansPerSecond
-                        * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY()
-                                * Math.cos(drivepose.getRotation().getRadians())
-                                - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX()
-                                        * Math.sin(drivepose.getRotation().getRadians()));
-        double turretVelocityY = fieldRelVel.vyMetersPerSecond
+                    * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians())
+                        - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians()));
+        double turretVelocityY =
+            fieldRelVel.vyMetersPerSecond
                 + fieldRelVel.omegaRadiansPerSecond
-                        * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX()
-                                * Math.cos(drivepose.getRotation().getRadians())
-                                - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY()
-                                        * Math.sin(drivepose.getRotation().getRadians()));
+                    * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians())
+                        - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians()));
 
         double timeOfFlight;
         Pose2d lookaheadPose = turretPosition;
 
         /*
          * Loop (20) until lookaheadPose converges BECAUSE -->
-         * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate
-         * for 6m (t = 0.8s)
-         * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was
-         * derived using t = 1.0s
+         * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s)
+         * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s
          * So we make a bunch of guesses until it converges
          */
         for (int i = 0; i < 20; i++) {
-            Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(),
-                    TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
-
-            Translation3d target3d = new Translation3d(target.getX(), target.getY(),
-                    target == FieldConstants.getHubTranslation().toTranslation2d()
-                            ? FieldConstants.getHubTranslation().getZ()
-                            : 0.0); // Height of 0 if it's not the hub
+            Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
+            
+            Translation3d target3d = new Translation3d(target.getX(), target.getY(), 
+                target == FieldConstants.getHubTranslation().toTranslation2d() ?
+                FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub
 
             goalState = ShooterPhysics.getShotParams(
-                    target3d.minus(lookahead3d),
-                    2.0);
+                               target3d.minus(lookahead3d),
+                               2.0);
 
             timeOfFlight = goalState.timeOfFlight();
             double offsetX = turretVelocityX * timeOfFlight;
             double offsetY = turretVelocityY * timeOfFlight;
-            lookaheadPose = new Pose2d(
+            lookaheadPose =
+                new Pose2d(
                     turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
                     turretPosition.getRotation());
         }
@@ -140,30 +132,26 @@ public class Superstructure extends Command {
             lastTurretAngle = turretAngle;
         }
 
-        // Take the filtered average as the turret's velocity when robot is moving
-        // translationally
-        turretVelocity = turretAngleFilter
-                .calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
-
+        // Take the filtered average as the turret's velocity when robot is moving translationally
+        turretVelocity =
+        turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
+        
         lastTurretAngle = turretAngle;
 
         Logger.recordOutput("Lookahead Pose", lookaheadPose);
 
         // Subtract the rotational angle of the robot from the setpoint
-        double adjustedTurretSetpoint = MathUtil
-                .angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
+        double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
 
         // Shortest path
-        double error = MathUtil.inputModulus(
-                Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180,
-                180);
+        double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
         double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error;
 
-        // Stay within +/- 200 -- if shortest path is past 200, we go long way around
+        // Stay within +/- 200 -- if  shortest path is past 200, we go long way around
         double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE;
-        if (potentialSetpoint > turretRange / 2) {
+        if (potentialSetpoint > turretRange/2) {
             potentialSetpoint -= 360;
-        } else if (potentialSetpoint < -turretRange / 2) {
+        } else if (potentialSetpoint < -turretRange/2) {
             potentialSetpoint += 360;
         }
 
@@ -171,35 +159,34 @@ public class Superstructure extends Command {
 
         // Pitch is in radians
         hoodAngle = goalState.pitch();
-        hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE,
-                HoodConstants.MAX_ANGLE);
+        hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
         hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
         lastHoodAngle = hoodAngle;
 
     }
 
-    public void updateDrivePose() {
+    public void updateDrivePose(){
         Pose2d currentPose = drivetrain.getPose();
 
         drivepose = new Pose2d(
-                currentPose.getTranslation(),
-                // Uncomment this if robot is backwards
-                currentPose.getRotation()// .plus(new Rotation2d(Math.PI))
+            currentPose.getTranslation(), 
+            // Uncomment this if robot is backwards
+            currentPose.getRotation()//.plus(new Rotation2d(Math.PI))
         );
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
 
         // Add a phase delay extrapolation component for latency delay
         drivepose.exp(
-                new Twist2d(
-                        robotRelVel.vxMetersPerSecond * phaseDelay,
-                        robotRelVel.vyMetersPerSecond * phaseDelay,
-                        robotRelVel.omegaRadiansPerSecond * phaseDelay));
+            new Twist2d(
+                robotRelVel.vxMetersPerSecond * phaseDelay,
+                robotRelVel.vyMetersPerSecond * phaseDelay,
+                robotRelVel.omegaRadiansPerSecond * phaseDelay));
     }
 
     /**
      * Stops and stows all subsystems involved in the command
      */
-    public void stowEverything() {
+    public void stowEverything(){
         turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
         hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
         shooter.setShooter(0.0);
@@ -218,8 +205,7 @@ public class Superstructure extends Command {
         if (phaseManager.isIdle()) {
             stowEverything();
         } else {
-            turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint),
-                    turretVelocity - drivetrain.getAngularRate(2));
+            turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2));
             hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity);
             shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 4527288f5a3c6977ce5d1919e811e3bea208ddef..4afbec2486833debacb57e8448d86ead62a535ba 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -83,7 +83,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         // Reverse motors
         if (intake != null && spindexer != null && shooter != null){
             driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(()->{
-                reverseMotors = new ReverseMotors(intake, spindexer, shooter);
+                reverseMotors = new ReverseMotors(intake, spindexer);
                 reverseMotors.schedule();
             })
             ).onFalse(new InstantCommand(()->{

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 3a9cc27ae576bdbb8c6ff3564924d273f4e92028..99e068eab60e73fa01e871868d53be7dffd1308b 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -143,7 +143,7 @@ public class Turret extends SubsystemBase implements TurretIO{
        }
 
        /**
-        * @return If the turret is at setpoint with tolerance of 2 degrees
+        * @return If the turret is at setpoint with tolerance of 5 degrees
         */
        public boolean atSetpoint() {
                return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(5.0);