finish phoenix6 conversion

build.gradle

@@ -83,7 +83,7 @@ dependencies {
 	simulationRelease wpi.sim.enableRelease()
 
 	implementation 'org.jetbrains.kotlin:kotlin-test-junit5'
-	implementation 'com.github.team4099:FalconUtils:1.1.24'
+	implementation 'com.github.team4099:FalconUtils:1.1.26'
 	implementation 'org.apache.commons:commons-collections4:4.0'
 	implementation 'com.google.code.gson:gson:2.10.1'
 	implementation "io.javalin:javalin:5.3.2"

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,32 +1,23 @@
 package com.team4099.robot2023
 
-import com.ctre.phoenix.motorcontrol.can.TalonFX
 import com.team4099.robot2023.auto.AutonomousSelector
 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
@@ -35,15 +26,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
@@ -63,7 +52,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(
@@ -122,7 +111,6 @@ object RobotContainer {
         drivetrain
       )
 
-
     /*
     module steeing tuning
 
@@ -228,11 +216,13 @@ 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.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
+    // ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
     //    ControlBoard.dpadDown.whileTrue(PickupFromSubstationCommand(drivetrain, superstructure))
 
     //    ControlBoard.doubleSubstationIntake.whileTrue(AutoScoreCommand(drivetrain,

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/drivetrain/DrivePathCommand.kt

@@ -32,10 +32,10 @@ import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.cos
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree
+import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond
 import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds
-import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreesPerSecond
 import org.team4099.lib.units.derived.inMetersPerSecondPerMeter
-import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSecond
+import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds
 import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond
 import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond
@@ -96,7 +96,7 @@ class DrivePathCommand(
       "Pathfollow/thetakD",
       DrivetrainConstants.PID.AUTO_THETA_PID_KD,
       Pair(
-        { it.inDegreesPerSecondPerDegreesPerSecond },
+        { it.inDegreesPerSecondPerDegreePerSecond },
         { it.degrees.perSecond.perDegreePerSecond }
       )
     )
@@ -116,7 +116,7 @@ class DrivePathCommand(
     LoggedTunableValue(
       "Pathfollow/poskI",
       DrivetrainConstants.PID.AUTO_POS_KI,
-      Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds })
+      Pair({ it.inMetersPerSecondPerMeterSeconds }, { it.meters.perSecond.perMeterSeconds })
     )
   val poskD =
     LoggedTunableValue(

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

@@ -15,8 +15,8 @@ import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree
+import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond
 import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds
-import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreesPerSecond
 import org.team4099.lib.units.derived.perDegree
 import org.team4099.lib.units.derived.perDegreePerSecond
 import org.team4099.lib.units.derived.perDegreeSeconds
@@ -44,7 +44,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() {
     LoggedTunableValue(
       "Pathfollow/thetakD",
       Pair(
-        { it.inDegreesPerSecondPerDegreesPerSecond },
+        { it.inDegreesPerSecondPerDegreePerSecond },
         { it.degrees.perSecond.perDegreePerSecond }
       )
     )

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/ElevatorConstants.kt

@@ -14,7 +14,6 @@ import org.team4099.lib.units.base.pounds
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.cos
 import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.gearRatio
 import org.team4099.lib.units.derived.sin
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.perSecond
@@ -23,7 +22,7 @@ import kotlin.math.PI
 object ElevatorConstants {
 
   const val SENSOR_CPR = 42
-  val GEAR_RATIO = ((58.0 / 14.0) * (84.0 / 58.0) * (28.0 / 84.0)).gearRatio
+  val GEAR_RATIO = ((58.0 / 14.0) * (84.0 / 58.0) * (28.0 / 84.0))
   val CARRIAGE_MASS = 10.pounds
 
   const val FOLLOW_MOTOR_INVERTED = true
@@ -72,7 +71,6 @@ object ElevatorConstants {
 
   val ENABLE_ELEVATOR = 1.0
 
-
   val ELEVATOR_MAX_EXTENSION = 54.8.inches
   val ELEVATOR_MAX_RETRACTION = 0.0.inches
   val ELEVATOR_SOFT_LIMIT_EXTENSION = 54.5.inches

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

@@ -84,15 +84,7 @@ object FieldConstants {
           Rotation3d()
         )
       ),
-      AprilTag(
-        8,
-        Pose3d(
-          (0).inches,
-          (0).inches,
-          (0).inches,
-          Rotation3d()
-        )
-      )
+      AprilTag(8, Pose3d((0).inches, (0).inches, (0).inches, Rotation3d()))
     )
 
   //  val homeAprilTags: List<AprilTag> =
@@ -156,7 +148,7 @@ object FieldConstants {
   // Rotation3d(0.0.degrees, 0.0.degrees, 180.degrees)))
   //    )
 
-  //church tags
+  // church tags
   val aprilTags: List<AprilTag> =
     listOf(
       AprilTag(
@@ -168,52 +160,47 @@ object FieldConstants {
           Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians)
         )
       ),
-
       AprilTag(
         1,
         Pose3d(
           0.inches,
-          62.25.inches + (3*4.25).inches,
+          62.25.inches + (3 * 4.25).inches,
           25.25.inches + 5.5.inches,
           Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians)
         )
       ),
-
       AprilTag(
         2,
         Pose3d(
           0.inches,
-          94.35.inches + (5*4.25).inches,
+          94.35.inches + (5 * 4.25).inches,
           25.inches + 5.5.inches,
           Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians)
         )
       ),
-
       AprilTag(
         3,
         Pose3d(
           0.inches,
           160.inches,
-          135.60.inches + (7*4.25).inches,
+          135.60.inches + (7 * 4.25).inches,
           Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians)
         )
       ),
-
       AprilTag(
         4,
         Pose3d(
           0.inches,
-          168.1.inches + (9*4.25).inches,
+          168.1.inches + (9 * 4.25).inches,
           24.875.inches + 5.5.inches,
           Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians)
         )
       ),
-
       AprilTag(
         5,
         Pose3d(
           0.inches,
-          199.35.inches + (11*4.25).inches,
+          199.35.inches + (11 * 4.25).inches,
           25.125.inches + 5.5.inches,
           Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians)
         )

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

@@ -8,9 +8,6 @@ import org.team4099.lib.units.base.pounds
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.driven
-import org.team4099.lib.units.derived.driving
-import org.team4099.lib.units.derived.gearRatio
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.kilo
@@ -44,17 +41,15 @@ object GroundIntakeConstants {
 
   val ABSOLUTE_ENCODER_OFFSET = (-196.87).degrees
   // From encoder to intake
-  val ROLLER_GEAR_RATIO = (36.0.driven / 18.0.driving).gearRatio
+  val ROLLER_GEAR_RATIO = (36.0 / 18.0)
 
   // units are kg * m^2'
   val ROLLER_MOMENT_INERTIA = 0.00313.kilo.grams * 1.0.meters.squared
 
   // gear reduction from absolute encoder to output
-  val ARM_ENCODER_GEAR_RATIO = (32.0.driven / 16.0.driving).gearRatio
+  val ARM_ENCODER_GEAR_RATIO = (32.0 / 16.0)
   // gear reduction from motor to output
-  val ARM_OUTPUT_GEAR_RATIO =
-    ((60.0.driven / 12.0.driving) * (80.0.driven / 18.0.driving) * (32.0.driven / 16.0.driving))
-      .gearRatio
+  val ARM_OUTPUT_GEAR_RATIO = ((60.0 / 12.0) * (80.0 / 18.0) * (32.0 / 16.0))
 
   val ARM_LENGTH = 12.695.inches // TODO figure out via cad (this should be distance to com)
 

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

@@ -1,7 +1,5 @@
 package com.team4099.robot2023.config.constants
 
-import com.ctre.phoenix.led.Animation
-import com.ctre.phoenix.led.StrobeAnimation
 import edu.wpi.first.wpilibj.util.Color
 import edu.wpi.first.wpilibj.util.Color8Bit
 import org.team4099.lib.drivers.BlinkinLedDriver as Blinkin
@@ -35,26 +33,6 @@ class LedConstants {
     MOVEMENT(Blinkin.BlinkinLedMode.FIXED_STROBE_WHITE)
   }
 
-  enum class CandleMode(
-    val animation: Animation?,
-    val r: Int,
-    val g: Int,
-    val b: Int,
-    val address: Pair<Int, Int>?
-  ) {
-    IDLE(null, 255, 0, 0, null),
-    OUTTAKE(null, 0, 0, 255, null),
-    INTAKE(null, 64, 255, 51, null),
-    AUTO(null, 255, 215, 0, null),
-    TELEOP(null, 255, 255, 255, null),
-    CUBE(null, 162, 25, 255, null),
-    CONE(null, 0, 0, 0, null),
-    SINGLE_SUBSTATION(StrobeAnimation(255, 255, 255), 0, 0, 0, Pair(0, 25)),
-    DOUBLE_SUBSTATION(StrobeAnimation(255, 255, 255), 0, 0, 0, Pair(100, 25)),
-    SCORE(StrobeAnimation(255, 255, 255), 0, 0, 0, Pair(50, 25)),
-    MOVEMENT(StrobeAnimation(255, 255, 255), 0, 0, 0, null)
-  }
-
   enum class SimMode(val color: Color8Bit) {
     IDLE(Color8Bit(Color.kOrange))
   }

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

@@ -8,9 +8,6 @@ import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.pounds
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.ElectricalPotential
-import org.team4099.lib.units.derived.driven
-import org.team4099.lib.units.derived.driving
-import org.team4099.lib.units.derived.gearRatio
 import org.team4099.lib.units.derived.rotations
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.kilo
@@ -66,8 +63,8 @@ object ManipulatorConstants {
 
   const val SENSOR_CPR = 42.0
 
-  val ARM_GEAR_RATIO = ((84.0.driven / 29.0.driving) * (60.0.driven / 16.0.driving)).gearRatio
-  val ROLLER_GEAR_RATIO = 20.0.gearRatio
+  val ARM_GEAR_RATIO = ((84.0 / 29.0) * (60.0 / 16.0))
+  val ROLLER_GEAR_RATIO = 20.0
 
   // TODO: Change current thresholds
   val CONE_CURRENT_THRESHOLD = 25.amps