add pivot follower motor

blair-robot-project · Jan 18, 2025 · 6a00f71 · 6a00f71
1 parent ecbdd60
commit 6a00f71
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 4 deletions.
diff --git a/src/main/kotlin/frc/team449/subsystems/elevator/Elevator.kt b/src/main/kotlin/frc/team449/subsystems/elevator/Elevator.kt
@@ -142,7 +142,7 @@ open class Elevator(
       if (!status1.isOK) println("Error applying configs to Elevator Lead Motor -> Error Code: $status1")
 
       val status2 = followerMotor.configurator.apply(config)
-      if (!status2.isOK) println("Error applying configs to Elevator Follower Motor -> Error Code: $status1")
+      if (!status2.isOK) println("Error applying configs to Elevator Follower Motor -> Error Code: $status2")
 
       BaseStatusSignal.setUpdateFrequencyForAll(
         ElevatorConstants.VALUE_UPDATE_RATE,

diff --git a/src/main/kotlin/frc/team449/subsystems/pivot/Pivot.kt b/src/main/kotlin/frc/team449/subsystems/pivot/Pivot.kt
@@ -2,6 +2,7 @@ package frc.team449.subsystems.pivot
 
 import com.ctre.phoenix6.BaseStatusSignal
 import com.ctre.phoenix6.configs.TalonFXConfiguration
+import com.ctre.phoenix6.controls.Follower
 import com.ctre.phoenix6.controls.MotionMagicVoltage
 import com.ctre.phoenix6.hardware.TalonFX
 import edu.wpi.first.units.Units.Radians
@@ -45,6 +46,7 @@ class Pivot(
       motor.setControl(
         request
           .withPosition(position)
+          .withUpdateFreqHz(PivotConstants.REQUEST_UPDATE_RATE)
           .withFeedForward(pivotFeedForward.calculateWithLength(motor.closedLoopReference.valueAsDouble, motor.closedLoopReferenceSlope.valueAsDouble))
       )
     }.until(::atSetpoint)
@@ -86,7 +88,9 @@ class Pivot(
 
   companion object {
     fun createPivot(): Pivot {
-      val leadMotor = TalonFX(PivotConstants.MOTOR_ID)
+      val leadMotor = TalonFX(PivotConstants.LEAD_MOTOR_ID)
+      val followerMotor = TalonFX(PivotConstants.FOLLOWER_MOTOR_ID)
+
       val config = TalonFXConfiguration()
 
       config.MotorOutput.Inverted = PivotConstants.INVERTED
@@ -112,7 +116,10 @@ class Pivot(
       config.MotionMagic.MotionMagicAcceleration = PivotConstants.MAX_ACCEL
 
       val status1 = leadMotor.configurator.apply(config)
-      if (!status1.isOK) println("Error applying configs to Pivot Motor -> Error Code: $status1")
+      if (!status1.isOK) println("Error applying configs to Elevator Lead Motor -> Error Code: $status1")
+
+      val status2 = followerMotor.configurator.apply(config)
+      if (!status2.isOK) println("Error applying configs to Elevator Follower Motor -> Error Code: $status2")
 
       BaseStatusSignal.setUpdateFrequencyForAll(
         PivotConstants.VALUE_UPDATE_RATE,
@@ -124,6 +131,24 @@ class Pivot(
         leadMotor.deviceTemp
       )
 
+      leadMotor.optimizeBusUtilization()
+
+      BaseStatusSignal.setUpdateFrequencyForAll(
+        PivotConstants.VALUE_UPDATE_RATE,
+        followerMotor.position,
+        followerMotor.velocity,
+        followerMotor.motorVoltage,
+        followerMotor.supplyCurrent,
+        followerMotor.statorCurrent,
+        followerMotor.deviceTemp
+      )
+
+      followerMotor.optimizeBusUtilization()
+
+      followerMotor.setControl(
+        Follower(PivotConstants.LEAD_MOTOR_ID, PivotConstants.FOLLOWER_INVERTED_TO_MASTER)
+      )
+
       return Pivot(leadMotor)
     }
   }

diff --git a/src/main/kotlin/frc/team449/subsystems/pivot/PivotConstants.kt b/src/main/kotlin/frc/team449/subsystems/pivot/PivotConstants.kt
@@ -7,7 +7,8 @@ import edu.wpi.first.units.measure.Frequency
 import kotlin.math.PI
 
 object PivotConstants {
-  const val MOTOR_ID = 2 // TODO(Change motor ID.)
+  const val LEAD_MOTOR_ID = 2 // TODO(Change motor ID.)
+  const val FOLLOWER_MOTOR_ID = 33
 
   // TODO(Adjust gearing and UPR.)
   const val GEARING = 1.0 / 75.0
@@ -20,12 +21,14 @@ object PivotConstants {
   const val MAX_ANGLE = 5 * PI / 9
 
   val INVERTED = InvertedValue.CounterClockwise_Positive
+  const val FOLLOWER_INVERTED_TO_MASTER = true
   val BRAKE_MODE = NeutralModeValue.Brake
 
   const val STATOR_LIM = 80.0
   const val SUPPLY_LIM = 40.0
 
   val VALUE_UPDATE_RATE: Frequency = Hertz.of(50.0)
+  val REQUEST_UPDATE_RATE: Frequency = Hertz.of(100.0)
 
   const val TOLERANCE = 0.05 // TODO(Adjust tolerance.)