Merge remote-tracking branch 'origin/main'

src/main/deploy/choreo/BRP-449.json

@@ -1,14 +1,14 @@
 {
     "version": "v0.0.1",
     "robotConfiguration": {
-        "mass": 54.5,
+        "mass": 60.0,
         "rotationalInertia": 6,
-        "wheelMaxTorque": 1.875,
+        "wheelMaxTorque": 1.675,
         "wheelMaxVelocity": 88.06,
-        "wheelbase": 0.62865,
-        "trackWidth": 0.55245,
-        "bumperLength": 0.851,
-        "bumperWidth": 0.775,
+        "wheelbase": 0.6223,
+        "trackWidth": 0.5461,
+        "bumperLength": 0.9271,
+        "bumperWidth": 0.851,
         "wheelRadius": 0.0508
     },
     "paths": {

src/main/kotlin/frc/team449/RobotLoop.kt

@@ -44,7 +44,6 @@ class RobotLoop : TimedRobot() {
 
     PathPlannerServer.startServer(5811)
 
-
     Logger.configureLoggingAndConfig(robot, false)
     SmartDashboard.putData("Field", robot.field)
     SmartDashboard.putData("Routine Chooser", routineChooser)

src/main/kotlin/frc/team449/control/auto/ChoreoFollower.kt

@@ -1,6 +1,5 @@
 package frc.team449.control.auto
 
-
 import edu.wpi.first.math.controller.PIDController
 import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
@@ -84,4 +83,4 @@ class ChoreoFollower(
     timer.reset()
     drivetrain.stop()
   }
-}
+}

src/main/kotlin/frc/team449/control/auto/ChoreoRoutine.kt

@@ -64,5 +64,4 @@ class ChoreoRoutine(
 
     return ezraGallun
   }
-
-}
+}

src/main/kotlin/frc/team449/control/auto/ChoreoTrajectory.kt

@@ -14,7 +14,6 @@ import org.json.simple.parser.JSONParser
 import java.io.File
 import java.io.FileReader
 
-
 class ChoreoTrajectory(
   val name: String,
   val stateMap: InterpolatingMatrixTreeMap<Double, N2, N3>,
@@ -57,7 +56,7 @@ class ChoreoTrajectory(
 
   companion object {
     fun createTrajectory(
-      filename: String,
+      filename: String
     ): MutableList<ChoreoTrajectory> {
       val path = Filesystem.getDeployDirectory().absolutePath.plus("/choreo/$filename.json")
       val document = (JSONParser().parse(FileReader(File(path).absolutePath)) as JSONObject)["paths"] as HashMap<*, *>
@@ -82,13 +81,11 @@ class ChoreoTrajectory(
             info.second
           )
         )
-
       }
 
       return trajList
     }
 
-
     private fun parse(trajectory: JSONArray): Pair<InterpolatingMatrixTreeMap<Double, N2, N3>, ArrayList<Double>> {
       val stateMap = InterpolatingMatrixTreeMap<Double, N2, N3>()
 
@@ -116,4 +113,4 @@ class ChoreoTrajectory(
       return stateMap to timestamps
     }
   }
-}
+}

src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt

@@ -141,7 +141,6 @@ open class SwerveDrive(
     this.set(ChassisSpeeds(0.0, 0.0, 0.0))
   }
 
-
   /** @return An array of [SwerveModulePosition] for each module, containing distance and angle. */
   private fun getPositions(): Array<SwerveModulePosition> {
     return Array(modules.size) { i -> modules[i].position }

src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt

@@ -103,8 +103,7 @@ class SwerveOrthogonalCommand(
         atGoal = false
         rotCtrl.setpoint = desAngleA
       }
-    }
-    else if (controller.yButtonPressed) {
+    } else if (controller.yButtonPressed) {
       val desAngleY = MathUtil.angleModulus(PI + allianceCompensation.invoke())
       if (abs(desAngleY - drive.heading.radians) > 0.075 && abs(desAngleY - drive.heading.radians) < 2 * PI - 0.075) {
         atGoal = false
@@ -115,7 +114,8 @@ class SwerveOrthogonalCommand(
     if (atGoal) {
       rotScaled = rotRamp.calculate(
         (if (abs(controller.rightX) < RobotConstants.ROTATION_DEADBAND) .0 else -controller.rightX) *
-        drive.maxRotSpeed)
+          drive.maxRotSpeed
+      )
     } else {
       rotScaled = MathUtil.clamp(
         rotCtrl.calculate(drive.heading.radians),
@@ -164,5 +164,4 @@ class SwerveOrthogonalCommand(
     builder.addStringProperty("speeds", { drive.desiredSpeeds.toString() }, null)
     builder.addDoubleProperty("skew constant", { skewConstant }, { k: Double -> skewConstant = k })
   }
-
-}
+}

src/main/kotlin/frc/team449/robot2023/auto/routines/DoNothing.kt

@@ -13,7 +13,7 @@ class DoNothing(
 
   override val routine =
     ChoreoRoutine(
-      drive = robot.drive,
+      drive = robot.drive
     )
 
   override val trajectory: MutableList<ChoreoTrajectory> = mutableListOf()

src/main/kotlin/frc/team449/robot2023/commands/driveAlign/ProfiledPoseAlign.kt

@@ -105,8 +105,10 @@ class ProfiledPoseAlign(
 
     return xPID.atSetpoint() && yPID.atSetpoint() && headingPID.atSetpoint() &&
       xProfile.isFinished(currTime) && yProfile.isFinished(currTime) &&
-      hypot(drive.currentSpeeds.vxMetersPerSecond,
-        drive.currentSpeeds.vyMetersPerSecond) < speedTol &&
+      hypot(
+        drive.currentSpeeds.vxMetersPerSecond,
+        drive.currentSpeeds.vyMetersPerSecond
+      ) < speedTol &&
       drive.currentSpeeds.omegaRadiansPerSecond < speedTolRot
   }
 

src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt

@@ -30,7 +30,7 @@ object RobotConstants {
   val ORTHOGONAL_CONTROLLER = PIDController(
     3.0,
     0.0,
-    0.0,
+    0.0
   )
 
   const val ALIGN_ROT_SPEED = 3 * PI / 2