diff --git a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt index ae119af..91f027b 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt @@ -36,7 +36,6 @@ enum class CTREMotorControllerId(val num: Int, val bus: String) { FrontRightDrivingMotor(4, "*"), RightArmMotor(10, "*"), LeftArmMotor(11, "*"), - WristMotor(12, "*"), } fun TalonFX(id: CTREMotorControllerId) = TalonFX(id.num, id.bus) \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index 29cc3b5..bcd5594 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -6,7 +6,6 @@ import com.frcteam3636.frc2024.subsystems.arm.Arm import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain import com.frcteam3636.frc2024.subsystems.indexer.Indexer import com.frcteam3636.frc2024.subsystems.intake.Intake -import com.frcteam3636.frc2024.subsystems.wrist.Wrist import com.frcteam3636.version.BUILD_DATE import com.frcteam3636.version.DIRTY import com.frcteam3636.version.GIT_BRANCH @@ -125,7 +124,6 @@ object Robot : LoggedRobot() { Drivetrain.register() Indexer.register() Intake.register() - Wrist.register() } /** Expose commands for autonomous routines to use and display an auto picker in Shuffleboard. */ diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt deleted file mode 100644 index 572b778..0000000 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt +++ /dev/null @@ -1,16 +0,0 @@ -package com.frcteam3636.frc2024.subsystems.wrist - -import com.frcteam3636.frc2024.subsystems.arm.Arm -import edu.wpi.first.wpilibj2.command.Subsystem -import org.littletonrobotics.junction.Logger - -object Wrist: Subsystem { - private var io: WristIO = WristIOKraken() - var inputs = LoggedWristInputs() - - override fun periodic() { - Logger.processInputs("Wrist", inputs) - //the wrist should always be parallel to the ground - io.pivotToAndStop(Arm.inputs.position * -1.0) //use position variable from arm - } -} \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/wrist/WristIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/wrist/WristIO.kt deleted file mode 100644 index 4c82c1f..0000000 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/wrist/WristIO.kt +++ /dev/null @@ -1,99 +0,0 @@ -package com.frcteam3636.frc2024.subsystems.wrist - -import com.ctre.phoenix6.configs.TalonFXConfiguration -import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC -import com.ctre.phoenix6.signals.GravityTypeValue -import com.ctre.phoenix6.signals.InvertedValue -import com.ctre.phoenix6.signals.NeutralModeValue -import com.frcteam3636.frc2024.CTREMotorControllerId -import com.frcteam3636.frc2024.TalonFX -import com.frcteam3636.frc2024.utils.math.MotorFFGains -import com.frcteam3636.frc2024.utils.math.PIDGains -import com.frcteam3636.frc2024.utils.math.motorFFGains -import com.frcteam3636.frc2024.utils.math.pidGains -import edu.wpi.first.units.Angle -import edu.wpi.first.units.Measure -import edu.wpi.first.units.Units.* -import edu.wpi.first.wpilibj.DigitalInput -import edu.wpi.first.wpilibj.DutyCycleEncoder -import org.littletonrobotics.junction.LogTable -import org.littletonrobotics.junction.Logger -import org.littletonrobotics.junction.inputs.LoggableInputs -import org.team9432.annotation.Logged -import kotlin.concurrent.thread - -interface WristIO { - fun updateInputs(inputs: WristInputs) - fun pivotToAndStop(position: Measure) -} - -@Logged -open class WristInputs { - var position = Radians.zero()!! - var velocity = RadiansPerSecond.zero()!! - var voltage = Volts.zero()!! -} - -class WristIOKraken: WristIO { - private val wristMotor = TalonFX(CTREMotorControllerId.WristMotor) - private val absoluteEncoder = DutyCycleEncoder(DigitalInput(7)) - - init { - val config = TalonFXConfiguration().apply { - MotorOutput.apply { - NeutralMode = NeutralModeValue.Brake - } - - Feedback.apply { - SensorToMechanismRatio = GEAR_RATIO - FeedbackRotorOffset = 0.0 - } - - Slot0.apply { - pidGains = PID_GAINS - motorFFGains = FF_GAINS - GravityType = GravityTypeValue.Arm_Cosine - kG = GRAVITY_GAIN - } - - MotionMagic.apply { - MotionMagicCruiseVelocity = PROFILE_VELOCITY - MotionMagicAcceleration = PROFILE_ACCELERATION - MotionMagicJerk = PROFILE_JERK - } - } - - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive - wristMotor.configurator.apply( - config - ) - } - - override fun updateInputs(inputs: WristInputs) { -// wristMotor.setPosition(absoluteEncoder.absolutePosition) - - inputs.position = Rotations.of(wristMotor.position.value) - inputs.velocity = RotationsPerSecond.of(wristMotor.velocity.value) - inputs.voltage = Volts.of(wristMotor.motorVoltage.value) - } - - override fun pivotToAndStop(position: Measure) { - Logger.recordOutput("Wrist/Position Setpoint", position) - - val wristControl = MotionMagicTorqueCurrentFOC(0.0).apply { - Slot = 0 - Position = position.`in`(Rotations) - } - wristMotor.setControl(wristControl) - } - - internal companion object Constants { - private const val GEAR_RATIO = 0.0 - val PID_GAINS = PIDGains(120.0, 0.0, 100.0) //placeholders - val FF_GAINS = MotorFFGains(7.8, 0.0, 0.0) //placeholders - private const val GRAVITY_GAIN = 0.0 - private const val PROFILE_ACCELERATION = 0.0 - private const val PROFILE_JERK = 0.0 - private const val PROFILE_VELOCITY = 0.0 - } -} \ No newline at end of file