Merge branch 'offseason-tuning' of https://github.com/team4099/Charge…

…dUp-2023 into offseason-tuning
team4099 · Oct 11, 2023 · d7fc46b · d7fc46b
2 parents 3fb2d80 + 01a520c
commit d7fc46b
Show file tree

Hide file tree

Showing 6 changed files with 59 additions and 33 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -5,6 +5,7 @@ import com.team4099.robot2023.auto.AutonomousSelector
 import com.team4099.robot2023.commands.AutoIntakeCommand
 import com.team4099.robot2023.commands.AutoScoreCommand
 import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
+import com.team4099.robot2023.commands.drivetrain.SwerveModuleTuningCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
@@ -121,6 +122,18 @@ object RobotContainer {
         { ControlBoard.slowMode },
         drivetrain
       )
+
+
+    /*
+    module steeing tuning
+
+    drivetrain.defaultCommand =
+      SwerveModuleTuningCommand(
+        drivetrain,
+        { (ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) * 180).degrees },
+      )
+
+     */
   }
 
   fun requestSuperstructureIdle() {

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt
@@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.inRotation2ds
 
-class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition: Angle) :
+class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition: () -> Angle) :
   CommandBase() {
   init {
     addRequirements(drivetrain)
@@ -15,8 +15,8 @@ class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition
   override fun execute() {
     for (module in drivetrain.swerveModules) {
       module.setPositionClosedLoop(
-        SwerveModuleState(0.0, steeringPosition.inRotation2ds),
-        SwerveModuleState(0.0, steeringPosition.inRotation2ds),
+        SwerveModuleState(0.0, steeringPosition().inRotation2ds),
+        SwerveModuleState(0.0, steeringPosition().inRotation2ds),
         false
       )
     }

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt
@@ -4,6 +4,8 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.util.driver.DriverProfile
 import edu.wpi.first.wpilibj2.command.CommandBase
 import org.littletonrobotics.junction.Logger
+import java.util.function.DoubleSupplier
+import java.util.function.Supplier
 
 class TeleopDriveCommand(
   val driver: DriverProfile,

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
@@ -55,7 +55,7 @@ object Constants {
 
   object Tuning {
 
-    const val TUNING_MODE = false
+    const val TUNING_MODE = true
     const val DEBUGING_MODE = true
     const val SIMULATE_DRIFT = false
     const val DRIFT_CONSTANT = 0.001

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
@@ -67,8 +67,8 @@ object ElevatorConstants {
   // circumference / 2pi = radius
   val SPOOL_RADIUS = 0.005.meters * 32.0 / (2 * PI)
 
-  val MAX_VELOCITY = 100.inches.perSecond // 75
-  val MAX_ACCELERATION = 350.inches.perSecond.perSecond // 225
+  val MAX_VELOCITY = 140.inches.perSecond // 75
+  val MAX_ACCELERATION = 415.inches.perSecond.perSecond // 225
 
   val ENABLE_ELEVATOR = 1.0
 

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt
@@ -15,6 +15,7 @@ import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.angle
 import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.derived.inRotation2ds
 import org.team4099.lib.units.derived.inVoltsPerDegree
@@ -34,6 +35,7 @@ import org.team4099.lib.units.inMetersPerSecond
 import org.team4099.lib.units.inMetersPerSecondPerSecond
 import org.team4099.lib.units.perSecond
 import kotlin.math.IEEErem
+import kotlin.math.abs
 import kotlin.math.withSign
 
 class SwerveModule(val io: SwerveModuleIO) {
@@ -130,24 +132,24 @@ class SwerveModule(val io: SwerveModuleIO) {
     }
 
     Logger.getInstance().processInputs(io.label, inputs)
-    //    Logger.getInstance()
-    //      .recordOutput(
-    //        "${io.label}/driveSpeedSetpointMetersPerSecond",
-    //        if (!shouldInvert) speedSetPoint.inMetersPerSecond
-    //        else -speedSetPoint.inMetersPerSecond
-    //      )
-    //    Logger.getInstance()
-    //      .recordOutput(
-    //        "${io.label}/driveAccelSetpointMetersPerSecondSq",
-    //        accelerationSetPoint.inMetersPerSecondPerSecond
-    //      )
-    //    Logger.getInstance()
-    //      .recordOutput("${io.label}/steeringSetpointDegrees", steeringSetPoint.inDegrees)
-    //    Logger.getInstance()
-    //      .recordOutput(
-    //        "${io.label}/steeringValueDegreesWithMod",
-    //        inputs.steeringPosition.inDegrees.IEEErem(360.0)
-    //      )
+        Logger.getInstance()
+          .recordOutput(
+            "${io.label}/driveSpeedSetpointMetersPerSecond",
+            if (!shouldInvert) speedSetPoint.inMetersPerSecond
+            else -speedSetPoint.inMetersPerSecond
+          )
+        Logger.getInstance()
+          .recordOutput(
+            "${io.label}/driveAccelSetpointMetersPerSecondSq",
+            accelerationSetPoint.inMetersPerSecondPerSecond
+          )
+        Logger.getInstance()
+          .recordOutput("${io.label}/steeringSetpointRadians", steeringSetPoint.inRadians)
+        Logger.getInstance()
+          .recordOutput(
+            "${io.label}/steeringValueDegreesWithMod",
+            inputs.steeringPosition.inDegrees.IEEErem(360.0)
+          )
 
     Logger.getInstance().recordOutput("SwerveModule/SpeedSetPoint", speedSetPoint.inMetersPerSecond)
     Logger.getInstance().recordOutput("SwerveModule/SteeringSetPoint", steeringSetPoint.inRadians)
@@ -238,13 +240,13 @@ class SwerveModule(val io: SwerveModuleIO) {
         optimizedState
           .speedMetersPerSecond
           .meters
-          .perSecond // consider desaturating wheel speeds here if it doesn't work
+          .perSecond * Math.cos(abs((optimizedState.angle.angle - inputs.steeringPosition).inRadians))// consider desaturating wheel speeds here if it doesn't work
         // from drivetrain
       )
       Logger.getInstance()
         .recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees)
     } else {
-      io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond)
+      io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond * Math.cos(abs((desiredState.angle.angle - inputs.steeringPosition).inRadians)))
       Logger.getInstance()
         .recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees)
     }
@@ -266,19 +268,28 @@ class SwerveModule(val io: SwerveModuleIO) {
   ) {
     if (optimize) {
       val optimizedVelState =
-        SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds)
+         SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds)
       val optimizedAccelState =
         SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds)
+
+      steeringSetPoint = optimizedVelState.angle.angle
+      speedSetPoint = optimizedVelState.speedMetersPerSecond.meters.perSecond
+      accelerationSetPoint = optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond
+
       io.setClosedLoop(
-        optimizedVelState.angle.angle,
-        optimizedVelState.speedMetersPerSecond.meters.perSecond,
-        optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond
+        steeringSetPoint,
+        speedSetPoint,
+        accelerationSetPoint
       )
     } else {
+      steeringSetPoint = desiredVelState.angle.angle
+      speedSetPoint = desiredVelState.speedMetersPerSecond.meters.perSecond
+      accelerationSetPoint = desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond
+
       io.setClosedLoop(
-        desiredVelState.angle.angle,
-        desiredVelState.speedMetersPerSecond.meters.perSecond,
-        desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond
+        steeringSetPoint,
+        speedSetPoint,
+        accelerationSetPoint
       )
     }
   }