a

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index bdd01ed205e3fa5bbd52c07d3e7be4e6f58f55f4..9ea32a2ccb556c8d658de053443ab7e3dab4dd1a 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -88,7 +88,7 @@ public class AutoShootCommand extends Command {
         this.shooter = shooter;
         this.spindexer = spindexer;
         //drivepose  = drivetrain.getPose();
-        drivepose  = drivetrain.getPose().rotateBy(new Rotation2d(Math.PI)); // TODO: Revert when robot isn't backwards 
+        drivepose  = new Pose2d(drivepose.getTranslation(), drivepose.getRotation().plus(new Rotation2d(Math.PI)));
 
         goalState = ShooterPhysics.getShotParams(
                                FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
@@ -173,8 +173,14 @@ public class AutoShootCommand extends Command {
     }
 
     public void updateDrivePose(){
+        Pose2d currentPose = drivetrain.getPose();
+        // Add 180 degrees to the rotation bc robot is backwards
+        drivepose = new Pose2d(
+            currentPose.getTranslation(), 
+            currentPose.getRotation().plus(new Rotation2d(Math.PI))
+        );
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
-        drivepose = drivetrain.getPose().exp(
+        drivepose.exp(
             new Twist2d(
                 robotRelVel.vxMetersPerSecond * phaseDelay,
                 robotRelVel.vyMetersPerSecond * phaseDelay,

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index b6e2eccdf43730e9a506188f48668ef28a81006e..4d0d3712514f78e0687a0d877ec1bd84037a18c3 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -49,11 +49,11 @@ public class Turret extends SubsystemBase implements TurretIO{
 
        /* ---------------- Constants ---------------- */
 
-       private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-90); // Change this later to the actual values
-       private static final double MAX_ANGLE_RAD = Units.degreesToRadians(90);
+       private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-180); // Change this later to the actual values
+       private static final double MAX_ANGLE_RAD = Units.degreesToRadians(180);
 
-       //private static final double MAX_VEL_RAD_PER_SEC = 25;
-       private static final double MAX_VEL_RAD_PER_SEC = 3.0; // Starting super duper slow for now
+       private static final double MAX_VEL_RAD_PER_SEC = 25;
+       //private static final double MAX_VEL_RAD_PER_SEC = 3.0; // Starting super duper slow for now
        private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
 
        private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO;