tuned waypoints p1

src/main/kotlin/com/team4099/robot2023/Robot.kt

@@ -11,11 +11,9 @@ import com.team4099.robot2023.util.Alert.AlertType
 import com.team4099.robot2023.util.FMSData
 import com.team4099.robot2023.util.NTSafePublisher
 import edu.wpi.first.hal.AllianceStationID
-import edu.wpi.first.wpilibj.AnalogInput
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.PowerDistribution
 import edu.wpi.first.wpilibj.RobotBase
-import edu.wpi.first.wpilibj.RobotController
 import edu.wpi.first.wpilibj.livewindow.LiveWindow
 import edu.wpi.first.wpilibj.simulation.DriverStationSim
 import edu.wpi.first.wpilibj2.command.Command
@@ -135,8 +133,6 @@ object Robot : LoggedRobot() {
     CommandScheduler.getInstance().onCommandInterrupt { command: Command ->
       Logger.getInstance().recordOutput("/ActiveCommands/${command.name}", false)
     }
-
-
   }
 
   override fun disabledExit() {

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -1,33 +1,24 @@
 package com.team4099.robot2023
 
-import com.ctre.phoenix.motorcontrol.can.TalonFX
 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
-import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
-import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule
-import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIO
-import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOFalcon
 import com.team4099.robot2023.subsystems.elevator.Elevator
-import com.team4099.robot2023.subsystems.elevator.ElevatorIO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIONeo
 import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
 import com.team4099.robot2023.subsystems.gameboy.GameBoy
 import com.team4099.robot2023.subsystems.gameboy.GameboyIOServer
 import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode
 import com.team4099.robot2023.subsystems.groundintake.GroundIntake
-import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIO
 import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIONeo
 import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIOSim
 import com.team4099.robot2023.subsystems.led.Led
@@ -36,15 +27,13 @@ import com.team4099.robot2023.subsystems.led.LedIOSim
 import com.team4099.robot2023.subsystems.limelight.LimelightVision
 import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
 import com.team4099.robot2023.subsystems.manipulator.Manipulator
-import com.team4099.robot2023.subsystems.manipulator.ManipulatorIO
 import com.team4099.robot2023.subsystems.manipulator.ManipulatorIONeo
 import com.team4099.robot2023.subsystems.manipulator.ManipulatorIOSim
 import com.team4099.robot2023.subsystems.superstructure.Request
 import com.team4099.robot2023.subsystems.superstructure.Superstructure
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
 import com.team4099.robot2023.util.driver.Ryan
-import edu.wpi.first.wpilibj.AnalogInput
 import edu.wpi.first.wpilibj.RobotBase
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.smoothDeadband
@@ -64,7 +53,7 @@ object RobotContainer {
   init {
     if (RobotBase.isReal()) {
       // Real Hardware Implementations
-      //drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {}
+      // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {}
       drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal)
       vision =
         Vision(
@@ -124,16 +113,14 @@ object RobotContainer {
         drivetrain
       )
 
+    /*
 
-  /*
-
-    drivetrain.defaultCommand =
-      SwerveModuleTuningCommand(
-        drivetrain,
-        { (ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) * 180).degrees },
-      )
-  */
-
+      drivetrain.defaultCommand =
+        SwerveModuleTuningCommand(
+          drivetrain,
+          { (ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) * 180).degrees },
+        )
+    */
   }
 
   fun requestSuperstructureIdle() {
@@ -229,11 +216,15 @@ object RobotContainer {
     )
 
     ControlBoard.groundIntakeCone.whileTrue(superstructure.groundIntakeConeCommand())
-    ControlBoard.autoScore.whileTrue(AutoScoreCommand(drivetrain, superstructure)
-      .finallyDo {  Logger.getInstance().recordOutput("Auto/isAutoDriving", false) }
+    ControlBoard.autoScore.whileTrue(
+      AutoScoreCommand(drivetrain, superstructure).finallyDo {
+        Logger.getInstance().recordOutput("Auto/isAutoDriving", false)
+      }
     )
-    ControlBoard.autoIntake.whileTrue(AutoIntakeCommand(drivetrain, superstructure)
-      .finallyDo {  Logger.getInstance().recordOutput("Auto/isAutoDriving", false) }
+    ControlBoard.autoIntake.whileTrue(
+      AutoIntakeCommand(drivetrain, superstructure).finallyDo {
+        Logger.getInstance().recordOutput("Auto/isAutoDriving", false)
+      }
     )
 
     ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())

src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt

@@ -90,8 +90,6 @@ object AutonomousSelector {
 
     autonomousModeChooser.addOption("Score Preload Cone", AutonomousMode.PRELOAD_SCORE_AUTO)
 
-
-
     autoTab.add("Mode", autonomousModeChooser.sendableChooser).withSize(4, 2).withPosition(2, 0)
 
     autoEngageWidgit =

src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt

@@ -88,5 +88,4 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr
       superstructure.groundIntakeConeCommand()
     )
   }
-
 }

src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt

@@ -2,8 +2,6 @@ package com.team4099.robot2023.commands
 
 import com.team4099.lib.math.Zone2d
 import com.team4099.lib.trajectory.Waypoint
-import com.team4099.robot2023.RobotContainer
-import com.team4099.robot2023.commands.drivetrain.DriveBrakeModeCommand
 import com.team4099.robot2023.commands.drivetrain.DrivePathCommand
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.FieldConstants
@@ -15,15 +13,13 @@ import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode
 import com.team4099.robot2023.subsystems.superstructure.Superstructure
 import com.team4099.robot2023.util.AllianceFlipUtil
 import com.team4099.robot2023.util.FMSData
-import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.wpilibj2.command.Commands.runOnce
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.units.base.meters
 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.inRotation2ds
 import org.team4099.lib.units.perSecond
 
@@ -42,7 +38,6 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
 
     val setupCommand =
       runOnce({
-
         Logger.getInstance().recordOutput("Auto/isAutoDriving", true)
         drivePose = drivetrain.odometryPose
         postAlignPose =
@@ -57,15 +52,16 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
             )
           )
 
-
         finalPose =
           AllianceFlipUtil.apply(
             Pose2d(
-              2.16.meters, // slightly offset in the x
+              1.9.meters, // slightly offset in the x
               FieldConstants.Grids.nodeFirstY +
                 FieldConstants.Grids.nodeSeparationY *
-                (if (FMSData.isBlue) superstructure.objective.nodeColumn
-                else 8 - superstructure.objective.nodeColumn),
+                (
+                  if (FMSData.isBlue) superstructure.objective.nodeColumn
+                  else 8 - superstructure.objective.nodeColumn
+                  ),
               180.degrees
             )
           )
@@ -90,14 +86,10 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
       })
 
     val breakCommand =
-      runOnce({
-        drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) }
-      })
+      runOnce({ drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } })
 
     val coastCommand =
-      runOnce({
-        drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(false) }
-      })
+      runOnce({ drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(false) } })
 
     addCommands(
       setupCommand,
@@ -116,8 +108,7 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
             intermediaryWaypoints +
             listOf(
               Waypoint(
-                finalPose.translation.translation2d,
-                finalPose.rotation.inRotation2ds
+                finalPose.translation.translation2d, finalPose.rotation.inRotation2ds
               ),
               Waypoint(
                 postAlignPose.translation.translation2d,

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt

@@ -6,7 +6,8 @@ import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.perSecond
 
-class DriveBrakeModeCommand(val drivetrain: Drivetrain, val isBreakMode: Boolean = true) : CommandBase() {
+class DriveBrakeModeCommand(val drivetrain: Drivetrain, val isBreakMode: Boolean = true) :
+  CommandBase() {
   init {
     addRequirements(drivetrain)
   }

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt

@@ -4,8 +4,6 @@ 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,

src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt

@@ -1,6 +1,5 @@
 package com.team4099.robot2023.config.constants
 
-import edu.wpi.first.wpilibj.AnalogInput
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.milli
 
@@ -25,9 +24,6 @@ object Constants {
     val LOOP_PERIOD_TIME = 20.milli.seconds
     val POWER_DISTRIBUTION_HUB_ID = 1
 
-
-
-
     enum class GamePiece {
       CUBE,
       CONE,

src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt

@@ -77,10 +77,10 @@ object DrivetrainConstants {
   val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps
   val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds
 
-  val FRONT_LEFT_MODULE_ZERO = 1.3.radians + 180.degrees  + 180.degrees
-  val FRONT_RIGHT_MODULE_ZERO = 4.49.radians + 180.degrees  + 180.degrees
-  val BACK_LEFT_MODULE_ZERO = 3.22.radians + 180.degrees  + 180.degrees
-  val BACK_RIGHT_MODULE_ZERO = 0.99.radians - 180.degrees  + 180.degrees
+  val FRONT_LEFT_MODULE_ZERO = 1.3.radians + 180.degrees + 180.degrees
+  val FRONT_RIGHT_MODULE_ZERO = 4.49.radians + 180.degrees + 180.degrees
+  val BACK_LEFT_MODULE_ZERO = 3.22.radians + 180.degrees + 180.degrees
+  val BACK_RIGHT_MODULE_ZERO = 0.99.radians - 180.degrees + 180.degrees
 
   val STEERING_COMPENSATION_VOLTAGE = 10.volts
   val DRIVE_COMPENSATION_VOLTAGE = 12.volts
@@ -109,7 +109,8 @@ object DrivetrainConstants {
     val AUTO_POS_KDX: DerivativeGain<Meter, Velocity<Meter>>
       get() {
         if (RobotBase.isReal()) {
-          return (0.1.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond // todo: 0.25
+          return (0.1.meters.perSecond / (1.0.meters.perSecond))
+            .metersPerSecondPerMetersPerSecond // todo: 0.25
         } else {
           return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond
         }
@@ -135,7 +136,8 @@ object DrivetrainConstants {
     val AUTO_POS_KDY: DerivativeGain<Meter, Velocity<Meter>>
       get() {
         if (RobotBase.isReal()) {
-          return (0.025.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond // todo: 0.25
+          return (0.025.meters.perSecond / (1.0.meters.perSecond))
+            .metersPerSecondPerMetersPerSecond // todo: 0.25
         } else {
           return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond
         }