fixed inversion issue with krakenshooter and changed pivot to use CAN…

…SparkMax for access to internals
blair-robot-project · Sep 11, 2024 · 0896d32 · 0896d32
1 parent 5d7683e
commit 0896d32
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 24 deletions.
diff --git a/src/main/kotlin/frc/team449/robot2024/commands/PivotCalibration.kt b/src/main/kotlin/frc/team449/robot2024/commands/PivotCalibration.kt
@@ -15,7 +15,7 @@ class PivotCalibration(
   private var samples = mutableListOf<Double>()
 
   override fun execute() {
-    samples.add(pivot.motor.position)
+    samples.add(pivot.motor.encoder.position)
   }
 
   override fun isFinished(): Boolean {

diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterKrakenConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterKrakenConstants.kt
@@ -13,7 +13,7 @@ object SpinShooterKrakenConstants {
   val RIGHT_MOTOR_ORIENTATION = InvertedValue.CounterClockwise_Positive
   val RIGHT_NEUTRAL_MODE = NeutralModeValue.Coast
   const val LEFT_MOTOR_ID = 46
-  val LEFT_MOTOR_ORIENTATION = InvertedValue.CounterClockwise_Positive
+  val LEFT_MOTOR_ORIENTATION = InvertedValue.Clockwise_Positive
   val LEFT_NEUTRAL_MODE = NeutralModeValue.Coast
 
   const val UPDATE_FREQUENCY = 100.0

diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/pivot/Pivot.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/pivot/Pivot.kt
@@ -1,5 +1,7 @@
 package frc.team449.robot2024.subsystems.pivot
 
+import com.revrobotics.CANSparkLowLevel
+import com.revrobotics.CANSparkMax
 import edu.wpi.first.math.*
 import edu.wpi.first.math.controller.LinearPlantInversionFeedforward
 import edu.wpi.first.math.controller.LinearQuadraticRegulator
@@ -18,17 +20,13 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase
 import frc.team449.robot2024.constants.MotorConstants
 import frc.team449.robot2024.constants.RobotConstants
 import frc.team449.robot2024.constants.subsystem.PivotConstants
-import frc.team449.system.encoder.AbsoluteEncoder
 import frc.team449.system.encoder.QuadEncoder
-import frc.team449.system.motor.WrappedMotor
-import frc.team449.system.motor.createSparkMax
 import java.util.function.Supplier
-import kotlin.Pair
 import kotlin.math.abs
 import kotlin.math.pow
 
 open class Pivot(
-  val motor: WrappedMotor,
+  val motor: CANSparkMax,
   val encoder: QuadEncoder,
   private val controller: LinearQuadraticRegulator<N2, N1, N1>,
   private val fastController: LinearQuadraticRegulator<N2, N1, N1>,
@@ -55,6 +53,7 @@ open class Pivot(
   open val positionSupplier: Supplier<Double> =
     Supplier { encoder.position }
 
+
   open val velocitySupplier: Supplier<Double> =
     Supplier { encoder.velocity }
 
@@ -81,6 +80,15 @@ open class Pivot(
       0.0
     )
 
+    motor.inverted = PivotConstants.INVERTED
+    motor.setSmartCurrentLimit(PivotConstants.CURRENT_LIM)
+    val followerSpark = CANSparkMax(PivotConstants.FOLLOWER_ID, CANSparkLowLevel.MotorType.kBrushless)
+    followerSpark.restoreFactoryDefaults()
+    followerSpark.follow(motor, PivotConstants.FOLLOWER_INVERTED)
+    followerSpark.idleMode = motor.idleMode
+    followerSpark.setSmartCurrentLimit(PivotConstants.CURRENT_LIM)
+    followerSpark.burnFlash()
+
     this.defaultCommand = hold()
   }
 
@@ -374,15 +382,15 @@ open class Pivot(
 
   override fun initSendable(builder: SendableBuilder) {
     builder.publishConstString("1.0", "Motor Voltages")
-    builder.addDoubleProperty("1.1 Last Voltage", { motor.lastVoltage }, null)
+    builder.addDoubleProperty("1.1 Last Voltage", { motor.get() }, null)
     builder.publishConstString("2.0", "Position and Velocity")
     builder.addDoubleProperty("2.1 Current Position", { positionSupplier.get() }, null)
     builder.addDoubleProperty("2.2 Current Velocity", { velocitySupplier.get() }, null)
     builder.addDoubleProperty("2.3 Desired Position", { lastProfileReference.position }, null)
     builder.addDoubleProperty("2.4 Desired Velocity", { lastProfileReference.velocity }, null)
     builder.addDoubleProperty("2.5 Error", { lastProfileReference.position - positionSupplier.get() }, null)
-    builder.addDoubleProperty("2.6 Absolute Position", { motor.position }, null)
-    builder.addDoubleProperty("2.7 Absolute Velocity", { motor.velocity }, null)
+    builder.addDoubleProperty("2.6 Absolute Position", { motor.encoder.position }, null)
+    builder.addDoubleProperty("2.7 Absolute Velocity", { motor.encoder.velocity }, null)
     builder.addBooleanProperty("2.8 In tolerance", ::inTolerance, null)
     builder.publishConstString("3.0", "State Space Stuff")
     builder.addDoubleProperty("3.1 Predicted Position", { observer.getXhat(0) }, null)
@@ -393,18 +401,9 @@ open class Pivot(
 
   companion object {
     fun createPivot(): Pivot {
-      val motor = createSparkMax(
-        "Pivot Motors",
+      val motor = CANSparkMax(
         PivotConstants.MOTOR_ID,
-        encCreator = AbsoluteEncoder.creator(
-          PivotConstants.ENC_CHANNEL,
-          PivotConstants.OFFSET,
-          PivotConstants.UPR,
-          PivotConstants.ENC_INVERTED,
-        ),
-        inverted = PivotConstants.INVERTED,
-        currentLimit = PivotConstants.CURRENT_LIM,
-        slaveSparks = mapOf(Pair(PivotConstants.FOLLOWER_ID, PivotConstants.FOLLOWER_INVERTED))
+        CANSparkLowLevel.MotorType.kBrushless
       )
 
       val encoder = QuadEncoder(

diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/pivot/PivotSim.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/pivot/PivotSim.kt
@@ -1,5 +1,6 @@
 package frc.team449.robot2024.subsystems.pivot
 
+import com.revrobotics.CANSparkMax
 import edu.wpi.first.math.MathUtil
 import edu.wpi.first.math.controller.LinearPlantInversionFeedforward
 import edu.wpi.first.math.controller.LinearQuadraticRegulator
@@ -16,11 +17,10 @@ import frc.team449.robot2024.constants.MotorConstants
 import frc.team449.robot2024.constants.RobotConstants
 import frc.team449.robot2024.constants.subsystem.PivotConstants
 import frc.team449.system.encoder.QuadEncoder
-import frc.team449.system.motor.WrappedMotor
 import java.util.function.Supplier
 
 class PivotSim(
-  simmedMotor: WrappedMotor,
+  simmedMotor: CANSparkMax,
   encoder: QuadEncoder,
   controller: LinearQuadraticRegulator<N2, N1, N1>,
   fastController: LinearQuadraticRegulator<N2, N1, N1>,
@@ -59,7 +59,7 @@ class PivotSim(
   private var currentDraw = 0.0
 
   override fun periodic() {
-    pivotSim.setInputVoltage(MathUtil.clamp(motor.lastVoltage, -12.0, 12.0))
+    pivotSim.setInputVoltage(MathUtil.clamp(motor.get(), -12.0, 12.0))
 
     pivotSim.update(RobotConstants.LOOP_TIME)