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( {