From 11b2e608c5770d3ba4ce10e5043ed00b543f8407 Mon Sep 17 00:00:00 2001 From: doinkythederp Date: Tue, 10 Dec 2024 20:27:14 -0800 Subject: [PATCH 01/13] fix: correct can ids, resolve command loop overruns --- build.gradle.kts | 6 + .../kotlin/com/frcteam3636/frc2024/CAN.kt | 3 +- .../kotlin/com/frcteam3636/frc2024/Robot.kt | 2 +- .../frcteam3636/frc2024/subsystems/arm/Arm.kt | 2 +- .../frc2024/subsystems/arm/ArmIO.kt | 225 +++++++++--------- .../subsystems/drivetrain/Drivetrain.kt | 2 +- 6 files changed, 121 insertions(+), 119 deletions(-) diff --git a/build.gradle.kts b/build.gradle.kts index 66b3c0d..58faaf0 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -75,6 +75,12 @@ deploy { roborio.artifacts { register("frcJava") { jvmArgs.add("-ea") // Remove this flag during comp to disable asserts +// jvmArgs.add("-Dcom.sun.management.jmxremote=true") +// jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") +// jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") +// jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") +// jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") +// jvmArgs.add("-Djava.rmi.server.hostname=10.36.36.2") // Replace TE.AM with team number setJarTask(tasks.jar) dependsOn(tasks.assemble) } diff --git a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt index 95ff0f3..44327c3 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt @@ -19,9 +19,8 @@ enum class REVMotorControllerId(val num: Int) { BackRightDrivingMotor(3), FrontRightDrivingMotor(4), + IntakeMotor(12), IndexerMotor(13), - - IntakeMotor(21), } fun CANSparkMax(id: REVMotorControllerId, type: CANSparkLowLevel.MotorType) = diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index e955cae..8048d8b 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -67,7 +67,7 @@ object Robot : PatchedLoggedRobot() { ) // Joysticks are likely to be missing in simulation, which usually isn't a problem. - DriverStation.silenceJoystickConnectionWarning(!isReal()) + DriverStation.silenceJoystickConnectionWarning(true) // FIXME: re-add before comp configureAdvantageKit() configureSubsystems() diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index 1b8538f..42e29cf 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -23,7 +23,7 @@ import kotlin.math.cos import kotlin.math.sin -private const val SECONDS_BETWEEN_ARM_UPDATES = 0.5 +private const val SECONDS_BETWEEN_ARM_UPDATES = 2.0 object Arm : Subsystem { private var io: ArmIO = when (Robot.model) { diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index 83f5d96..a725453 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -39,7 +39,7 @@ open class ArmInputs { var absoluteEncoderConnected = false } -interface ArmIO{ +interface ArmIO { fun updateInputs(inputs: ArmInputs) @@ -48,157 +48,154 @@ interface ArmIO{ fun setVoltage(volts: Measure) fun updatePosition(position: Measure) - } +} - class ArmIOReal: ArmIO{ +class ArmIOReal: ArmIO { - private val leftMotor = TalonFX(CTREDeviceId.LeftArmMotor) + private val leftMotor = TalonFX(CTREDeviceId.LeftArmMotor) - private val rightMotor = TalonFX(CTREDeviceId.RightArmMotor) + private val rightMotor = TalonFX(CTREDeviceId.RightArmMotor) - private val absoluteEncoder = DutyCycleEncoder(DigitalInput(8)) + private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0)) - override fun updateInputs(inputs: ArmInputs) { - inputs.position = Rotations.of(absoluteEncoder.absolutePosition) - inputs.absoluteEncoderConnected = absoluteEncoder.isConnected + override fun updateInputs(inputs: ArmInputs) { + inputs.position = Rotations.of(absoluteEncoder.absolutePosition) + inputs.absoluteEncoderConnected = absoluteEncoder.isConnected - inputs.absoluteEncoderPosition = Rotations.of(absoluteEncoder.absolutePosition) + inputs.absoluteEncoderPosition = Rotations.of(absoluteEncoder.absolutePosition) - inputs.leftPosition = Rotations.of(leftMotor.position.value) - inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) + inputs.leftPosition = Rotations.of(leftMotor.position.value) + inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) - inputs.rightPosition = Rotations.of(rightMotor.position.value) - inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value) + inputs.rightPosition = Rotations.of(rightMotor.position.value) + inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value) - inputs.rightCurrent = Volts.of(leftMotor.motorVoltage.value) + inputs.rightCurrent = Volts.of(leftMotor.motorVoltage.value) + } - leftMotor.setPosition(inputs.absoluteEncoderPosition.`in`(Rotations)) - rightMotor.setPosition(inputs.absoluteEncoderPosition.`in`(Rotations)) - } + override fun updatePosition(position: Measure) { + leftMotor.setPosition(position.`in`(Rotations)) + rightMotor.setPosition(position.`in`(Rotations)) + } - override fun updatePosition(position: Measure) { - leftMotor.setPosition(position.`in`(Rotations)) - rightMotor.setPosition(position.`in`(Rotations)) - } + override fun pivotToPosition(position: Measure) { + Logger.recordOutput("Shooter/Pivot/Position Setpoint", position) - override fun pivotToPosition(position: Measure) { - Logger.recordOutput("Shooter/Pivot/Position Setpoint", position) + val control = MotionMagicTorqueCurrentFOC(0.0).apply { + Slot = 0 + Position = position.`in`(Rotations) + } + leftMotor.setControl(control) + rightMotor.setControl(control) - val control = MotionMagicTorqueCurrentFOC(0.0).apply { - Slot = 0 - Position = position.`in`(Rotations) - } - leftMotor.setControl(control) - rightMotor.setControl(control) + } - } + override fun setVoltage(volts: Measure) { + assert(volts in Volts.of(-12.0)..Volts.of(12.0)) + val control = VoltageOut(volts.`in`(Volts)) + leftMotor.setControl(control) + rightMotor.setControl(control) + } - override fun setVoltage(volts: Measure) { - assert(volts in Volts.of(-12.0)..Volts.of(12.0)) - val control = VoltageOut(volts.`in`(Volts)) - leftMotor.setControl(control) - rightMotor.setControl(control) - } + init { + val config = TalonFXConfiguration().apply { + MotorOutput.apply { + NeutralMode = NeutralModeValue.Brake + } - 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 - } + Feedback.apply { + SensorToMechanismRatio = GEAR_RATIO + FeedbackRotorOffset = 0.0 } - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive - leftMotor.configurator.apply( - config - ) + Slot0.apply { + pidGains = PID_GAINS + motorFFGains = FF_GAINS + GravityType = GravityTypeValue.Arm_Cosine + kG = GRAVITY_GAIN + } - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive - rightMotor.configurator.apply(config) + MotionMagic.apply { + MotionMagicCruiseVelocity = PROFILE_VELOCITY + MotionMagicAcceleration = PROFILE_ACCELERATION + MotionMagicJerk = PROFILE_JERK + } } + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + leftMotor.configurator.apply( + config + ) + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + rightMotor.configurator.apply(config) + } - 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 - } + 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 } - class ArmIOSim : ArmIO { - val armSim = SingleJointedArmSim( - DCMotor.getKrakenX60(1), - 3.0, - 0.244, - 0.331414, - -0.142921, - 2.32814365359, - true, - -0.142921 - ) +} - // TODO: Once we know the PID gains, this should be added back. +class ArmIOSim : ArmIO { + val armSim = SingleJointedArmSim( + DCMotor.getKrakenX60(1), + 3.0, + 0.244, + 0.331414, + -0.142921, + 2.32814365359, + true, + -0.142921 + ) + + // TODO: Once we know the PID gains, this should be added back. // val pidController = PIDController(ArmIOReal.PID_GAINS) - var setPoint = 0.0 + var setPoint = 0.0 - override fun updateInputs(inputs: ArmInputs) { - armSim.update(Robot.period) - inputs.rightVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) - inputs.leftVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) - inputs.position = Radians.of(armSim.angleRads) + override fun updateInputs(inputs: ArmInputs) { + armSim.update(Robot.period) + inputs.rightVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) + inputs.leftVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) + inputs.position = Radians.of(armSim.angleRads) // val pidVoltage = pidController.calculate(inputs.position.`in`(Radians),setPoint) // armSim.setInputVoltage(pidVoltage) - } + } - override fun pivotToPosition(position: Measure) { - setPoint = position.`in`(Radians) - } + override fun pivotToPosition(position: Measure) { + setPoint = position.`in`(Radians) + } - override fun setVoltage(volts: Measure) { - armSim.setInputVoltage(volts.`in`(Volts)) - Logger.recordOutput("/Arm/OutVolt", volts) - } + override fun setVoltage(volts: Measure) { + armSim.setInputVoltage(volts.`in`(Volts)) + Logger.recordOutput("/Arm/OutVolt", volts) + } - override fun updatePosition(position: Measure) { - // no drifting in sim so no need to update - } + override fun updatePosition(position: Measure) { + // no drifting in sim so no need to update } +} - class ArmIOPrototype: ArmIO { - //placeholder - override fun updateInputs(inputs: ArmInputs) { - } +class ArmIOPrototype: ArmIO { + //placeholder + override fun updateInputs(inputs: ArmInputs) { + } - override fun pivotToPosition(position: Measure) { - } + override fun pivotToPosition(position: Measure) { + } - override fun setVoltage(volts: Measure) { - } + override fun setVoltage(volts: Measure) { + } - override fun updatePosition(position: Measure) { - } + override fun updatePosition(position: Measure) { } +} diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt index 19e5978..9ac4428 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt @@ -279,7 +279,7 @@ object Drivetrain : Subsystem, Sendable { internal object Constants { // Translation/rotation coefficient for teleoperated driver controls /** Unit: Percent of max robot speed */ - const val TRANSLATION_SENSITIVITY = 0.1 + const val TRANSLATION_SENSITIVITY = 1.0 /** Unit: Rotations per second */ const val ROTATION_SENSITIVITY = 0.4 From 8d64bbcd527cd8d028d6df2e4d14ffce00ecfe9a Mon Sep 17 00:00:00 2001 From: drive station Date: Wed, 11 Dec 2024 15:15:10 -0800 Subject: [PATCH 02/13] fix: PDH CAN ID --- src/main/kotlin/com/frcteam3636/frc2024/Robot.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index 8048d8b..99079c6 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -104,7 +104,7 @@ object Robot : PatchedLoggedRobot() { // Enables power distribution logging if (model == Model.COMPETITION) { PowerDistribution( - 1, PowerDistribution.ModuleType.kRev + 62, PowerDistribution.ModuleType.kRev ) } else { PowerDistribution( From b326f2b37a4d9bd4e65531908806edafb0fd5b1f Mon Sep 17 00:00:00 2001 From: drive station Date: Wed, 11 Dec 2024 16:10:49 -0800 Subject: [PATCH 03/13] fix: remove negative multiplier --- .../com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt index 9ac4428..3c4a032 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt @@ -208,7 +208,7 @@ object Drivetrain : Subsystem, Sendable { desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, - -rotationInput.y * TAU * ROTATION_SENSITIVITY, + rotationInput.y * TAU * ROTATION_SENSITIVITY, inputs.gyroRotation.toRotation2d() ) } From 9edc4838ecab38564f1935a25bd96764d3ec7d08 Mon Sep 17 00:00:00 2001 From: taiMorioka Date: Wed, 11 Dec 2024 20:05:48 -0800 Subject: [PATCH 04/13] feat: guessing and checking --- .../kotlin/com/frcteam3636/frc2024/CAN.kt | 4 +- .../kotlin/com/frcteam3636/frc2024/Robot.kt | 42 ++++++++++----- .../frcteam3636/frc2024/subsystems/arm/Arm.kt | 20 +++++-- .../frc2024/subsystems/arm/ArmIO.kt | 52 ++++++++++++------- .../frc2024/subsystems/indexer/Indexer.kt | 49 +++++++++++------ .../frc2024/subsystems/indexer/IndexerIO.kt | 1 + .../frc2024/subsystems/intake/Intake.kt | 4 +- 7 files changed, 113 insertions(+), 59 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt index 44327c3..70f2a6b 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt @@ -34,8 +34,8 @@ enum class CTREDeviceId(val num: Int, val bus: String) { BackLeftDrivingMotor(2, "*"), BackRightDrivingMotor(3, "*"), FrontRightDrivingMotor(4, "*"), - RightArmMotor(10, "*"), - LeftArmMotor(11, "*"), + RightArmMotor(11, "*"), + LeftArmMotor(10, "*"), PigeonGyro(20, "*"), } diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index 99079c6..0d91b27 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -74,11 +74,6 @@ object Robot : PatchedLoggedRobot() { configureAutos() configureBindings() configureDashboard() - - Intake.register() - - //configure bindings - configureBindings() } /** Start logging or pull replay logs from a file */ @@ -155,8 +150,8 @@ object Robot : PatchedLoggedRobot() { /** Configure which commands each joystick button triggers. */ private fun configureBindings() { - Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight) - Indexer.defaultCommand = Indexer.autoRun() +// Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight) +// Indexer.defaultCommand = Indexer.autoRun() // (The button with the yellow tape on it) JoystickButton(joystickLeft, 8).onTrue(Commands.runOnce({ @@ -164,7 +159,7 @@ object Robot : PatchedLoggedRobot() { Drivetrain.zeroGyro() }).ignoringDisable(true)) -// //Intake + //Intake // controller.a() // .debounce(0.150) // .whileTrue( @@ -178,6 +173,17 @@ object Robot : PatchedLoggedRobot() { // ) // // controller.b() +// .debounce(0.15) +// .whileTrue( +// Indexer.indexBalloon() +// ) +// controller.y() +// .debounce(0.15) +// .whileTrue( +// Indexer.outtakeBalloon() +// ) +// +// controller.b() // .debounce(0.150) // .whileTrue( // Indexer.outtakeBalloon() @@ -205,18 +211,26 @@ object Robot : PatchedLoggedRobot() { .onTrue(Commands.runOnce(SignalLogger::stop)) controller.y() - .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward)) + .whileTrue( + Commands.sequence( + Commands.print("Starting quasistatic"), + Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward) + ) + .until(Arm.inSysIdUpperRange.negate()) + ) + controller.a() .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)) - controller.b() - .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward)) - - controller.x() + controller.rightBumper() + .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward) +// .until(Arm.inSysIdUpperRange.negate())) + ) + controller.leftBumper() .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse)) - //Arm positions + //Arm positions1 // controller.a() // .onTrue( // Arm.moveToPosition(Arm.Position.Stowed) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index 42e29cf..75b230a 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -8,6 +8,7 @@ import com.frcteam3636.frc2024.subsystems.intake.Intake.intakeAngleLigament import edu.wpi.first.units.Angle import edu.wpi.first.units.Measure import edu.wpi.first.units.Units.Degrees +import edu.wpi.first.units.Units.Radians import edu.wpi.first.units.Units.Volts import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d @@ -15,6 +16,7 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d import edu.wpi.first.wpilibj.util.Color import edu.wpi.first.wpilibj.util.Color8Bit import edu.wpi.first.wpilibj2.command.Subsystem +import edu.wpi.first.wpilibj2.command.button.Trigger import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism @@ -23,7 +25,7 @@ import kotlin.math.cos import kotlin.math.sin -private const val SECONDS_BETWEEN_ARM_UPDATES = 2.0 +private const val SECONDS_BETWEEN_ARM_UPDATES = 1.0 object Arm : Subsystem { private var io: ArmIO = when (Robot.model) { @@ -53,6 +55,13 @@ object Arm : Subsystem { Mechanism(io::setVoltage, null, this) ) + var inSysIdUpperRange = Trigger { + val lowerLimit = Radians.of(3.167) + inputs.position < lowerLimit + && inputs.leftPosition < lowerLimit +// && inputs.rightPosition < lowerLimit + } + private var timer = Timer().apply { start() } @@ -64,8 +73,9 @@ object Arm : Subsystem { armWristAngleLigament.angle = 90.0 - inputs.position.`in`(Degrees) if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){ - io.updatePosition(inputs.absoluteEncoderPosition) + io.updatePosition(inputs.position) } + Logger.recordOutput("/Arm/Mechanism", mechanism) } @@ -84,8 +94,8 @@ object Arm : Subsystem { sysID.dynamic(direction)!! enum class Position(val angle: Measure) { - Stowed(Degrees.of(135.0)), - PickUp(Degrees.of(30.0)), - Lower(Degrees.of(-15.0)) + Stowed(Radians.of(0.0)), + PickUp(Radians.of(-0.6)), + Lower(Radians.of(0.0)) } } \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index a725453..714de13 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -17,9 +17,7 @@ import edu.wpi.first.units.Voltage import edu.wpi.first.wpilibj.DigitalInput import edu.wpi.first.wpilibj.DutyCycleEncoder import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim -import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.Logger -import org.littletonrobotics.junction.inputs.LoggableInputs import org.team9432.annotation.Logged @Logged @@ -28,8 +26,6 @@ open class ArmInputs { var leftPosition = Radians.zero()!! var position = Radians.zero()!! - var absoluteEncoderPosition = Radians.zero()!! - var rightCurrent = Volts.zero()!! var leftCurrent = Volts.zero()!! @@ -60,11 +56,12 @@ class ArmIOReal: ArmIO { private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0)) override fun updateInputs(inputs: ArmInputs) { - inputs.position = Rotations.of(absoluteEncoder.absolutePosition) + val unoffsetPosition = Rotations.of(-absoluteEncoder.get() * CHAIN_GEAR_RATIO) + inputs.position = unoffsetPosition + ZERO_OFFSET + Logger.recordOutput("/Arm/Required Offset", unoffsetPosition.negate()) +// inputs.position = Rotations.of(-absoluteEncoder.absolutePosition) + ZERO_OFFSET inputs.absoluteEncoderConnected = absoluteEncoder.isConnected - inputs.absoluteEncoderPosition = Rotations.of(absoluteEncoder.absolutePosition) - inputs.leftPosition = Rotations.of(leftMotor.position.value) inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) @@ -81,22 +78,24 @@ class ArmIOReal: ArmIO { } override fun pivotToPosition(position: Measure) { - Logger.recordOutput("Shooter/Pivot/Position Setpoint", position) + val resolvedPosition = position + Logger.recordOutput("Shooter/Pivot/Position Setpoint", resolvedPosition) val control = MotionMagicTorqueCurrentFOC(0.0).apply { Slot = 0 - Position = position.`in`(Rotations) + Position = resolvedPosition.`in`(Rotations) } leftMotor.setControl(control) - rightMotor.setControl(control) +// rightMotor.setControl(control) } override fun setVoltage(volts: Measure) { assert(volts in Volts.of(-12.0)..Volts.of(12.0)) + Logger.recordOutput("/Arm/Voltage", volts) val control = VoltageOut(volts.`in`(Volts)) - leftMotor.setControl(control) - rightMotor.setControl(control) + leftMotor.setControl(control) // TODO: fix gearbox +// rightMotor.setControl(control) } init { @@ -122,6 +121,10 @@ class ArmIOReal: ArmIO { MotionMagicAcceleration = PROFILE_ACCELERATION MotionMagicJerk = PROFILE_JERK } + + ClosedLoopGeneral.apply { + ContinuousWrap = true + } } config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive @@ -135,13 +138,24 @@ class ArmIOReal: ArmIO { 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 +// private const val GEAR_RATIO = 125.0 +// val PID_GAINS = PIDGains(60.88, 0.0, 0.40035) //placeholders +// val FF_GAINS = MotorFFGains(0.1448, 0.11929, 0.001579) //placeholders +// private const val GRAVITY_GAIN = 0.0095692 +// private const val PROFILE_ACCELERATION = 1.0 +// private const val PROFILE_JERK = 1.0 +// private const val PROFILE_VELOCITY = 1.0 + + private const val CHAIN_GEAR_RATIO = 1.0 / 3.0 + private const val GEAR_RATIO = 125.0 / CHAIN_GEAR_RATIO + val PID_GAINS = PIDGains(64.857, 0.0, 20.319) //placeholders + val FF_GAINS = MotorFFGains(0.33157, 14.214, 0.15033) //placeholders + private const val GRAVITY_GAIN = 0.28233 + private const val PROFILE_ACCELERATION = 1.0 + private const val PROFILE_JERK = 1.0 + private const val PROFILE_VELOCITY = 1.0 + + val ZERO_OFFSET = Radians.of(1.01) } } diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt index 9ed00d9..64589e6 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt @@ -4,13 +4,14 @@ import com.frcteam3636.frc2024.Robot import com.frcteam3636.frc2024.subsystems.intake.Intake import edu.wpi.first.units.Units.Degrees import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.util.Color import edu.wpi.first.wpilibj.util.Color8Bit import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger -object Indexer: Subsystem { +object Indexer : Subsystem { private var io: IndexerIO = when (Robot.model) { Robot.Model.SIMULATION -> IndexerIOSim() Robot.Model.COMPETITION -> IndexerIOReal() @@ -36,34 +37,48 @@ object Indexer: Subsystem { * Does not run if no balloon. * Reverses if balloon is wrong alliance. */ - fun autoRun(): Command = - runEnd( + fun autoRun(): Command { + var previousState = BalloonState.None; + var timer = Timer() + timer.start() + return runEnd( { - DriverStation.getAlliance().map { - if ( - (inputs.balloonState == BalloonState.Blue && it == DriverStation.Alliance.Blue) - || (inputs.balloonState == BalloonState.Red && it == DriverStation.Alliance.Red) - ) { - io.setSpinSpeed(0.5) - } else if (inputs.balloonState == BalloonState.None) { - io.setSpinSpeed(0.0) - } else { - io.setSpinSpeed(-0.5) + if (previousState != inputs.balloonState) { + timer.reset() + } + + if (timer.hasElapsed(0.15)) { + DriverStation.getAlliance().map { + if ( + (inputs.balloonState == BalloonState.Blue && it == DriverStation.Alliance.Blue) + || (inputs.balloonState == BalloonState.Red && it == DriverStation.Alliance.Red) + + ) { + + io.setSpinSpeed(0.1) + } else if (inputs.balloonState == BalloonState.None) { + io.setSpinSpeed(0.0) + } else { + io.setSpinSpeed(-0.1) + } } } + + previousState = inputs.balloonState; }, { io.setSpinSpeed(0.0) } ) + } fun indexBalloon(): Command = runEnd( - {io.setSpinSpeed(0.5)}, - {io.setSpinSpeed(0.0)} + { io.setSpinSpeed(0.05) }, + { io.setSpinSpeed(0.0) } ) fun outtakeBalloon(): Command = runEnd( - {io.setSpinSpeed(-0.5)}, - {io.setSpinSpeed(0.0)} + { io.setSpinSpeed(-0.05) }, + { io.setSpinSpeed(0.0) } ) } \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt index 5dda329..d3eca63 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt @@ -49,6 +49,7 @@ class IndexerIOReal : IndexerIO{ CANSparkLowLevel.MotorType.kBrushless ) + override fun updateInputs(inputs: IndexerInputs) { inputs.indexerVelocity = Rotations.per(Minute).of(indexerMotor.encoder.velocity) inputs.indexerCurrent = Amps.of(indexerMotor.outputCurrent) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt index 4ff8b0b..20d57b7 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt @@ -43,7 +43,7 @@ object Intake: Subsystem { fun outtake(): Command = startEnd( { - io.setSpeed(-0.5) + io.setSpeed(-0.1) }, { io.setSpeed(0.0) @@ -53,7 +53,7 @@ object Intake: Subsystem { fun intake(): Command = startEnd( { - io.setSpeed(0.7) + io.setSpeed(0.1) }, { io.setSpeed(0.0) From 41f14f5b7710fe15519ba63345d0adf7457b1538 Mon Sep 17 00:00:00 2001 From: gniederman2083 Date: Thu, 12 Dec 2024 15:42:59 -0800 Subject: [PATCH 05/13] feat: tune pid --- .../kotlin/com/frcteam3636/frc2024/Robot.kt | 69 +++++++------- .../frcteam3636/frc2024/subsystems/arm/Arm.kt | 32 +++---- .../frc2024/subsystems/arm/ArmIO.kt | 95 +++++++++++-------- 3 files changed, 105 insertions(+), 91 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index 0d91b27..e833e6a 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -204,47 +204,46 @@ object Robot : PatchedLoggedRobot() { // ) //SysId - controller.leftBumper() - .onTrue(Commands.runOnce(SignalLogger::start)) - - controller.rightBumper() - .onTrue(Commands.runOnce(SignalLogger::stop)) +// controller.leftBumper() +// .onTrue(Commands.runOnce(SignalLogger::start)) +// +// controller.rightBumper() +// .onTrue(Commands.runOnce(SignalLogger::stop)) +// +// controller.y() +// .whileTrue( +// Commands.sequence( +// Commands.print("Starting quasistatic"), +// Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward) +// ) +// ) +// +// +// controller.a() +// .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)) +// +// controller.x() +// .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse)) +// +// controller.b() +// .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward)) - controller.y() - .whileTrue( - Commands.sequence( - Commands.print("Starting quasistatic"), - Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward) - ) - .until(Arm.inSysIdUpperRange.negate()) +// Arm positions + controller.a() + .onTrue( + Arm.moveToPosition(Arm.Position.Stowed) ) + controller.x() + .onTrue( + Arm.moveToPosition(Arm.Position.PickUp) + ) - controller.a() - .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)) - controller.rightBumper() - .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward) -// .until(Arm.inSysIdUpperRange.negate())) + controller.y() + .onTrue( + Arm.moveToPosition(Arm.Position.Lower) ) - controller.leftBumper() - .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse)) - - //Arm positions1 -// controller.a() -// .onTrue( -// Arm.moveToPosition(Arm.Position.Stowed) -// ) -// -// controller.x() -// .onTrue( -// Arm.moveToPosition(Arm.Position.PickUp) -// ) -// -// controller.y() -// .onTrue( -// Arm.moveToPosition(Arm.Position.Lower) -// ) } /** Add data to the driver station dashboard. */ diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index 75b230a..27a27ac 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -2,9 +2,6 @@ package com.frcteam3636.frc2024.subsystems.arm import com.ctre.phoenix6.SignalLogger import com.frcteam3636.frc2024.Robot -import com.frcteam3636.frc2024.subsystems.intake.Intake -import com.frcteam3636.frc2024.subsystems.intake.Intake.indexerAngleLigament -import com.frcteam3636.frc2024.subsystems.intake.Intake.intakeAngleLigament import edu.wpi.first.units.Angle import edu.wpi.first.units.Measure import edu.wpi.first.units.Units.Degrees @@ -16,13 +13,10 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d import edu.wpi.first.wpilibj.util.Color import edu.wpi.first.wpilibj.util.Color8Bit import edu.wpi.first.wpilibj2.command.Subsystem -import edu.wpi.first.wpilibj2.command.button.Trigger import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism import org.littletonrobotics.junction.Logger -import kotlin.math.cos -import kotlin.math.sin private const val SECONDS_BETWEEN_ARM_UPDATES = 1.0 @@ -55,12 +49,12 @@ object Arm : Subsystem { Mechanism(io::setVoltage, null, this) ) - var inSysIdUpperRange = Trigger { - val lowerLimit = Radians.of(3.167) - inputs.position < lowerLimit - && inputs.leftPosition < lowerLimit -// && inputs.rightPosition < lowerLimit - } +// var inSysIdUpperRange = Trigger { +// val lowerLimit = Radians.of(3.167) +// inputs.position < lowerLimit +// && inputs.leftPosition < lowerLimit +//// && inputs.rightPosition < lowerLimit +// } private var timer = Timer().apply { start() @@ -69,11 +63,13 @@ object Arm : Subsystem { override fun periodic() { io.updateInputs(inputs) Logger.processInputs("/Arm", inputs) - armAngleLigament.angle = inputs.position.`in`(Degrees) - armWristAngleLigament.angle = 90.0 - inputs.position.`in`(Degrees) + armAngleLigament.angle = inputs.leftPosition.`in`(Degrees) + armWristAngleLigament.angle = 90.0 - inputs.leftPosition.`in`(Degrees) - if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){ - io.updatePosition(inputs.position) + if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) + && inputs.leftAbsoluteEncoderConnected + && inputs.rightAbsoluteEncoderConnected) { + io.updatePosition(left = inputs.leftPosition, right = inputs.rightPosition) } Logger.recordOutput("/Arm/Mechanism", mechanism) @@ -84,7 +80,7 @@ object Arm : Subsystem { startEnd({ io.pivotToPosition(position.angle) }, { - io.pivotToPosition(inputs.position) + io.pivotToPosition(inputs.leftPosition) })!! fun sysIdQuasistatic(direction: Direction) = @@ -96,6 +92,6 @@ object Arm : Subsystem { enum class Position(val angle: Measure) { Stowed(Radians.of(0.0)), PickUp(Radians.of(-0.6)), - Lower(Radians.of(0.0)) + Lower(Radians.of(-0.4)) } } \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index 714de13..49d462e 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -22,9 +22,10 @@ import org.team9432.annotation.Logged @Logged open class ArmInputs { - var rightPosition = Radians.zero()!! + var rightRelativePosition = Radians.zero()!! + var leftRelativePosition = Radians.zero()!! var leftPosition = Radians.zero()!! - var position = Radians.zero()!! + var rightPosition = Radians.zero()!! var rightCurrent = Volts.zero()!! var leftCurrent = Volts.zero()!! @@ -32,7 +33,8 @@ open class ArmInputs { var rightVelocity = RadiansPerSecond.zero()!! var leftVelocity = RadiansPerSecond.zero()!! - var absoluteEncoderConnected = false + var leftAbsoluteEncoderConnected = false + var rightAbsoluteEncoderConnected = false } interface ArmIO { @@ -43,7 +45,7 @@ interface ArmIO { fun setVoltage(volts: Measure) - fun updatePosition(position: Measure) + fun updatePosition(left: Measure, right: Measure) } @@ -53,48 +55,53 @@ class ArmIOReal: ArmIO { private val rightMotor = TalonFX(CTREDeviceId.RightArmMotor) - private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0)) + private val leftAbsoluteEncoder = DutyCycleEncoder(DigitalInput(0)) + private val rightAbsoluteEncoder = DutyCycleEncoder(DigitalInput(1)) override fun updateInputs(inputs: ArmInputs) { - val unoffsetPosition = Rotations.of(-absoluteEncoder.get() * CHAIN_GEAR_RATIO) - inputs.position = unoffsetPosition + ZERO_OFFSET - Logger.recordOutput("/Arm/Required Offset", unoffsetPosition.negate()) -// inputs.position = Rotations.of(-absoluteEncoder.absolutePosition) + ZERO_OFFSET - inputs.absoluteEncoderConnected = absoluteEncoder.isConnected + val offsetlessLeftPosition = Rotations.of(-leftAbsoluteEncoder.get() * CHAIN_GEAR_RATIO) + inputs.leftPosition = offsetlessLeftPosition + LEFT_ZERO_OFFSET + Logger.recordOutput("/Arm/Required Left Offset", offsetlessLeftPosition.negate()) - inputs.leftPosition = Rotations.of(leftMotor.position.value) - inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) + val offsetlessRightPosition = Rotations.of(-rightAbsoluteEncoder.get() * CHAIN_GEAR_RATIO) + inputs.rightPosition = offsetlessLeftPosition + RIGHT_ZERO_OFFSET + Logger.recordOutput("/Arm/Required Right Offset", offsetlessRightPosition.negate()) + + inputs.leftRelativePosition = Rotations.of(leftMotor.position.value) + inputs.rightRelativePosition = Rotations.of(rightMotor.position.value) + inputs.leftAbsoluteEncoderConnected = leftAbsoluteEncoder.isConnected + inputs.rightAbsoluteEncoderConnected = rightAbsoluteEncoder.isConnected - inputs.rightPosition = Rotations.of(rightMotor.position.value) + inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value) - inputs.rightCurrent = Volts.of(leftMotor.motorVoltage.value) + inputs.leftCurrent = Volts.of(leftMotor.motorVoltage.value) + inputs.rightCurrent = Volts.of(rightMotor.motorVoltage.value) } - override fun updatePosition(position: Measure) { - leftMotor.setPosition(position.`in`(Rotations)) - rightMotor.setPosition(position.`in`(Rotations)) + override fun updatePosition(left: Measure, right: Measure) { + leftMotor.setPosition(left.`in`(Rotations)) + rightMotor.setPosition(right.`in`(Rotations)) } + private val pivotControl = MotionMagicTorqueCurrentFOC(0.0) override fun pivotToPosition(position: Measure) { - val resolvedPosition = position - Logger.recordOutput("Shooter/Pivot/Position Setpoint", resolvedPosition) + Logger.recordOutput("Shooter/Pivot/Position Setpoint", position) - val control = MotionMagicTorqueCurrentFOC(0.0).apply { + val control = pivotControl.apply { Slot = 0 - Position = resolvedPosition.`in`(Rotations) + Position = position.`in`(Rotations) } leftMotor.setControl(control) -// rightMotor.setControl(control) - + rightMotor.setControl(control) } override fun setVoltage(volts: Measure) { assert(volts in Volts.of(-12.0)..Volts.of(12.0)) Logger.recordOutput("/Arm/Voltage", volts) val control = VoltageOut(volts.`in`(Volts)) - leftMotor.setControl(control) // TODO: fix gearbox +// leftMotor.setControl(control) // rightMotor.setControl(control) } @@ -109,13 +116,6 @@ class ArmIOReal: ArmIO { 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 @@ -128,12 +128,26 @@ class ArmIOReal: ArmIO { } config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + config.Slot0.apply { + pidGains = LEFT_PID_GAINS + motorFFGains = LEFT_FF_GAINS + GravityType = GravityTypeValue.Arm_Cosine + kG = LEFT_GRAVITY_GAIN + } leftMotor.configurator.apply( config ) config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + config.Slot0.apply { + pidGains = RIGHT_PID_GAINS + motorFFGains = RIGHT_FF_GAINS + GravityType = GravityTypeValue.Arm_Cosine + kG = RIGHT_GRAVITY_GAIN + } rightMotor.configurator.apply(config) + + } @@ -148,14 +162,18 @@ class ArmIOReal: ArmIO { private const val CHAIN_GEAR_RATIO = 1.0 / 3.0 private const val GEAR_RATIO = 125.0 / CHAIN_GEAR_RATIO - val PID_GAINS = PIDGains(64.857, 0.0, 20.319) //placeholders - val FF_GAINS = MotorFFGains(0.33157, 14.214, 0.15033) //placeholders - private const val GRAVITY_GAIN = 0.28233 + val LEFT_PID_GAINS = PIDGains(64.138, 0.0, 59.411) + val LEFT_FF_GAINS = MotorFFGains(0.3289, 42.817, 0.39107) + private const val LEFT_GRAVITY_GAIN = 0.17119 + val RIGHT_PID_GAINS = PIDGains(63.339, 0.0, 59.134) + val RIGHT_FF_GAINS = MotorFFGains(0.29364, 43.77, 0.34751) + private const val RIGHT_GRAVITY_GAIN = 0.12181 private const val PROFILE_ACCELERATION = 1.0 private const val PROFILE_JERK = 1.0 private const val PROFILE_VELOCITY = 1.0 - val ZERO_OFFSET = Radians.of(1.01) + val LEFT_ZERO_OFFSET = Radians.of(1.05) + val RIGHT_ZERO_OFFSET = Radians.of(1.05) } } @@ -180,7 +198,8 @@ class ArmIOSim : ArmIO { armSim.update(Robot.period) inputs.rightVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) inputs.leftVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) - inputs.position = Radians.of(armSim.angleRads) + inputs.leftPosition = Radians.of(armSim.angleRads) + inputs.rightPosition = Radians.of(armSim.angleRads) // val pidVoltage = pidController.calculate(inputs.position.`in`(Radians),setPoint) // armSim.setInputVoltage(pidVoltage) } @@ -194,7 +213,7 @@ class ArmIOSim : ArmIO { Logger.recordOutput("/Arm/OutVolt", volts) } - override fun updatePosition(position: Measure) { + override fun updatePosition(left: Measure, right: Measure) { // no drifting in sim so no need to update } } @@ -210,6 +229,6 @@ class ArmIOPrototype: ArmIO { override fun setVoltage(volts: Measure) { } - override fun updatePosition(position: Measure) { + override fun updatePosition(left: Measure, right: Measure) { } } From 636d6c2a5263faebf450468979a784693f0b9401 Mon Sep 17 00:00:00 2001 From: doinkythederp Date: Thu, 12 Dec 2024 16:52:42 -0800 Subject: [PATCH 06/13] fix: invert right encoder direction --- .../com/frcteam3636/frc2024/Dashboard.kt | 5 +--- .../com/frcteam3636/frc2024/Diagnostics.kt | 12 +++------ .../kotlin/com/frcteam3636/frc2024/Robot.kt | 26 ++++++++++++++----- .../frc2024/subsystems/arm/ArmIO.kt | 8 +++--- 4 files changed, 28 insertions(+), 23 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Dashboard.kt b/src/main/kotlin/com/frcteam3636/frc2024/Dashboard.kt index 7cb62d0..2927aa4 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Dashboard.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Dashboard.kt @@ -71,12 +71,9 @@ object Dashboard { .withSize(6, 2) .withPosition(0, 2) - tab.addBoolean("NavX OK") { Diagnostics.latest.navXConnected } + tab.addBoolean("Gyro OK") { Diagnostics.latest.gyroConnected } .withPosition(10, 0) .withSize(2, 1) - tab.addBoolean("Cameras OK") { Diagnostics.latest.navXConnected } - .withPosition(10, 1) - .withSize(2, 1) tab.addBoolean("CAN Bus OK") { Diagnostics.latest.canStatus.transmitErrorCount == 0 && Diagnostics.latest.canStatus.receiveErrorCount == 0 } .withPosition(10, 2) .withSize(2, 1) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Diagnostics.kt b/src/main/kotlin/com/frcteam3636/frc2024/Diagnostics.kt index 7864a89..68455f5 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Diagnostics.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Diagnostics.kt @@ -17,8 +17,7 @@ import edu.wpi.first.wpilibj.RobotController */ data class Diagnostics( val batteryFull: Boolean, - val navXConnected: Boolean, - val camerasConnected: Boolean, + val gyroConnected: Boolean, val errorStatusCodes: Map, val canStatus: CANStatus, ) { @@ -29,8 +28,7 @@ data class Diagnostics( val messages = mutableListOf() // Add alerts for each suspicious diagnostic condition - if (!navXConnected) messages.add(ElasticNotification("NavX disconnected", "The naxX gyro has disconnected!")) - if (!camerasConnected) messages.add(ElasticNotification("Camera disconnected", "A camera has disconnected!")) + if (!gyroConnected) messages.add(ElasticNotification("IMU disconnected", "The gyro has disconnected!")) if (canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0) { messages.add(ElasticNotification("roboRIO CAN Errors", "CAN bus errors detected!")) } @@ -56,8 +54,7 @@ data class Diagnostics( /** The most recent diagnostics. */ var latest = Diagnostics( batteryFull = true, - navXConnected = true, - camerasConnected = true, + gyroConnected = true, errorStatusCodes = emptyMap(), canStatus = CANStatus(), ) @@ -73,8 +70,7 @@ data class Diagnostics( return Diagnostics( batteryFull = RobotController.getBatteryVoltage() >= FULL_BATTERY_VOLTAGE, - navXConnected = Drivetrain.inputs.gyroConnected, - camerasConnected = Drivetrain.allCamerasConnected, + gyroConnected = Drivetrain.inputs.gyroConnected, errorStatusCodes = errorCodes, canStatus = RobotController.getCANStatus(), ) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index e833e6a..52a710f 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -13,6 +13,7 @@ import com.frcteam3636.version.BUILD_DATE import com.frcteam3636.version.DIRTY import com.frcteam3636.version.GIT_BRANCH import com.frcteam3636.version.GIT_SHA +import edu.wpi.first.cameraserver.CameraServer import edu.wpi.first.hal.FRCNetComm.tInstances import edu.wpi.first.hal.FRCNetComm.tResourceType import edu.wpi.first.hal.HAL @@ -74,6 +75,8 @@ object Robot : PatchedLoggedRobot() { configureAutos() configureBindings() configureDashboard() + + CameraServer.startAutomaticCapture(); } /** Start logging or pull replay logs from a file */ @@ -195,13 +198,22 @@ object Robot : PatchedLoggedRobot() { // Indexer.indexBalloon() // ) -// //Outtake -// controller.leftBumper() -// .whileTrue( -// Commands.parallel( -// Intake.outtake(), -// ) -// ) + //Outtake + controller.leftBumper() + .whileTrue( + Commands.parallel( + Intake.outtake(), + Indexer.outtakeBalloon() + ) + ) + + controller.rightBumper() + .whileTrue( + Commands.parallel( + Intake.intake(), + Indexer.indexBalloon() + ) + ) //SysId // controller.leftBumper() diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index 49d462e..d780b0e 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -63,8 +63,8 @@ class ArmIOReal: ArmIO { inputs.leftPosition = offsetlessLeftPosition + LEFT_ZERO_OFFSET Logger.recordOutput("/Arm/Required Left Offset", offsetlessLeftPosition.negate()) - val offsetlessRightPosition = Rotations.of(-rightAbsoluteEncoder.get() * CHAIN_GEAR_RATIO) - inputs.rightPosition = offsetlessLeftPosition + RIGHT_ZERO_OFFSET + val offsetlessRightPosition = Rotations.of(rightAbsoluteEncoder.get() * CHAIN_GEAR_RATIO) + inputs.rightPosition = offsetlessRightPosition + RIGHT_ZERO_OFFSET Logger.recordOutput("/Arm/Required Right Offset", offsetlessRightPosition.negate()) inputs.leftRelativePosition = Rotations.of(leftMotor.position.value) @@ -172,8 +172,8 @@ class ArmIOReal: ArmIO { private const val PROFILE_JERK = 1.0 private const val PROFILE_VELOCITY = 1.0 - val LEFT_ZERO_OFFSET = Radians.of(1.05) - val RIGHT_ZERO_OFFSET = Radians.of(1.05) + val LEFT_ZERO_OFFSET = Radians.of(1.09) + val RIGHT_ZERO_OFFSET = Radians.of(-0.99) } } From 2f2f34a15b6d6c077c031af30a694c46e713c953 Mon Sep 17 00:00:00 2001 From: doinkythederp Date: Thu, 12 Dec 2024 20:45:21 -0800 Subject: [PATCH 07/13] feat: working arm, intake bindings --- .../kotlin/com/frcteam3636/frc2024/Robot.kt | 105 ++++-------------- .../frcteam3636/frc2024/subsystems/arm/Arm.kt | 32 +++++- .../frc2024/subsystems/arm/ArmIO.kt | 41 ++++++- .../subsystems/drivetrain/Drivetrain.kt | 5 + .../frc2024/subsystems/drivetrain/Module.kt | 7 +- .../frc2024/subsystems/indexer/Indexer.kt | 23 ++-- .../frc2024/subsystems/indexer/IndexerIO.kt | 20 ++-- .../frc2024/subsystems/intake/Intake.kt | 10 -- 8 files changed, 119 insertions(+), 124 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index 52a710f..20f8d41 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -1,6 +1,5 @@ package com.frcteam3636.frc2024 -import com.ctre.phoenix6.SignalLogger import com.ctre.phoenix6.StatusSignal import com.frcteam3636.frc2024.subsystems.arm.Arm import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain @@ -14,6 +13,8 @@ import com.frcteam3636.version.DIRTY import com.frcteam3636.version.GIT_BRANCH import com.frcteam3636.version.GIT_SHA import edu.wpi.first.cameraserver.CameraServer +import edu.wpi.first.cameraserver.CameraServerSharedStore +import edu.wpi.first.cscore.UsbCamera import edu.wpi.first.hal.FRCNetComm.tInstances import edu.wpi.first.hal.FRCNetComm.tResourceType import edu.wpi.first.hal.HAL @@ -25,14 +26,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.button.CommandXboxController import edu.wpi.first.wpilibj2.command.button.JoystickButton -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import org.littletonrobotics.junction.LogFileUtil import org.littletonrobotics.junction.Logger import org.littletonrobotics.junction.PatchedLoggedRobot import org.littletonrobotics.junction.networktables.NT4Publisher import org.littletonrobotics.junction.wpilog.WPILOGReader import org.littletonrobotics.junction.wpilog.WPILOGWriter -import java.io.File import kotlin.io.path.Path import kotlin.io.path.exists @@ -76,7 +75,8 @@ object Robot : PatchedLoggedRobot() { configureBindings() configureDashboard() - CameraServer.startAutomaticCapture(); + val camera = CameraServer.startAutomaticCapture() + camera.setResolution(240, 240) } /** Start logging or pull replay logs from a file */ @@ -153,8 +153,8 @@ object Robot : PatchedLoggedRobot() { /** Configure which commands each joystick button triggers. */ private fun configureBindings() { -// Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight) -// Indexer.defaultCommand = Indexer.autoRun() + Drivetrain.defaultCommand = Drivetrain.driveWithOneJoystick(joystickLeft) + Indexer.defaultCommand = Indexer.autoRun() // (The button with the yellow tape on it) JoystickButton(joystickLeft, 8).onTrue(Commands.runOnce({ @@ -162,86 +162,21 @@ object Robot : PatchedLoggedRobot() { Drivetrain.zeroGyro() }).ignoringDisable(true)) - //Intake -// controller.a() -// .debounce(0.150) -// .whileTrue( -// Intake.outtake() -// ) -// -// controller.x() -// .debounce(0.150) -// .whileTrue( -// Intake.intake() -// ) -// -// controller.b() -// .debounce(0.15) -// .whileTrue( -// Indexer.indexBalloon() -// ) -// controller.y() -// .debounce(0.15) -// .whileTrue( -// Indexer.outtakeBalloon() -// ) -// -// controller.b() -// .debounce(0.150) -// .whileTrue( -// Indexer.outtakeBalloon() -// ) -// -// controller.y() -// .debounce(0.150) -// .whileTrue( -// Indexer.indexBalloon() -// ) - - //Outtake + // Intake controller.leftBumper() - .whileTrue( - Commands.parallel( - Intake.outtake(), - Indexer.outtakeBalloon() - ) - ) + .whileTrue(Intake.intake()) controller.rightBumper() - .whileTrue( - Commands.parallel( - Intake.intake(), - Indexer.indexBalloon() - ) - ) + .whileTrue(Intake.intake()) + + controller.rightTrigger().whileTrue(Indexer.indexBalloon()) + controller.leftTrigger().whileTrue(Indexer.outtakeBalloon()) + + - //SysId -// controller.leftBumper() -// .onTrue(Commands.runOnce(SignalLogger::start)) -// -// controller.rightBumper() -// .onTrue(Commands.runOnce(SignalLogger::stop)) -// -// controller.y() -// .whileTrue( -// Commands.sequence( -// Commands.print("Starting quasistatic"), -// Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward) -// ) -// ) -// -// -// controller.a() -// .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)) -// -// controller.x() -// .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse)) -// -// controller.b() -// .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward)) // Arm positions - controller.a() + controller.y() .onTrue( Arm.moveToPosition(Arm.Position.Stowed) ) @@ -252,10 +187,14 @@ object Robot : PatchedLoggedRobot() { ) - controller.y() + controller.a() .onTrue( - Arm.moveToPosition(Arm.Position.Lower) - ) + Arm.moveToPosition(Arm.Position.Lower)) + + + controller.b() + .whileTrue(Arm.coastMode().ignoringDisable(true)) + } /** Add data to the driver station dashboard. */ diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index 27a27ac..b1f6381 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -12,11 +12,14 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d import edu.wpi.first.wpilibj.util.Color import edu.wpi.first.wpilibj.util.Color8Bit +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.StartEndCommand import edu.wpi.first.wpilibj2.command.Subsystem import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism import org.littletonrobotics.junction.Logger +import kotlin.math.sin private const val SECONDS_BETWEEN_ARM_UPDATES = 1.0 @@ -83,6 +86,25 @@ object Arm : Subsystem { io.pivotToPosition(inputs.leftPosition) })!! + fun coastMode() = + startEnd({ + io.setCoastMode(true) + }, { + io.setCoastMode(false) + }) + + fun followWave(center: Measure, magnitude: Measure): Command { + val timer = Timer().apply { + start() + } + return runEnd({ + val setpoint = magnitude * sin(timer.get() / 2.0) + center + io.pivotToPosition(setpoint) + }, { + io.pivotToPosition(inputs.leftPosition) + })!! + } + fun sysIdQuasistatic(direction: Direction) = sysID.quasistatic(direction)!! @@ -90,8 +112,12 @@ object Arm : Subsystem { sysID.dynamic(direction)!! enum class Position(val angle: Measure) { - Stowed(Radians.of(0.0)), - PickUp(Radians.of(-0.6)), - Lower(Radians.of(-0.4)) + /** All the way up */ + Stowed(Degrees.of(-127.0)), +// Stowed(Degrees.of(-80.0)), + /** Slightly picked up */ + PickUp(Degrees.of(0.0)), + /** Ready to pick up */ + Lower(Degrees.of(15.0)) } } \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index d780b0e..ed9f784 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -1,4 +1,5 @@ package com.frcteam3636.frc2024.subsystems.arm +import com.ctre.phoenix6.configs.MotorOutputConfigs import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC import com.ctre.phoenix6.controls.VoltageOut @@ -22,6 +23,8 @@ import org.team9432.annotation.Logged @Logged open class ArmInputs { + var reference = Radians.zero()!! + var rightRelativePosition = Radians.zero()!! var leftRelativePosition = Radians.zero()!! var leftPosition = Radians.zero()!! @@ -46,6 +49,8 @@ interface ArmIO { fun setVoltage(volts: Measure) fun updatePosition(left: Measure, right: Measure) + + fun setCoastMode(enabled: Boolean) } @@ -58,6 +63,10 @@ class ArmIOReal: ArmIO { private val leftAbsoluteEncoder = DutyCycleEncoder(DigitalInput(0)) private val rightAbsoluteEncoder = DutyCycleEncoder(DigitalInput(1)) + init { + Logger.recordOutput("/Arm/CoastMode", false) + } + override fun updateInputs(inputs: ArmInputs) { val offsetlessLeftPosition = Rotations.of(-leftAbsoluteEncoder.get() * CHAIN_GEAR_RATIO) inputs.leftPosition = offsetlessLeftPosition + LEFT_ZERO_OFFSET @@ -74,10 +83,12 @@ class ArmIOReal: ArmIO { inputs.rightAbsoluteEncoderConnected = rightAbsoluteEncoder.isConnected inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) - inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value) + inputs.rightVelocity = RotationsPerSecond.of(rightMotor.velocity.value) inputs.leftCurrent = Volts.of(leftMotor.motorVoltage.value) inputs.rightCurrent = Volts.of(rightMotor.motorVoltage.value) + + inputs.reference = Rotations.of(leftMotor.closedLoopReference.value) } override fun updatePosition(left: Measure, right: Measure) { @@ -87,7 +98,7 @@ class ArmIOReal: ArmIO { private val pivotControl = MotionMagicTorqueCurrentFOC(0.0) override fun pivotToPosition(position: Measure) { - Logger.recordOutput("Shooter/Pivot/Position Setpoint", position) + Logger.recordOutput("Arm/Position Setpoint", position) val control = pivotControl.apply { Slot = 0 @@ -97,6 +108,19 @@ class ArmIOReal: ArmIO { rightMotor.setControl(control) } + override fun setCoastMode(enabled: Boolean) { + Logger.recordOutput("/Arm/CoastMode", enabled) + val config = MotorOutputConfigs().apply { + NeutralMode = if (enabled) { + NeutralModeValue.Coast + } else { + NeutralModeValue.Brake + } + } + leftMotor.configurator.apply(config) + rightMotor.configurator.apply(config) + } + override fun setVoltage(volts: Measure) { assert(volts in Volts.of(-12.0)..Volts.of(12.0)) Logger.recordOutput("/Arm/Voltage", volts) @@ -162,16 +186,17 @@ class ArmIOReal: ArmIO { private const val CHAIN_GEAR_RATIO = 1.0 / 3.0 private const val GEAR_RATIO = 125.0 / CHAIN_GEAR_RATIO - val LEFT_PID_GAINS = PIDGains(64.138, 0.0, 59.411) + val LEFT_PID_GAINS = PIDGains(54.138, 0.0, 169.411) val LEFT_FF_GAINS = MotorFFGains(0.3289, 42.817, 0.39107) private const val LEFT_GRAVITY_GAIN = 0.17119 - val RIGHT_PID_GAINS = PIDGains(63.339, 0.0, 59.134) + val RIGHT_PID_GAINS = PIDGains(53.339, 0.0, 169.134) val RIGHT_FF_GAINS = MotorFFGains(0.29364, 43.77, 0.34751) private const val RIGHT_GRAVITY_GAIN = 0.12181 private const val PROFILE_ACCELERATION = 1.0 private const val PROFILE_JERK = 1.0 private const val PROFILE_VELOCITY = 1.0 + val LEFT_ZERO_OFFSET = Radians.of(1.09) val RIGHT_ZERO_OFFSET = Radians.of(-0.99) } @@ -216,6 +241,10 @@ class ArmIOSim : ArmIO { override fun updatePosition(left: Measure, right: Measure) { // no drifting in sim so no need to update } + + override fun setCoastMode(enabled: Boolean) { + + } } class ArmIOPrototype: ArmIO { @@ -231,4 +260,8 @@ class ArmIOPrototype: ArmIO { override fun updatePosition(left: Measure, right: Measure) { } + + override fun setCoastMode(enabled: Boolean) { + + } } diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt index 3c4a032..c1b008d 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt @@ -220,6 +220,11 @@ object Drivetrain : Subsystem, Sendable { drive(translationJoystick.translation2d, rotationJoystick.translation2d) } + fun driveWithOneJoystick(joystick: Joystick): Command = + run { + drive(joystick.translation2d, Translation2d(0.0, -joystick.z)) + } + @Suppress("unused") fun driveWithController(controller: CommandXboxController): Command = run { diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt index b11f8e3..1d0c9f0 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt @@ -133,15 +133,18 @@ class DrivingTalon(id: CTREDeviceId) : DrivingMotor { } + private val positionSignal = inner.position + private val velocitySignal = inner.velocity + init { Robot.statusSignals[id.name] = inner.version } override val position: Measure - get() = Meters.of(inner.position.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters)) + get() = Meters.of(positionSignal.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters)) override var velocity: Measure> - get() = MetersPerSecond.of(inner.velocity.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters)) + get() = MetersPerSecond.of(velocitySignal.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters)) set(value) { inner.setControl(VelocityTorqueCurrentFOC(value.`in`(MetersPerSecond) / DRIVING_GEAR_RATIO_TALON / WHEEL_CIRCUMFERENCE.`in`(Meters))) } diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt index 64589e6..b33e39d 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt @@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger object Indexer : Subsystem { + private val INDEXER_SPEED = 1.0 + private var io: IndexerIO = when (Robot.model) { Robot.Model.SIMULATION -> IndexerIOSim() Robot.Model.COMPETITION -> IndexerIOReal() @@ -47,19 +49,16 @@ object Indexer : Subsystem { timer.reset() } - if (timer.hasElapsed(0.15)) { + if (timer.hasElapsed(0.5) || inputs.balloonState != BalloonState.None) { DriverStation.getAlliance().map { if ( (inputs.balloonState == BalloonState.Blue && it == DriverStation.Alliance.Blue) || (inputs.balloonState == BalloonState.Red && it == DriverStation.Alliance.Red) - + || inputs.balloonState == BalloonState.None ) { - - io.setSpinSpeed(0.1) - } else if (inputs.balloonState == BalloonState.None) { - io.setSpinSpeed(0.0) + io.setVoltage(INDEXER_SPEED) } else { - io.setSpinSpeed(-0.1) + io.setVoltage(-INDEXER_SPEED) } } } @@ -67,18 +66,18 @@ object Indexer : Subsystem { previousState = inputs.balloonState; }, { - io.setSpinSpeed(0.0) + io.setVoltage(0.0) } ) } fun indexBalloon(): Command = runEnd( - { io.setSpinSpeed(0.05) }, - { io.setSpinSpeed(0.0) } + { io.setVoltage(INDEXER_SPEED) }, + { io.setVoltage(0.0) } ) fun outtakeBalloon(): Command = runEnd( - { io.setSpinSpeed(-0.05) }, - { io.setSpinSpeed(0.0) } + { io.setVoltage(-INDEXER_SPEED) }, + { io.setVoltage(0.0) } ) } \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt index d3eca63..c734ca9 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt @@ -32,8 +32,8 @@ open class IndexerInputs { interface IndexerIO { fun updateInputs(inputs: IndexerInputs) - fun setSpinSpeed(speed: Double) - // percent of full speed + fun setVoltage(voltage: Double) + // percent of full voltage } class IndexerIOReal : IndexerIO{ @@ -64,10 +64,10 @@ class IndexerIOReal : IndexerIO{ } } - override fun setSpinSpeed(speed: Double) { - assert(speed in -1.0..1.0) - println("Speed: $speed") - indexerMotor.set(speed) + override fun setVoltage(voltage: Double) { + assert(voltage in -12.0..12.0) + Logger.recordOutput("/Indexer/OutputVoltage", voltage) + indexerMotor.setVoltage(voltage) } } @@ -85,9 +85,9 @@ class IndexerIOSim: IndexerIO { inputs.position = Radians.of(inputs.position.`in`(Radians).mod(TAU)) } - override fun setSpinSpeed(speed: Double) { - assert(speed in -1.0..1.0) - flywheelSim.setInputVoltage(speed*12.0) + override fun setVoltage(voltage: Double) { + assert(voltage in -1.0..1.0) + flywheelSim.setInputVoltage(voltage*12.0) } } @@ -95,6 +95,6 @@ class IndexerIOPrototype: IndexerIO { override fun updateInputs(inputs: IndexerInputs) { } - override fun setSpinSpeed(speed: Double) { + override fun setVoltage(voltage: Double) { } } \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt index 20d57b7..8a08711 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt @@ -40,16 +40,6 @@ object Intake: Subsystem { Logger.recordOutput("Intake Angle", mechanism) } - fun outtake(): Command = - startEnd( - { - io.setSpeed(-0.1) - }, - { - io.setSpeed(0.0) - } - ) - fun intake(): Command = startEnd( { From 74dd3f16c0feafed1bee3eaa4f514d268875281d Mon Sep 17 00:00:00 2001 From: drive station Date: Fri, 13 Dec 2024 15:30:49 -0800 Subject: [PATCH 08/13] fix: last minute changes that fix the robot --- src/main/kotlin/com/frcteam3636/frc2024/Robot.kt | 2 +- .../kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index 20f8d41..f527975 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -153,7 +153,7 @@ object Robot : PatchedLoggedRobot() { /** Configure which commands each joystick button triggers. */ private fun configureBindings() { - Drivetrain.defaultCommand = Drivetrain.driveWithOneJoystick(joystickLeft) + Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight) Indexer.defaultCommand = Indexer.autoRun() // (The button with the yellow tape on it) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index ed9f784..2036187 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -197,8 +197,8 @@ class ArmIOReal: ArmIO { private const val PROFILE_VELOCITY = 1.0 - val LEFT_ZERO_OFFSET = Radians.of(1.09) - val RIGHT_ZERO_OFFSET = Radians.of(-0.99) + val LEFT_ZERO_OFFSET = Radians.of(1.1) + val RIGHT_ZERO_OFFSET = Radians.of(-2.06) } } From 6a8ddc3e11408185ad6e6258ac2b6c23cf99c317 Mon Sep 17 00:00:00 2001 From: drive station Date: Fri, 13 Dec 2024 16:52:01 -0800 Subject: [PATCH 09/13] fix: last minute changes that fix the robot --- .../frc2024/subsystems/arm/ArmIO.kt | 122 +++++++++--------- .../subsystems/drivetrain/Drivetrain.kt | 17 ++- 2 files changed, 77 insertions(+), 62 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index 2036187..3d8289c 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -55,16 +55,61 @@ interface ArmIO { class ArmIOReal: ArmIO { - private val leftMotor = TalonFX(CTREDeviceId.LeftArmMotor) - private val rightMotor = TalonFX(CTREDeviceId.RightArmMotor) private val leftAbsoluteEncoder = DutyCycleEncoder(DigitalInput(0)) private val rightAbsoluteEncoder = DutyCycleEncoder(DigitalInput(1)) + private val leftConfig = TalonFXConfiguration().apply { + Slot0.apply { + pidGains = LEFT_PID_GAINS + motorFFGains = LEFT_FF_GAINS + GravityType = GravityTypeValue.Arm_Cosine + kG = LEFT_GRAVITY_GAIN + } + + MotorOutput.apply { + NeutralMode = NeutralModeValue.Brake + Inverted = InvertedValue.CounterClockwise_Positive + } + + Feedback.apply { + SensorToMechanismRatio = GEAR_RATIO + FeedbackRotorOffset = 0.0 + } + + MotionMagic.apply { + MotionMagicCruiseVelocity = PROFILE_VELOCITY + MotionMagicAcceleration = PROFILE_ACCELERATION + MotionMagicJerk = PROFILE_JERK + } + + ClosedLoopGeneral.apply { + ContinuousWrap = true + } + } + + private val rightConfig = TalonFXConfiguration().apply { + deserialize(leftConfig.serialize()) // TalonFXConfiguration doesn't implement Cloneable, so we do this instead + + MotorOutput.apply { + Inverted = InvertedValue.Clockwise_Positive + } + + Slot0.apply { + pidGains = RIGHT_PID_GAINS + motorFFGains = RIGHT_FF_GAINS + GravityType = GravityTypeValue.Arm_Cosine + kG = RIGHT_GRAVITY_GAIN + } + } + init { Logger.recordOutput("/Arm/CoastMode", false) + + leftMotor.configurator.apply(leftConfig) + rightMotor.configurator.apply(rightConfig) } override fun updateInputs(inputs: ArmInputs) { @@ -110,15 +155,22 @@ class ArmIOReal: ArmIO { override fun setCoastMode(enabled: Boolean) { Logger.recordOutput("/Arm/CoastMode", enabled) - val config = MotorOutputConfigs().apply { - NeutralMode = if (enabled) { - NeutralModeValue.Coast - } else { - NeutralModeValue.Brake - } + val neutralModeValue = if (enabled) { + NeutralModeValue.Coast + } else { + NeutralModeValue.Brake } - leftMotor.configurator.apply(config) - rightMotor.configurator.apply(config) + + leftMotor.configurator.apply(leftConfig.apply { + MotorOutput.apply { + NeutralMode = neutralModeValue + } + }) + rightMotor.configurator.apply(rightConfig.apply { + MotorOutput.apply { + NeutralMode = neutralModeValue + } + }) } override fun setVoltage(volts: Measure) { @@ -129,52 +181,6 @@ class ArmIOReal: ArmIO { // rightMotor.setControl(control) } - init { - val config = TalonFXConfiguration().apply { - MotorOutput.apply { - NeutralMode = NeutralModeValue.Brake - } - - Feedback.apply { - SensorToMechanismRatio = GEAR_RATIO - FeedbackRotorOffset = 0.0 - } - - MotionMagic.apply { - MotionMagicCruiseVelocity = PROFILE_VELOCITY - MotionMagicAcceleration = PROFILE_ACCELERATION - MotionMagicJerk = PROFILE_JERK - } - - ClosedLoopGeneral.apply { - ContinuousWrap = true - } - } - - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive - config.Slot0.apply { - pidGains = LEFT_PID_GAINS - motorFFGains = LEFT_FF_GAINS - GravityType = GravityTypeValue.Arm_Cosine - kG = LEFT_GRAVITY_GAIN - } - leftMotor.configurator.apply( - config - ) - - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive - config.Slot0.apply { - pidGains = RIGHT_PID_GAINS - motorFFGains = RIGHT_FF_GAINS - GravityType = GravityTypeValue.Arm_Cosine - kG = RIGHT_GRAVITY_GAIN - } - rightMotor.configurator.apply(config) - - - } - - internal companion object Constants { // private const val GEAR_RATIO = 125.0 // val PID_GAINS = PIDGains(60.88, 0.0, 0.40035) //placeholders @@ -197,8 +203,8 @@ class ArmIOReal: ArmIO { private const val PROFILE_VELOCITY = 1.0 - val LEFT_ZERO_OFFSET = Radians.of(1.1) - val RIGHT_ZERO_OFFSET = Radians.of(-2.06) + val LEFT_ZERO_OFFSET = Radians.of(1.08) + val RIGHT_ZERO_OFFSET = Radians.of(-2.05) } } diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt index c1b008d..901e0b2 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt @@ -7,6 +7,7 @@ import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain.Constants.BRAKE_ import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain.Constants.DEFAULT_PATHING_CONSTRAINTS import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain.Constants.FREE_SPEED import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain.Constants.JOYSTICK_DEADBAND +import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain.Constants.JOYSTICK_EXPONENT import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain.Constants.ROTATION_SENSITIVITY import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain.Constants.TRANSLATION_SENSITIVITY import com.frcteam3636.frc2024.utils.ElasticWidgets @@ -43,6 +44,7 @@ import java.util.* import kotlin.jvm.optionals.getOrNull import kotlin.math.PI import kotlin.math.abs +import kotlin.math.pow /** A singleton object representing the drivetrain. */ object Drivetrain : Subsystem, Sendable { @@ -205,9 +207,15 @@ object Drivetrain : Subsystem, Sendable { // No joystick input - stop moving! desiredModuleStates = BRAKE_POSITION } else { + // Prevent even powers from removing negatives by multiplying outside of abs + val exponentialTranslation = Translation2d( + translationInput.x * abs(translationInput.x.pow(JOYSTICK_EXPONENT - 1)), + translationInput.y * abs(translationInput.y.pow(JOYSTICK_EXPONENT - 1)), + ) + desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, - translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + exponentialTranslation.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + exponentialTranslation.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, rotationInput.y * TAU * ROTATION_SENSITIVITY, inputs.gyroRotation.toRotation2d() ) @@ -287,12 +295,13 @@ object Drivetrain : Subsystem, Sendable { const val TRANSLATION_SENSITIVITY = 1.0 /** Unit: Rotations per second */ - const val ROTATION_SENSITIVITY = 0.4 + const val ROTATION_SENSITIVITY = 0.6 private val WHEEL_BASE: Double = Units.inchesToMeters(30.0) private val TRACK_WIDTH: Double = Units.inchesToMeters(28.0) - const val JOYSTICK_DEADBAND = 0.15 + const val JOYSTICK_DEADBAND = 0.075 + const val JOYSTICK_EXPONENT = 2 val MODULE_POSITIONS = PerCorner( From 2b0b1de4e3e6ee18812b9b3644a916680fbf5d83 Mon Sep 17 00:00:00 2001 From: drive station Date: Fri, 13 Dec 2024 19:19:54 -0800 Subject: [PATCH 10/13] refactor: add period argument to followWave Co-authored-by: doinkythederp --- .../frcteam3636/frc2024/subsystems/arm/Arm.kt | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index b1f6381..921dfd0 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -2,11 +2,11 @@ package com.frcteam3636.frc2024.subsystems.arm import com.ctre.phoenix6.SignalLogger import com.frcteam3636.frc2024.Robot +import com.frcteam3636.frc2024.utils.math.TAU import edu.wpi.first.units.Angle import edu.wpi.first.units.Measure -import edu.wpi.first.units.Units.Degrees -import edu.wpi.first.units.Units.Radians -import edu.wpi.first.units.Units.Volts +import edu.wpi.first.units.Time +import edu.wpi.first.units.Units.* import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism import org.littletonrobotics.junction.Logger import kotlin.math.sin +import kotlin.time.Duration private const val SECONDS_BETWEEN_ARM_UPDATES = 1.0 @@ -91,14 +92,24 @@ object Arm : Subsystem { io.setCoastMode(true) }, { io.setCoastMode(false) - }) + })!! - fun followWave(center: Measure, magnitude: Measure): Command { + /** + * Follow a sine wave to test the arm's motion control. + * + * The wave begins as soon as the command is instantiated, not when the command is scheduled. + * + * @param center the angle which the wave oscillates around + * @param magnitude the size of the wave + * @param period the time before the wave returns to its original position + */ + fun followWave(center: Measure, magnitude: Measure, period: Measure