diff --git a/src/main/deploy/pathplanner/paths/New New New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New New New Path.path new file mode 100644 index 00000000..fc68dc9f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New New New New New Path.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9121061779449953, + "y": 6.983739254587885 + }, + "prevControl": null, + "nextControl": { + "x": 3.9121061779449935, + "y": 6.483739254587885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 5.0 + }, + "prevControl": { + "x": 4.0, + "y": 6.0 + }, + "nextControl": { + "x": 6.0, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 1.0 + }, + "prevControl": { + "x": 6.75, + "y": 2.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index f696341c..3cb15237 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -3,6 +3,8 @@ package com.team4099.robot2023.auto import com.team4099.robot2023.auto.mode.ExamplePathAuto import com.team4099.robot2023.auto.mode.FiveNoteAutoPath import com.team4099.robot2023.auto.mode.FourNoteAutoPath +import com.team4099.robot2023.auto.mode.FourNoteAutoPathWithFirstCenterSide +import com.team4099.robot2023.auto.mode.FourNoteAutoPathWithFirstSourceSide import com.team4099.robot2023.auto.mode.FourNoteLeftCenterLine import com.team4099.robot2023.auto.mode.FourNoteMiddleCenterLine import com.team4099.robot2023.auto.mode.FourNoteRightCenterLine @@ -46,7 +48,9 @@ object AutonomousSelector { // orientationChooser.addOption("Right", 270.degrees) // autoTab.add("Starting Orientation", orientationChooser) - autonomousModeChooser.addOption("Four Note Wing Auto", AutonomousMode.FOUR_NOTE_AUTO_PATH) + autonomousModeChooser.addOption("Four Note Wing Auto (Amp Side Note First, Default)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST) + autonomousModeChooser.addOption("Four Note Wing Auto (Center Wing Note First)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST) + autonomousModeChooser.addOption("Four Note Wing Auto (Source Side Note First)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST) /* @@ -169,7 +173,7 @@ object AutonomousSelector { ) }) .andThen(SixNoteAutoPath(drivetrain, superstructure)) - AutonomousMode.FOUR_NOTE_AUTO_PATH -> + AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST -> return WaitCommand(waitTime.inSeconds) .andThen({ val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPath.startingPose) @@ -177,6 +181,22 @@ object AutonomousSelector { drivetrain.resetFieldFrameEstimator(flippedPose) }) .andThen(FourNoteAutoPath(drivetrain, superstructure)) + AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstCenterSide.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(FourNoteAutoPathWithFirstCenterSide(drivetrain, superstructure)) + AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstSourceSide.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(FourNoteAutoPathWithFirstSourceSide(drivetrain, superstructure)) AutonomousMode.FOUR_NOTE_RIGHT_AUTO_PATH -> return WaitCommand(waitTime.inSeconds) .andThen({ @@ -321,7 +341,9 @@ object AutonomousSelector { private enum class AutonomousMode { TEST_AUTO_PATH, - FOUR_NOTE_AUTO_PATH, + FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST, + FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST, + FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST, FOUR_NOTE_RIGHT_AUTO_PATH, FOUR_NOTE_MIDDLE_AUTO_PATH, FOUR_NOTE_LEFT_AUTO_PATH, diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt index a0f21fd5..b9fece1e 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt @@ -36,14 +36,14 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru startingPose.rotation.inRotation2ds ), FieldWaypoint( - Translation2d(2.9.meters - 0.4.meters, 6.9.meters + 0.1.meters) + Translation2d(2.91.meters - 0.75.meters, 6.82.meters) .translation2d, null, 180.degrees.inRotation2ds, ), FieldWaypoint( - Translation2d(2.9.meters, 6.9.meters + 0.1.meters).translation2d, - null, + Translation2d(2.91.meters - 0.25.meters, 7.meters).translation2d, + 0.degrees.inRotation2ds, 180.degrees.inRotation2ds, ), FieldWaypoint( @@ -73,13 +73,13 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru 180.degrees.inRotation2ds ), // Subwoofer FieldWaypoint( - Translation2d(2.41.meters - 0.4.meters, 4.1.meters).translation2d, + Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(2.41.meters + 0.225.meters, 4.1.meters).translation2d, - null, + Translation2d(2.41.meters, 4.1.meters).translation2d, + 0.degrees.inRotation2ds, 180.degrees.inRotation2ds ), FieldWaypoint( @@ -181,6 +181,6 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } companion object { - val startingPose = Pose2d(Translation2d(1.42.meters - 3.inches, 5.535.meters), 180.degrees) + val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees) } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt new file mode 100644 index 00000000..9d96428f --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt @@ -0,0 +1,186 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FourNoteAutoPathWithFirstCenterSide(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand().withTimeout(0.5), + WaitCommand(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, + 5.55.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.34.meters + 0.3.meters + 0.25.meters, 5.535.meters) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, + 5.45.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) // Subwoofer + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.3).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.41.meters, 4.1.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.91.meters - 0.75.meters, 6.9.meters) + .translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(2.91.meters - 0.25.meters, 7.1.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.35.meters, 4.85.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.92.meters, 3.9.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.29.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ) + ) + } + + companion object { + val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstSourceSide.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstSourceSide.kt new file mode 100644 index 00000000..9ce1739c --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstSourceSide.kt @@ -0,0 +1,187 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FourNoteAutoPathWithFirstSourceSide(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand().withTimeout(0.5), + WaitCommand(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.41.meters, 4.1.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, + 5.55.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.34.meters + 0.3.meters + 0.25.meters, 5.535.meters) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, + 5.45.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) // Subwoofer + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.3).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.91.meters - 0.75.meters, 6.82.meters) + .translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(2.91.meters - 0.25.meters, 7.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.35.meters, 4.85.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.92.meters, 3.9.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.29.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ) + ) + } + + companion object { + val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt index 3aaf5ed1..3c2fc197 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt @@ -1,8 +1,10 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.led.Leds import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.driver.DriverProfile diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 4598ced7..039ca9ec 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -84,10 +84,10 @@ object DrivetrainConstants { val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds - val FRONT_LEFT_MODULE_ZERO = 3.285.radians + PI.radians // good - val FRONT_RIGHT_MODULE_ZERO = 2.91.radians + PI.radians // good - val BACK_LEFT_MODULE_ZERO = 5.665.radians + PI.radians // good - val BACK_RIGHT_MODULE_ZERO = 3.50.radians + PI.radians // good + val FRONT_LEFT_MODULE_ZERO = 0.19.radians // good + val FRONT_RIGHT_MODULE_ZERO = 6.016.radians // good + val BACK_LEFT_MODULE_ZERO = 2.538.radians // good + val BACK_RIGHT_MODULE_ZERO = 1.25.radians // good val STEERING_COMPENSATION_VOLTAGE = 10.volts val DRIVE_COMPENSATION_VOLTAGE = 12.volts diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 44840a96..e886b127 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -58,7 +58,6 @@ object WristConstants { val MAX_WRIST_VELOCITY = 300.degrees.perSecond val MAX_WRIST_ACCELERATION = 500.degrees.perSecond.perSecond - val HARDSTOP_OFFSET = 47.degrees object PID { val ARBITRARY_FEEDFORWARD = 0.03.volts diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index e695222e..806f098b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -80,7 +80,7 @@ class SwerveModuleIOTalon( private val potentiometerOutput: Double get() { - return if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME) { + return if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) { potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * PI } else { 2 * PI - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI @@ -341,9 +341,11 @@ class SwerveModuleIOTalon( override fun zeroSteering(isInAutonomous: Boolean) { steeringFalcon.setPosition( steeringSensor.positionToRawUnits( - if (label != Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME) + if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME){ + (potentiometerOutput.radians - zeroOffset) + } else { (2 * PI).radians - (potentiometerOutput.radians - zeroOffset) - else (potentiometerOutput.radians - zeroOffset) + } ) ) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt index eaf5f4cb..c3206956 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -19,6 +19,7 @@ class Leds(val io: LedIO) { var isPreping = false var seesGamePiece = false var seesTag = true + var ejectingGamePiece = false var isTuningDebugging = Constants.Tuning.TUNING_MODE || Constants.Tuning.DEBUGING_MODE var state = LEDConstants.CandleState.NO_NOTE @@ -46,6 +47,8 @@ class Leds(val io: LedIO) { } else { state = LEDConstants.CandleState.RED } + } else if (ejectingGamePiece) { + state = LEDConstants.CandleState.LIGHT_RED } else if (hasNote) { if (isAutoAiming) { if (!seesTag) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 5b747145..0997e374 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -18,6 +18,7 @@ import com.team4099.robot2023.util.NoteSimulation import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.RepeatCommand import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose3d @@ -789,6 +790,8 @@ class Superstructure( } } SuperstructureStates.EJECT_GAME_PIECE -> { + leds.ejectingGamePiece = true + intake.currentRequest = Request.IntakeRequest.OpenLoop( Intake.TunableIntakeStates.outtakeRolllerVoltage.get(), @@ -800,9 +803,14 @@ class Superstructure( when (currentRequest) { is Request.SuperstructureRequest.Idle -> { + leds.ejectingGamePiece = false currentRequest = Request.SuperstructureRequest.Idle() nextState = SuperstructureStates.IDLE } + is Request.SuperstructureRequest.GroundIntake -> { + leds.ejectingGamePiece = false + nextState = SuperstructureStates.IDLE + } } } SuperstructureStates.EJECT_GAME_PIECE_PREP -> {