From 0896d3285d041077d6e69a7665247049917de776 Mon Sep 17 00:00:00 2001 From: Jeffery Date: Tue, 10 Sep 2024 21:49:39 -0400 Subject: [PATCH] fixed inversion issue with krakenshooter and changed pivot to use CANSparkMax for access to internals --- .../robot2024/commands/PivotCalibration.kt | 2 +- .../subsystem/SpinShooterKrakenConstants.kt | 2 +- .../robot2024/subsystems/pivot/Pivot.kt | 37 +++++++++---------- .../robot2024/subsystems/pivot/PivotSim.kt | 6 +-- 4 files changed, 23 insertions(+), 24 deletions(-) diff --git a/src/main/kotlin/frc/team449/robot2024/commands/PivotCalibration.kt b/src/main/kotlin/frc/team449/robot2024/commands/PivotCalibration.kt index 756fc97..0a7fd2c 100644 --- 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() 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 index b302507..e0df5e4 100644 --- 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 index 71a7a21..82653ad 100644 --- 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, private val fastController: LinearQuadraticRegulator, @@ -55,6 +53,7 @@ open class Pivot( open val positionSupplier: Supplier = Supplier { encoder.position } + open val velocitySupplier: Supplier = 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 index 7351f8d..eaee772 100644 --- 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, fastController: LinearQuadraticRegulator, @@ -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)