From 9edc4838ecab38564f1935a25bd96764d3ec7d08 Mon Sep 17 00:00:00 2001 From: taiMorioka Date: Wed, 11 Dec 2024 20:05:48 -0800 Subject: [PATCH] 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)