some stuff with kavin

RoboLoCo-5338 · Oct 12, 2024 · ec1caf5 · ec1caf5
1 parent 723fb1f
commit ec1caf5
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -81,6 +81,7 @@ public Command getAutonomousCommand() {
       RobotContainer.m_xrpDrivetrain, RobotContainer.m_rangefinder
     );
     */
+
     return new SequentialCommandGroup(
       DriveCommands.tankDriveBackwards(2),
       ServoCommands.servoPresetCommand(1),
@@ -91,5 +92,11 @@ public Command getAutonomousCommand() {
         ServoCommands.servoSetAngleCommand(0)
       )
     );
+    //return DriveCommands.tankDriveDistance(5);
+    /*return new SequentialCommandGroup(
+      DriveCommands.tankDriveDistance(5),
+      DriveCommands.tankTurnDegrees(0.5, 50)
+      );
+      */
   }
 }
diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -262,13 +262,15 @@ public static Command tankDriveCommand(Supplier<Double> left_speed, Supplier<Dou
   public static Command tankTurnDegrees(double speed, double degrees){
     final XRPDrivetrain m_drive = RobotContainer.m_xrpDrivetrain;
     final double m_speed=speed;
+    /*
     if (degrees<0) {
-      final double m_degrees=360*Math.abs(degrees);
+      final double m_degrees=360-Math.abs(degrees);
     } else {
       final double m_degrees=degrees;
     }
+    */
     //im count 90 degrees as 90 degrees counterclockwise, 270 degrees as 270 degrees ccw aka 90 cw, so on
-    final double m_degrees=degrees%360;
+    final double m_degrees=degrees;
     return new FunctionalCommand(
       () -> {
         m_drive.tankDrive(0, 0);
@@ -277,11 +279,11 @@ public static Command tankTurnDegrees(double speed, double degrees){
       () -> {
         if (m_degrees>180) {
           //turn right
-          m_drive.tankDrive(m_speed, 0);  
+          m_drive.tankDrive(m_speed, -m_speed);  
         }
         if (m_degrees<=180) {
           //turn left
-          m_drive.tankDrive(0,m_speed);
+          m_drive.tankDrive(-m_speed,m_speed);
         }
       },
       interrupted -> m_drive.tankDrive(0, 0),