Added tests, changed appliedVoltage to voltageOut, bugfixes

src/main/deploy/pathplanner/paths/RandomStuff.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 4.696339831588324,
-        "y": 2.0742350381497916
+        "x": 3.4481143673911125,
+        "y": 2.913681071372791
       },
       "prevControl": {
-        "x": 4.267859354224047,
-        "y": 1.09067757874543
+        "x": 3.019633890026836,
+        "y": 1.9301236119684295
       },
       "nextControl": null,
       "isLocked": false,

src/main/kotlin/frc/chargers/hardware/motorcontrol/ChargerSpark.kt

@@ -104,7 +104,7 @@ open class ChargerSpark<BaseMotorType: CANSparkBase>(
             get() = absoluteEncoder.position.ofUnit(rotations)
     }
 
-    override var appliedVoltage: Voltage
+    override var voltageOut: Voltage
         get() = Voltage(base.appliedOutput * base.busVoltage)
         set(voltage){
             base.setVoltage(voltage.inUnit(volts))

src/main/kotlin/frc/chargers/hardware/motorcontrol/ChargerTalonFX.kt

@@ -81,11 +81,11 @@ class ChargerTalonFX(
             get() = velocitySignal.refresh(true).value.ofUnit(rotations/seconds)
     }
 
-    override var appliedVoltage: Voltage
+    override var voltageOut: Voltage
         get() = voltageSignal.refresh(true).value.ofUnit(volts)
         set(value){
             base.setVoltage(value.siValue)
-            nonTalonFXFollowers.forEach{ it.appliedVoltage = value }
+            nonTalonFXFollowers.forEach{ it.voltageOut = value }
         }
 
     override val statorCurrent: Current

src/main/kotlin/frc/chargers/hardware/motorcontrol/Motor.kt

@@ -24,7 +24,7 @@ interface Motor {
      * A property that either fetches the current applied voltage,
      * or sets its value.
      */
-    var appliedVoltage: Voltage
+    var voltageOut: Voltage
 
     /**
      * The motor's stator current.
@@ -78,13 +78,13 @@ interface Motor {
 
 
     var speed: Double
-        get() = (appliedVoltage / 12.volts).siValue
+        get() = (voltageOut / 12.volts).siValue
         set(value) {
-            appliedVoltage = value * 12.volts
+            voltageOut = value * 12.volts
         }
 
     /**
      * Stops the motor.
      */
-    fun stop() { appliedVoltage = 0.volts }
+    fun stop() { voltageOut = 0.volts }
 }

src/main/kotlin/frc/chargers/hardware/motorcontrol/simulation/ArmMotorSim.kt

@@ -51,7 +51,7 @@ class ArmMotorSim(
             get() = base.velocityRadPerSec.ofUnit(radians / seconds)
     }
 
-    override var appliedVoltage: Voltage = 0.volts
+    override var voltageOut: Voltage = 0.volts
         set(value) {
             field = value
             base.setInputVoltage(value.siValue * if (super.inverted) -1.0 else 1.0)

src/main/kotlin/frc/chargers/hardware/motorcontrol/simulation/ElevatorMotorSim.kt

@@ -50,7 +50,7 @@ class ElevatorMotorSim(
             get() = base.velocityMetersPerSecond.ofUnit(meters / seconds) / 1.meters
     }
 
-    override var appliedVoltage: Voltage = 0.volts
+    override var voltageOut: Voltage = 0.volts
         set(value) {
             field = value
             base.setInputVoltage(value.siValue * if (inverted) -1.0 else 1.0)

src/main/kotlin/frc/chargers/hardware/motorcontrol/simulation/MotorSim.kt

@@ -40,7 +40,7 @@ class MotorSim(
             get() = base.angularVelocityRadPerSec.ofUnit(radians / seconds)
     }
 
-    override var appliedVoltage: Voltage = 0.volts
+    override var voltageOut: Voltage = 0.volts
         set(value) {
             field = value
             base.setInputVoltage(value.siValue * if (super.inverted) -1.0 else 1.0)

src/main/kotlin/frc/chargers/hardware/motorcontrol/simulation/SimulatedMotorBase.kt

@@ -28,13 +28,13 @@ abstract class SimulatedMotorBase: Motor {
         var encoderReading = this.encoder.angularPosition
         if (positionController.isContinuousInputEnabled) encoderReading %= 360.degrees
         val pidOutput = positionController.calculate(encoderReading.inUnit(radians), position.inUnit(radians))
-        this.appliedVoltage = Voltage(pidOutput) + feedforward
+        this.voltageOut = Voltage(pidOutput) + feedforward
     }
 
     override fun setVelocitySetpoint(velocity: AngularVelocity, feedforward: Voltage) {
         require(velocityPIDConfigured){" You must specify a velocityPID value using the configure(velocityPID = PIDConstants(p,i,d)) method. "}
         val pidOutput = velocityController.calculate(encoder.angularVelocity.siValue, velocity.siValue)
-        this.appliedVoltage = Voltage(pidOutput) + feedforward
+        this.voltageOut = Voltage(pidOutput) + feedforward
     }
 
     override fun configure(

src/main/kotlin/frc/chargers/hardware/subsystems/differentialdrive/EncoderDifferentialDrivetrain.kt

@@ -275,8 +275,8 @@ open class EncoderDifferentialDrivetrain(
         ) { log("DriveSysIDState", it.toString()) },
         SysIdRoutine.Mechanism(
             { voltage ->
-                leftMotors.forEach{ it.appliedVoltage = voltage.toKmeasure() }
-                rightMotors.forEach{ it.appliedVoltage = voltage.toKmeasure() }
+                leftMotors.forEach{ it.voltageOut = voltage.toKmeasure() }
+                rightMotors.forEach{ it.voltageOut = voltage.toKmeasure() }
             },
             null, // no need for log consumer since data is recorded by logging
             this

src/main/kotlin/frc/chargers/hardware/subsystems/swervedrive/EncoderHolonomicDrivetrain.kt

@@ -57,7 +57,7 @@ open class EncoderHolonomicDrivetrain(
     turnEncoders: List<PositionEncoder?> = List(4){ null },
     driveMotors: List<Motor>,
     val constants: SwerveConstants,
-    private val gyro: HeadingProvider? = null
+    val gyro: HeadingProvider? = null
 ): PoseEstimatingDrivetrain() {
     init {
         ensureFour("turn motor", turnMotors)
@@ -115,12 +115,11 @@ open class EncoderHolonomicDrivetrain(
     private val velocityPID by tunable(constants.velocityPID, "$name/velocityPID")
         .onChange{ pid -> driveMotors.forEach{ it.configure(velocityPID = pid) } }
 
-    private val allianceFieldRelativeOffset by lazy {
-        when (DriverStation.getAlliance().getOrNull()){
+    private val allianceFieldRelativeOffset get() =
+        when (DriverStation.getAlliance().get()){
             DriverStation.Alliance.Red -> 180.degrees
             else -> 0.degrees
         }
-    }
     private val defaultFieldRelative = RobotBase.isSimulation() || gyro != null
 
     private fun updatePoseEstimation() {
@@ -307,13 +306,13 @@ open class EncoderHolonomicDrivetrain(
 
     /* Drive Functions */
     private fun setSpeeds(xVel: Double, yVel: Double, rotVel: Double, fieldRelative: Boolean) {
-        if (fieldRelative) {
-            goal = ChassisSpeeds.fromFieldRelativeSpeeds(
+        goal = if (fieldRelative) {
+            ChassisSpeeds.fromFieldRelativeSpeeds(
                 xVel, yVel, rotVel,
                 Rotation2d((gyro?.heading ?: calculatedHeading) + allianceFieldRelativeOffset)
             )
         }else{
-            goal = ChassisSpeeds(xVel, yVel, rotVel)
+            ChassisSpeeds(xVel, yVel, rotVel)
         }
         log("$name/ChassisSpeeds/GoalWithoutModifiers", goal)
     }

src/main/kotlin/frc/chargers/hardware/subsystems/swervedrive/SwerveModule.kt

@@ -32,10 +32,17 @@ class SwerveModule(
     private var couplingOffset = 0.degrees
     private var azimuthProfileState = AngularMotionProfileState(startingDirection ?: turnMotor.encoder.angularPosition)
 
-    val direction: Angle get() = turnMotor.encoder.angularPosition % 360.degrees
-    val driveAngularVelocity: AngularVelocity get() = driveMotor.encoder.angularVelocity
-    val driveLinearVelocity: Velocity get() = driveAngularVelocity * wheelRadius
-    val wheelTravel: Distance get() = driveMotor.encoder.angularPosition * wheelRadius
+    val direction
+        get() = (turnMotor.encoder.angularPosition + couplingOffset) % 360.degrees
+
+    val driveAngularVelocity
+        get() = driveMotor.encoder.angularVelocity
+
+    val driveLinearVelocity
+        get() = driveAngularVelocity * wheelRadius
+
+    val wheelTravel
+        get() = driveMotor.encoder.angularPosition * wheelRadius
 
     init {
         turnMotor.configure(
@@ -61,6 +68,7 @@ class SwerveModule(
                 couplingOffset -= constants.couplingRatio * (direction % 360.degrees - 180.degrees)
                 log("CouplingOffset", couplingOffset)
             }
+            if (turnEncoder != null) log("$name/absolutePosition", turnEncoder.angularPosition)
             log("$name/Direction", direction)
             log("$name/DriveLinearVelocity", driveLinearVelocity)
             log("$name/WheelTravel", wheelTravel)
@@ -87,13 +95,13 @@ class SwerveModule(
         )
 
     fun setDriveVoltage(target: Voltage) {
-        driveMotor.appliedVoltage = target
+        driveMotor.voltageOut = target
         log("$name/DriveVoltage", target)
     }
 
     fun setTurnVoltage(target: Voltage) {
         val trueVoltage = if (abs(target) < 0.01.volts) 0.volts else target
-        turnMotor.appliedVoltage = trueVoltage
+        turnMotor.voltageOut = trueVoltage
         log("$name/TurnVoltage", trueVoltage)
     }
 

src/main/kotlin/frc/robot/TestRobots.kt

@@ -0,0 +1,71 @@
+package frc.robot
+
+import com.batterystaple.kmeasure.units.degrees
+import com.batterystaple.kmeasure.units.inches
+import com.batterystaple.kmeasure.units.radians
+import com.batterystaple.kmeasure.units.volts
+import com.pathplanner.lib.util.PIDConstants
+import frc.chargers.controls.feedforward.AngularMotorFeedforward
+import frc.chargers.framework.ChargerRobot
+import frc.chargers.framework.HorseLog.log
+import frc.chargers.hardware.motorcontrol.ChargerSparkMax
+import frc.chargers.hardware.motorcontrol.ChargerTalonFX
+import frc.chargers.hardware.motorcontrol.Motor
+import frc.chargers.hardware.sensors.encoders.ChargerCANcoder
+import frc.chargers.hardware.subsystems.swervedrive.ModuleType
+import frc.chargers.hardware.subsystems.swervedrive.SwerveConstants
+import frc.chargers.hardware.subsystems.swervedrive.SwerveModule
+
+class MotorTestingBot(private val motor: Motor): ChargerRobot() {
+    init {
+        motor.configure(
+            gearRatio = 1.0, // Change this
+            positionPID = PIDConstants(0.3, 0.0, 0.001),
+            startingPosition = 0.degrees
+        )
+    }
+
+    override fun robotPeriodic() {
+        log("MotorPosition", motor.encoder.angularPosition)
+    }
+
+    override fun testPeriodic() {
+        motor.setPositionSetpoint(90.degrees)
+    }
+
+    override fun autonomousPeriodic() {
+        motor.voltageOut = 5.volts
+    }
+
+    override fun disabledInit() {
+        motor.stop()
+    }
+}
+
+class SwerveModuleTestingBot: ChargerRobot() {
+    private val constants = SwerveConstants(
+        moduleType = ModuleType.MK4iL2,
+        trackWidth = 27.inches,
+        wheelBase = 27.inches,
+        azimuthPID = PIDConstants(7.0,0.0,0.0),
+        azimuthPIDTolerance = 1.degrees,
+        velocityPID = PIDConstants(0.05,0.0,0.0),
+        velocityFF = AngularMotorFeedforward(0.0,0.13),
+    )
+
+    private val module = SwerveModule(
+        "TestModule",
+        ChargerSparkMax(12),
+        ChargerCANcoder(44) - 0.621.radians,
+        ChargerTalonFX(10),
+        constants
+    )
+
+    override fun autonomousPeriodic() {
+        module.setDirection(90.degrees)
+    }
+
+    override fun testPeriodic() {
+        module.setDirection(-90.degrees)
+    }
+}

src/main/kotlin/frc/robot/rigatoni/AngleAimCommand.kt

@@ -39,12 +39,14 @@ class AngleAimCommand(
         getChassisPowers
     )
 
+    fun getHeading() = drivetrain.gyro?.heading ?: drivetrain.calculatedHeading
+
     private val pidController = PIDController(AIM_TO_ANGLE_PID)
     private val motionProfile: AngularMotionProfile = AngularTrapezoidProfile(
         drivetrain.maxRotationalVelocity,
         drivetrain.maxRotationalVelocity / 1.5.seconds
     )
-    private var setpoint = AngularMotionProfileState(drivetrain.heading)
+    private var setpoint = AngularMotionProfileState(getHeading())
     private var goal = AngularMotionProfileState()
 
     init {
@@ -56,7 +58,7 @@ class AngleAimCommand(
         goal.position = getTarget().flipWhenRedAlliance()
         setpoint = motionProfile.calculate(setpoint, goal)
         val swerveOutput = getChassisPowers()
-        swerveOutput.rotationPower = pidController.calculate(drivetrain.heading.siValue, setpoint.position.siValue)
+        swerveOutput.rotationPower = pidController.calculate(getHeading().siValue, setpoint.position.siValue)
         drivetrain.swerveDrive(swerveOutput, fieldRelative = true)
     }
 }

src/main/kotlin/frc/robot/rigatoni/CompetitionRobot.kt

@@ -22,6 +22,7 @@ import kcommand.commandbuilder.buildCommand
 import kcommand.setDefaultRunCommand
 import frc.chargers.framework.ChargerRobot
 import frc.chargers.framework.HorseLog
+import frc.chargers.framework.tunable
 import frc.chargers.hardware.sensors.imu.ChargerNavX
 import frc.chargers.utils.squareMagnitude
 import frc.chargers.wpilibextensions.distanceTo
@@ -54,6 +55,8 @@ class CompetitionRobot: ChargerRobot() {
 
     private val autoChooser = SendableChooser<Command>()
 
+    val testTunable by tunable(2.0, "SomeValue").onChange { println("Hi!") }
+
     init {
         DriverStation.silenceJoystickConnectionWarning(true)
         gyro.simHeadingSource = { drivetrain.calculatedHeading }

src/main/kotlin/frc/robot/rigatoni/subsystems/Climber.kt

@@ -51,20 +51,20 @@ class Climber: SubsystemBase() {
 
     fun moveLeftHook(speed: Double){
         if (willSurpassLimit(speed, leftPosition)){
-            leftMotor.appliedVoltage = 0.volts
+            leftMotor.voltageOut = 0.volts
             log("Climber/LeftRequest", 0.0)
         }else{
-            leftMotor.appliedVoltage = speed * MAX_VOLTAGE
+            leftMotor.voltageOut = speed * MAX_VOLTAGE
             log("Climber/LeftRequest", speed)
         }
     }
 
     fun moveRightHook(speed: Double){
         if (willSurpassLimit(speed, rightPosition)){
-            rightMotor.appliedVoltage = 0.volts
+            rightMotor.voltageOut = 0.volts
             log("Climber/RightRequest", 0.0)
         }else{
-            rightMotor.appliedVoltage = speed * MAX_VOLTAGE
+            rightMotor.voltageOut = speed * MAX_VOLTAGE
             log("Climber/RightRequest", speed)
         }
     }
@@ -75,7 +75,7 @@ class Climber: SubsystemBase() {
     }
 
     override fun periodic() {
-        log("Climber/LeftV", leftMotor.appliedVoltage)
-        log("Climber/RightV", rightMotor.appliedVoltage)
+        log("Climber/LeftV", leftMotor.voltageOut)
+        log("Climber/RightV", rightMotor.voltageOut)
     }
 }

src/main/kotlin/frc/robot/rigatoni/subsystems/GroundIntakeSerializer.kt

@@ -47,12 +47,12 @@ class GroundIntakeSerializer: SubsystemBase() {
     private var requestedVoltages by logged(mutableListOf(0.volts, 0.volts))
 
     fun setIntakeVoltage(voltage: Voltage){
-        groundIntakeMotor.appliedVoltage = voltage
+        groundIntakeMotor.voltageOut = voltage
         requestedVoltages[0] = voltage
     }
 
     fun setConveyorVoltage(voltage: Voltage){
-        serializerMotor?.appliedVoltage = voltage
+        serializerMotor?.voltageOut = voltage
         requestedVoltages[1] = voltage
     }
 
@@ -86,7 +86,7 @@ class GroundIntakeSerializer: SubsystemBase() {
             setIdle()
         }
         log("GroundIntakeSerializer/StatorCurrentReadings", listOfNotNull(groundIntakeMotor.statorCurrent, serializerMotor?.statorCurrent))
-        log("GroundIntakeSerializer/VoltageReadings", listOfNotNull(groundIntakeMotor.appliedVoltage, serializerMotor?.appliedVoltage))
+        log("GroundIntakeSerializer/VoltageReadings", listOfNotNull(groundIntakeMotor.voltageOut, serializerMotor?.voltageOut))
         log("GroundIntakeSerializer/AngularVelocityReadings", listOfNotNull(groundIntakeMotor.encoder.angularVelocity, serializerMotor?.encoder?.angularVelocity))
     }
 }