diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 9fb9a95..7343b2b 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -239,7 +239,6 @@ object RobotContainer { ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand()) ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand()) - /* ControlBoard.targetAmp.whileTrue( TargetAngleCommand( driver = Jessika(), @@ -252,8 +251,6 @@ object RobotContainer { ) ) - */ - ControlBoard.climbAlignFar.whileTrue( runOnce({ setClimbAngle = diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index 3b43cd5..0df27c6 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -48,9 +48,18 @@ object AutonomousSelector { // orientationChooser.addOption("Right", 270.degrees) // autoTab.add("Starting Orientation", orientationChooser) - 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) + 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 + ) /* @@ -184,7 +193,8 @@ object AutonomousSelector { AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST -> return WaitCommand(waitTime.inSeconds) .andThen({ - val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstCenterSide.startingPose) + val flippedPose = + AllianceFlipUtil.apply(FourNoteAutoPathWithFirstCenterSide.startingPose) drivetrain.tempZeroGyroYaw(flippedPose.rotation) drivetrain.resetFieldFrameEstimator(flippedPose) }) @@ -192,7 +202,8 @@ object AutonomousSelector { AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST -> return WaitCommand(waitTime.inSeconds) .andThen({ - val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstSourceSide.startingPose) + val flippedPose = + AllianceFlipUtil.apply(FourNoteAutoPathWithFirstSourceSide.startingPose) drivetrain.tempZeroGyroYaw(flippedPose.rotation) drivetrain.resetFieldFrameEstimator(flippedPose) }) @@ -360,4 +371,4 @@ object AutonomousSelector { SIX_NOTE_AUTO_PATH, SIX_NOTE_WITH_PICKUP_PATH } -} \ No newline at end of file +} 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 c9c8f58..ce9f51b 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt @@ -36,8 +36,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru startingPose.rotation.inRotation2ds ), FieldWaypoint( - Translation2d(2.91.meters - 0.75.meters, 6.82.meters) - .translation2d, + Translation2d(2.91.meters - 0.75.meters, 6.82.meters).translation2d, null, 180.degrees.inRotation2ds, ), @@ -111,7 +110,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru FieldWaypoint( Translation2d( ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + - 0.25.meters, + 0.25.meters, 5.55.meters ) .translation2d, @@ -127,7 +126,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru FieldWaypoint( Translation2d( ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + - 0.25.meters, + 0.25.meters, 5.45.meters ) .translation2d, @@ -183,4 +182,4 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru companion object { val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees) } -} \ No newline at end of file +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt index 285831b..5346968 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt @@ -16,171 +16,172 @@ 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) +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 - ) - } + 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 ) - .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 - ) - ) - } + .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 ) - .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()) + .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) - } -} \ No newline at end of file + 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 index 0cb2455..6ce98ea 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstSourceSide.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstSourceSide.kt @@ -16,172 +16,172 @@ 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) +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 - ) - } + 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 ) - .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 - ) - ) - } + .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 ) - .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()) + .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) - } -} \ No newline at end of file + 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/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 2fe5292..a77b993 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -56,7 +56,7 @@ object ControlBoard { // val intake = Trigger { driver.rightShoulderButton} val targetAmp = Trigger { driver.aButton } - val prepAmp = Trigger { driver.aButton } + val prepAmp = Trigger { operator.aButton } val prepLow = Trigger { operator.xButton } val prepHighProtected = Trigger { operator.bButton } val prepHigh = Trigger { operator.yButton } 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 e6fc39f..0fedc97 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -455,7 +455,7 @@ class Superstructure( } SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK -> { feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-2.volts) - intake.currentRequest = Request.IntakeRequest.OpenLoop(-0.5.volts, 0.0.volts) + intake.currentRequest = Request.IntakeRequest.OpenLoop(-0.75.volts, 0.0.volts) if (!feeder.hasNote) { nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD @@ -468,8 +468,8 @@ class Superstructure( } } SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD -> { - feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(2.0.volts) - intake.currentRequest = Request.IntakeRequest.OpenLoop(0.25.volts, 0.0.volts) + feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(1.5.volts) + intake.currentRequest = Request.IntakeRequest.OpenLoop(0.5.volts, 0.0.volts) if (feeder.hasNote || RobotBase.isSimulation()) { currentRequest = Request.SuperstructureRequest.Idle() nextState = SuperstructureStates.IDLE