Merge pull request #3 from frc-emotion/SaturdayB4BB

fully fixed refactored autonomous and mostly solved drifting
frc-emotion · Dec 7, 2023 · 63d5a34 · 63d5a34
2 parents 3ca4663 + e6fd65a
commit 63d5a34
Show file tree

Hide file tree

Showing 6 changed files with 64 additions and 36 deletions.
diff --git a/src/main/deploy/pathplanner/Rotate.path b/src/main/deploy/pathplanner/Rotate.path
@@ -45,5 +45,8 @@
       }
     }
   ],
+  "maxVelocity": 3.0,
+  "maxAcceleration": 2.0,
+  "isReversed": null,
   "markers": []
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -121,10 +121,15 @@ public static final class DriveConstants {
         /**
          * Calculate by Positioning wheels at zero manually and reading absolute encoder values
          */
-         public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + 0.5); 
-         public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + 39.8 - 3.1);
-         public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + -154.098 - 0.7); 
-         public static final double kBackRightDriveAbsoluteEncoderOffsetRad =  -Units.degreesToRadians(157.7 - 4.7);
+
+        public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + 0.5); 
+        public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + 39.8 - 3.1);
+        public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + -154.098 - 0.7); 
+        public static final double kBackRightDriveAbsoluteEncoderOffsetRad =  -Units.degreesToRadians(157.7 - 4.7 + 12.351);
+        //  public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + 0.5); 
+        //  public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + 39.8 - 3.1);
+        //  public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + -154.098 - 0.7); 
+        //  public static final double kBackRightDriveAbsoluteEncoderOffsetRad =  -Units.degreesToRadians(157.7 - 4.7);
         // public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(-154.098 - 0.7);
         // public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(180 + 0.5);
         // public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -Units.degreesToRadians(157.7 - 4.7);

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -114,32 +114,33 @@ public void autonomousInit() {
         m_autonomousCommand = m_robotContainer.EasyToUse(
           levelcenter, 
           5.5,
+          true,
           new LockWheels(RobotContainer.swerveSubsytem).withTimeout(2) // Extra command for special cases such as leveling
           );
         break;
 
       case 2:
-        m_autonomousCommand = m_robotContainer.EasyToUse(rightMost, 8.5);
+        m_autonomousCommand = m_robotContainer.EasyToUse(rightMost, 8.5, false);
         break;
 
       case 3:
-        m_autonomousCommand = m_robotContainer.EasyToUse(placeauto, 16.5);
+        m_autonomousCommand = m_robotContainer.EasyToUse(placeauto, 16.5, false);
         break;
 
       case 4:
-        m_autonomousCommand = m_robotContainer.EasyToUse(bottomCurved, 10.5);
+        m_autonomousCommand = m_robotContainer.EasyToUse(bottomCurved, 10.5, false);
         break;
 
       case 5:
-        m_autonomousCommand = m_robotContainer.EasyToUse(straightBottomCone, 10.5);
+        m_autonomousCommand = m_robotContainer.EasyToUse(straightBottomCone, 10.5, false);
         break;
 
       case 6:
-        m_autonomousCommand = m_robotContainer.EasyToUse(smoothCurveTop, 12.5);
+        m_autonomousCommand = m_robotContainer.EasyToUse(smoothCurveTop, 12.5, false);
         break;
 
       case 7:
-        m_autonomousCommand = m_robotContainer.EasyToUse(topStraight, 5.5);
+        m_autonomousCommand = m_robotContainer.EasyToUse(topStraight, 5.5, false);
         break;
 
       default:

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -104,14 +104,14 @@ public Command getAutonomousCommand() {
   // Returns the command based on path & timeout given
   // Overload incase additional commands wanted
 
-  public Command EasyToUse(PathPlannerTrajectory pathToUse, Double timeoutTime) {
+  public Command EasyToUse(PathPlannerTrajectory pathToUse, Double timeoutTime, Boolean place) {
     //swerveSubsytem.autoGyro();
-    return new Autonomous(swerveSubsytem, armSubsystem, eSubsystem, clawSubsytem, pathToUse, timeoutTime);
+    return new Autonomous(swerveSubsytem, armSubsystem, eSubsystem, clawSubsytem, pathToUse, timeoutTime, place);
   }
 
-  public Command EasyToUse(PathPlannerTrajectory pathToUse, Double timeoutTime, Command additional) {
+  public Command EasyToUse(PathPlannerTrajectory pathToUse, Double timeoutTime, Boolean place, Command additional) {
     //swerveSubsytem.autoGyro();
-    return new Autonomous(swerveSubsytem, armSubsystem, eSubsystem, clawSubsytem, pathToUse, timeoutTime, additional);
+    return new Autonomous(swerveSubsytem, armSubsystem, eSubsystem, clawSubsytem, pathToUse, timeoutTime, place, additional);
   }
 
 

diff --git a/src/main/java/frc/robot/commands/SwerveXboxCommand.java b/src/main/java/frc/robot/commands/SwerveXboxCommand.java
@@ -47,16 +47,19 @@ public void execute() {
         double ySpeed = ySpdFunc.get();
         double turningSpeed = turningSpdFunc.get();
 
-        double currentTranslationalSpeed = DriveConstants.kTeleDriveNormalSpeedMetersPerSecond;
-        double currentAngularSpeed = DriveConstants.kTeleDriveNormalAngularSpeedRadiansPerSecond;
-
-        if (slowModeFunc.get()) {
-            currentTranslationalSpeed = DriveConstants.kTeleDriveSlowSpeedMetersPerSecond;
-            currentAngularSpeed = DriveConstants.kTeleDriveSlowlAngularSpeedRadiansPerSecond;
-        } else if (turboModeFunc.get()) {
-            currentTranslationalSpeed = DriveConstants.kTeleDriveMaxSpeedMetersPerSecond;
-            currentAngularSpeed = DriveConstants.kTeleDriveMaxAngularSpeedRadiansPerSecond;
-        }
+        // double currentTranslationalSpeed = DriveConstants.kTeleDriveNormalSpeedMetersPerSecond;
+        // double currentAngularSpeed = DriveConstants.kTeleDriveNormalAngularSpeedRadiansPerSecond;
+
+        double currentTranslationalSpeed = DriveConstants.kTeleDriveSlowSpeedMetersPerSecond;
+        double  currentAngularSpeed = DriveConstants.kTeleDriveSlowlAngularSpeedRadiansPerSecond;
+
+        // if (slowModeFunc.get()) {
+        //     currentTranslationalSpeed = DriveConstants.kTeleDriveSlowSpeedMetersPerSecond;
+        //     currentAngularSpeed = DriveConstants.kTeleDriveSlowlAngularSpeedRadiansPerSecond;
+        // } else if (turboModeFunc.get()) {
+        //     currentTranslationalSpeed = DriveConstants.kTeleDriveMaxSpeedMetersPerSecond;
+        //     currentAngularSpeed = DriveConstants.kTeleDriveMaxAngularSpeedRadiansPerSecond;
+        // }
         // deadBand
         xSpeed = Math.abs(xSpeed) > (OIConstants.kDeadband / 2) ? xSpeed : 0.0;
         ySpeed = Math.abs(ySpeed) > (OIConstants.kDeadband / 2) ? ySpeed : 0.0;

diff --git a/src/main/java/frc/robot/commands/auton/Autonomous.java b/src/main/java/frc/robot/commands/auton/Autonomous.java
@@ -13,28 +13,44 @@ public class Autonomous extends SequentialCommandGroup {
 
 
 
-    public Autonomous (SwerveSubsytem swerveSubsytem, ArmSubsystem aSubsystem, ElevatorSubsystem ElevatorSubsystem, ClawSubsystem ClawSubsystem, PathPlannerTrajectory path, Double timeoutTime){
+    public Autonomous (SwerveSubsytem swerveSubsytem, ArmSubsystem aSubsystem, ElevatorSubsystem ElevatorSubsystem, ClawSubsystem ClawSubsystem, PathPlannerTrajectory path, Double timeoutTime, Boolean place){
 
         AutoAbstracted autoCommands = new AutoAbstracted(swerveSubsytem, aSubsystem, ElevatorSubsystem, ClawSubsystem);
 
-        addCommands(
-            autoCommands.PlaceConeMid(),
-            SwerveController.followTrajectoryCommand(path, true, swerveSubsytem).withTimeout(timeoutTime)
-
-            );
+        if (place) {
+            addCommands(
+                autoCommands.PlaceConeMid(),
+                SwerveController.followTrajectoryCommand(path, true, swerveSubsytem).withTimeout(timeoutTime)
+                );   
+        } else {
+            addCommands(
+                SwerveController.followTrajectoryCommand(path, true, swerveSubsytem).withTimeout(timeoutTime)
+                );
+        }
+
+
 
 
     }
 
-    public Autonomous (SwerveSubsytem swerveSubsytem, ArmSubsystem aSubsystem, ElevatorSubsystem ElevatorSubsystem, ClawSubsystem ClawSubsystem, PathPlannerTrajectory path, Double timeoutTime, Command AdditionalCommands){
+    public Autonomous (SwerveSubsytem swerveSubsytem, ArmSubsystem aSubsystem, ElevatorSubsystem ElevatorSubsystem, ClawSubsystem ClawSubsystem, PathPlannerTrajectory path, Double timeoutTime, Boolean place, Command AdditionalCommands){
 
         AutoAbstracted autoCommands = new AutoAbstracted(swerveSubsytem, aSubsystem, ElevatorSubsystem, ClawSubsystem);
 
-        addCommands(
-            autoCommands.PlaceConeMid(),
-            SwerveController.followTrajectoryCommand(path, true, swerveSubsytem).withTimeout(timeoutTime),
-            AdditionalCommands
-            );
+        if (place) {
+            addCommands(
+                autoCommands.PlaceConeMid(),
+                SwerveController.followTrajectoryCommand(path, true, swerveSubsytem).withTimeout(timeoutTime),
+                AdditionalCommands
+                );
+        } else {
+            addCommands(
+                SwerveController.followTrajectoryCommand(path, true, swerveSubsytem).withTimeout(timeoutTime),
+                AdditionalCommands
+                );
+        }
+
+
     }
 
 }