fix drivetrain zeroing

team4099 · Mar 2, 2024 · 9ae44c7 · 9ae44c7
1 parent b5ab4ea
commit 9ae44c7
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 10 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt
@@ -46,7 +46,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
               )
             )
           },
-          keepTrapping = true
+          keepTrapping = false
         ),
         WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
       ),
@@ -97,7 +97,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
               )
             )
           },
-          keepTrapping = true
+          keepTrapping = false
         ),
         WaitCommand(1.0).andThen(superstructure.groundIntakeCommand())
       ),
@@ -148,7 +148,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
               ) // Subwoofer
             )
           },
-          keepTrapping = true
+          keepTrapping = false
         ),
         WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
       ),

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -217,7 +217,7 @@ private constructor(
     if (keepTrapping) {
       swerveDriveController.setTolerance(Pose2d(3.inches, 3.inches, 2.5.degrees).pose2d)
     } else {
-      swerveDriveController.setTolerance(Pose2d(6.inches, 6.inches, 5.degrees).pose2d)
+      swerveDriveController.setTolerance(Pose2d(6.inches, 6.inches, 10.degrees).pose2d)
     }
   }
 
@@ -420,8 +420,8 @@ private constructor(
   override fun isFinished(): Boolean {
     trajCurTime = Clock.fpgaTime - trajStartTime
     return endPathOnceAtReference &&
-      (swerveDriveController.atReference()) &&
-      trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds
+            ((swerveDriveController.atReference()) &&
+      trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds)
   }
 
   override fun end(interrupted: Boolean) {

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
@@ -425,6 +425,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
     val allianceFlippedDriveVector =
       Pair(driveVector.first * flipDrive, driveVector.second * flipDrive)
 
+    Logger.recordOutput("Drivetrain/driveVectorFirst", allianceFlippedDriveVector.first.inMetersPerSecond)
+    Logger.recordOutput("Drivetrain/driveVectorSecond", allianceFlippedDriveVector.second.inMetersPerSecond)
+
     val swerveModuleStates: Array<SwerveModuleState>
     var desiredChassisSpeeds: ChassisSpeeds
 
@@ -435,7 +438,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
           allianceFlippedDriveVector.first,
           allianceFlippedDriveVector.second,
           angularVelocity,
-          odomTRobot.rotation
+          odomTField.rotation
         )
     } else {
       desiredChassisSpeeds =

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -631,9 +631,8 @@ class Superstructure(
 
   fun groundIntakeCommand(): Command {
     val returnCommand =
-      runOnce { currentRequest = Request.SuperstructureRequest.GroundIntake() }.until {
-        isAtRequestedState && currentState == SuperstructureStates.GROUND_INTAKE
-      }
+      runOnce { currentRequest = Request.SuperstructureRequest.GroundIntake() }
+
     returnCommand.name = "GroundIntakeCommand"
     return returnCommand
   }

diff --git a/src/main/kotlin/com/team4099/robot2023/util/FMSData.kt b/src/main/kotlin/com/team4099/robot2023/util/FMSData.kt
@@ -7,4 +7,5 @@ object FMSData {
 
   val isBlue: Boolean
     get() = DriverStation.getAlliance().get() == DriverStation.Alliance.Blue
+
 }
Original file line number	Diff line number	Diff line change
Expand Up		@@ -7,4 +7,5 @@ object FMSData {

		val isBlue: Boolean
		get() = DriverStation.getAlliance().get() == DriverStation.Alliance.Blue

		}