diff --git a/ForTeaching/build.gradle b/ForTeaching/build.gradle index 020d362c..8b655dd0 100644 --- a/ForTeaching/build.gradle +++ b/ForTeaching/build.gradle @@ -26,8 +26,8 @@ android { dependencies { implementation project(':FtcRobotController') // Uncomment this to use a local version of TechnoLib - // implementation project(':RobotLibrary') // FLIP: TechnoLibLocal - // implementation project(':Path') // FLIP: TechnoLibLocal - // implementation project(':Vision') // FLIP: TechnoLibLocal + implementation project(':RobotLibrary') // FLIP: TechnoLibLocal + implementation project(':Path') // FLIP: TechnoLibLocal + implementation project(':Vision') // FLIP: TechnoLibLocal annotationProcessor files('lib/OpModeAnnotationProcessor.jar') } diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/CloseClawCommand.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/CloseClawCommand.java deleted file mode 100644 index f6df3629..00000000 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/CloseClawCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.forteaching.TechnoBot.Commands; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.ClawSubsystem; - -public class CloseClawCommand implements Command { - - private ClawSubsystem clawSubsystem; - - public CloseClawCommand(ClawSubsystem claw) { - addRequirements(claw); - clawSubsystem = claw; - } - - @Override - public void execute() { - clawSubsystem.close(); - } -} diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/Commands.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/Commands.java new file mode 100644 index 00000000..58c80de3 --- /dev/null +++ b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/Commands.java @@ -0,0 +1,68 @@ +package org.firstinspires.ftc.forteaching.TechnoBot.Commands; + +import com.technototes.library.command.SimpleCommand; +import com.technototes.library.command.SimpleRequiredCommand; + +import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.ClawSubsystem; +import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.LiftSubsystem; +import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.MotorAsServoSubsystem; +import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.MovementTestingSubsystem; + +public class Commands { + public static class Claw { + public static SimpleRequiredCommand open(ClawSubsystem clawSubsystem) { + return new SimpleRequiredCommand<>(clawSubsystem, ClawSubsystem::open); + } + public static SimpleRequiredCommand close(ClawSubsystem clawSubsystem) { + return new SimpleRequiredCommand<>(clawSubsystem, ClawSubsystem::close); + } + } + + public static class Lift { + public static SimpleRequiredCommand moveUp(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::moveUp); + } + public static SimpleRequiredCommand moveDown(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::moveUp); + } + } + + public static class MotorAsServo { + public static SimpleRequiredCommand increaseEncMotor(MotorAsServoSubsystem m) { + return new SimpleRequiredCommand<>(m, MotorAsServoSubsystem::increaseEncMotor); + } + public static SimpleRequiredCommand decreaseEncMotor(MotorAsServoSubsystem m) { + return new SimpleRequiredCommand<>(m, MotorAsServoSubsystem::decreaseEncMotor); + } + public static SimpleRequiredCommand stop(MotorAsServoSubsystem m) { + return new SimpleRequiredCommand<>(m, MotorAsServoSubsystem::stop); + } + public static SimpleRequiredCommand halt(MotorAsServoSubsystem m) { + return new SimpleRequiredCommand<>(m, MotorAsServoSubsystem::halt); + } + } + + public static class MovementTesting { + public static SimpleRequiredCommand incMotorSpeed(MovementTestingSubsystem m) { + return new SimpleRequiredCommand<>(m, MovementTestingSubsystem::increaseMotorSpeed); + } + public static SimpleRequiredCommand decMotorSpeed(MovementTestingSubsystem m) { + return new SimpleRequiredCommand<>(m, MovementTestingSubsystem::decreaseMotorSpeed); + } + public static SimpleRequiredCommand incServoSpeed(MovementTestingSubsystem m) { + return new SimpleRequiredCommand<>(m, MovementTestingSubsystem::increaseServoMotor); + } + public static SimpleRequiredCommand decServoSpeed(MovementTestingSubsystem m) { + return new SimpleRequiredCommand<>(m, MovementTestingSubsystem::decreaseServoMotor); + } + public static SimpleCommand neutralMotorSpeed(MovementTestingSubsystem m) { + return new SimpleCommand(m::neutralMotorSpeed); + } + public static SimpleCommand neutralServoSpeed(MovementTestingSubsystem m) { + return new SimpleCommand(m::neutralServoMotor); + } + public static SimpleCommand brakeMotor(MovementTestingSubsystem m) { + return new SimpleCommand(m::brakeMotor); + } + } +} diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/LiftDownCommand.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/LiftDownCommand.java deleted file mode 100644 index 4a9ed5a3..00000000 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/LiftDownCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.forteaching.TechnoBot.Commands; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.LiftSubsystem; - -public class LiftDownCommand implements Command { - - public LiftSubsystem lift; - - public LiftDownCommand(LiftSubsystem liftSubsystem) { - addRequirements(liftSubsystem); - lift = liftSubsystem; - } - - @Override - public void execute() { - lift.moveDown(); - } -} diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/LiftUpCommand.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/LiftUpCommand.java deleted file mode 100644 index 45228b0a..00000000 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/LiftUpCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.forteaching.TechnoBot.Commands; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.LiftSubsystem; - -public class LiftUpCommand implements Command { - - public LiftSubsystem lift; - - public LiftUpCommand(LiftSubsystem liftSubsystem) { - addRequirements(liftSubsystem); - lift = liftSubsystem; - } - - @Override - public void execute() { - lift.moveUp(); - } -} diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/OpenClawCommand.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/OpenClawCommand.java deleted file mode 100644 index 3135824a..00000000 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/OpenClawCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.forteaching.TechnoBot.Commands; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.ClawSubsystem; - -public class OpenClawCommand implements Command { - - private ClawSubsystem clawSubsystem; - - public OpenClawCommand(ClawSubsystem claw) { - addRequirements(claw); - clawSubsystem = claw; - } - - @Override - public void execute() { - clawSubsystem.open(); - } -} diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/Operations.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/Operations.java deleted file mode 100644 index 69fd7019..00000000 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/Operations.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.forteaching.TechnoBot.Commands; - -public enum Operations { - Increase, - Decrease, - Stop, - Halt, -} diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/TestEncodedMotorCommand.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/TestEncodedMotorCommand.java deleted file mode 100644 index d4c06efd..00000000 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/TestEncodedMotorCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.forteaching.TechnoBot.Commands; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.MotorAsServoSubsystem; - -public class TestEncodedMotorCommand implements Command { - - MotorAsServoSubsystem motorAsServo; - Operations which; - - public TestEncodedMotorCommand(MotorAsServoSubsystem ss, Operations op) { - motorAsServo = ss; - addRequirements(motorAsServo); - which = op; - } - - @Override - public void execute() { - if (which == Operations.Decrease) { - motorAsServo.decreaseEncMotor(); - } else if (which == Operations.Increase) { - motorAsServo.increaseEncMotor(); - } else if (which == Operations.Stop) { - motorAsServo.stopEncMotor(); - } else if (which == Operations.Halt) { - motorAsServo.halt(); - } else { - // Shouldn't ever get here: - motorAsServo.halt(); - } - } -} diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/TestMotorCommand.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/TestMotorCommand.java deleted file mode 100644 index 7c312879..00000000 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/TestMotorCommand.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.firstinspires.ftc.forteaching.TechnoBot.Commands; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.MovementTestingSubsystem; - -public class TestMotorCommand implements Command { - - MovementTestingSubsystem movement; - Operations which; - - public TestMotorCommand(MovementTestingSubsystem ss, Operations op) { - movement = ss; - addRequirements(movement); - which = op; - } - - @Override - public void execute() { - switch (which) { - case Increase: - movement.increaseMotorSpeed(); - break; - case Decrease: - movement.decreaseMotorSpeed(); - break; - case Stop: - movement.neutralMotorSpeed(); - break; - case Halt: - movement.brakeMotor(); - break; - } - } -} diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/TestServoCommand.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/TestServoCommand.java deleted file mode 100644 index 488b050a..00000000 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Commands/TestServoCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.forteaching.TechnoBot.Commands; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.forteaching.TechnoBot.Subsystems.MovementTestingSubsystem; - -public class TestServoCommand implements Command { - - MovementTestingSubsystem movement; - Operations which; - - public TestServoCommand(MovementTestingSubsystem ss, Operations op) { - movement = ss; - addRequirements(movement); - which = op; - } - - @Override - public void execute() { - switch (which) { - case Increase: - movement.increaseServoMotor(); - break; - case Decrease: - movement.decreaseServoMotor(); - break; - case Stop: - case Halt: - movement.neutralServoMotor(); - break; - } - } -} diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Controls.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Controls.java index ec1af255..f2f2d0ca 100644 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Controls.java +++ b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/Controls.java @@ -7,15 +7,9 @@ import com.technototes.library.control.CommandGamepad; import com.technototes.library.control.Stick; import com.technototes.library.util.Alliance; -import org.firstinspires.ftc.forteaching.TechnoBot.Commands.CloseClawCommand; -import org.firstinspires.ftc.forteaching.TechnoBot.Commands.LiftDownCommand; -import org.firstinspires.ftc.forteaching.TechnoBot.Commands.LiftUpCommand; + +import org.firstinspires.ftc.forteaching.TechnoBot.Commands.Commands; import org.firstinspires.ftc.forteaching.TechnoBot.Commands.MecDriveCommand; -import org.firstinspires.ftc.forteaching.TechnoBot.Commands.OpenClawCommand; -import org.firstinspires.ftc.forteaching.TechnoBot.Commands.Operations; -import org.firstinspires.ftc.forteaching.TechnoBot.Commands.TestEncodedMotorCommand; -import org.firstinspires.ftc.forteaching.TechnoBot.Commands.TestMotorCommand; -import org.firstinspires.ftc.forteaching.TechnoBot.Commands.TestServoCommand; public class Controls { @@ -94,65 +88,52 @@ public Controls(CommandGamepad gpad, TheBot bot, Alliance ally) { } public void bindClawControls() { - openClaw.whenPressed(new OpenClawCommand(robot.clawSubsystem)); - closeClaw.whenPressed(new CloseClawCommand(robot.clawSubsystem)); + openClaw.whenPressed(Commands.Claw.open(robot.clawSubsystem)); + closeClaw.whenPressed(Commands.Claw.close(robot.clawSubsystem)); } public void bindLiftControls() { - liftUp.whenPressed(new LiftUpCommand(robot.liftSubsystem)); - liftDown.whenPressed(new LiftDownCommand(robot.liftSubsystem)); + liftUp.whenPressed(Commands.Lift.moveUp(robot.liftSubsystem)); + liftDown.whenPressed(Commands.Lift.moveDown(robot.liftSubsystem)); } // Joysticks require a "scheduleJoystick" thing, so the commands are invoked all the time private void bindDrivebaseControls() { CommandScheduler - .getInstance() - .scheduleJoystick( - // new TankDriveCommand(robot.tankDriveBase, leftTankStick, rightTankStick, snapToAngle) - new MecDriveCommand(robot.mecanumDrivebase, leftMecDriveStick, rightMecDriveStick) - ); - } - - // Silly helpers to make the code more succinct below - private TestEncodedMotorCommand MakeEncCommand(Operations op) { - return new TestEncodedMotorCommand(robot.encodedMotorSubsystem, op); - } - - private TestMotorCommand MakeMotorCommand(Operations op) { - return new TestMotorCommand(robot.movementTestingSubsystem, op); - } - private TestServoCommand MakeServoCommand(Operations op) { - return new TestServoCommand(robot.movementTestingSubsystem, op); + .scheduleJoystick( + // new TankDriveCommand(robot.tankDriveBase, leftTankStick, rightTankStick, snapToAngle) + new MecDriveCommand(robot.mecanumDrivebase, leftMecDriveStick, rightMecDriveStick) + ); } private void bindTesterControls() { - encMotorTestUp.whenPressed(MakeEncCommand(Operations.Increase)); - encMotorTestDown.whenPressed(MakeEncCommand(Operations.Decrease)); + encMotorTestUp.whenPressed(Commands.MotorAsServo.increaseEncMotor(robot.encodedMotorSubsystem)); + encMotorTestDown.whenPressed(Commands.MotorAsServo.decreaseEncMotor(robot.encodedMotorSubsystem)); - motorTestUp.whenPressed(MakeMotorCommand(Operations.Increase)); - motorTestDown.whenPressed(MakeMotorCommand(Operations.Decrease)); + motorTestUp.whenPressed(Commands.MovementTesting.incMotorSpeed(robot.movementTestingSubsystem)); + motorTestDown.whenPressed(Commands.MovementTesting.decMotorSpeed(robot.movementTestingSubsystem)); - servoTestUp.whenPressed(MakeServoCommand(Operations.Increase)); - servoTestDown.whenPressed(MakeServoCommand(Operations.Decrease)); + servoTestUp.whenPressed(Commands.MovementTesting.incServoSpeed(robot.movementTestingSubsystem)); + servoTestDown.whenPressed(Commands.MovementTesting.decServoSpeed(robot.movementTestingSubsystem)); // For this stuff, ParallelCommandGroups or SequentialCommandGroups both work the same // For command that take "time" you parallel command groups (as they will run at the same // time as the other commands) but if you want "do A, then do B, then do C" you should // use a sequential command group stop.whenPressed( - new ParallelCommandGroup( - MakeEncCommand(Operations.Stop), - MakeMotorCommand(Operations.Stop), - MakeServoCommand(Operations.Stop) - ) + new ParallelCommandGroup( + Commands.MovementTesting.neutralMotorSpeed(robot.movementTestingSubsystem), + Commands.MovementTesting.neutralServoSpeed(robot.movementTestingSubsystem), + Commands.MotorAsServo.stop(robot.encodedMotorSubsystem) + ) ); halt.whenPressed( - new ParallelCommandGroup( - MakeMotorCommand(Operations.Halt), - MakeEncCommand(Operations.Halt), - MakeServoCommand(Operations.Halt) - ) + new ParallelCommandGroup( + Commands.MovementTesting.brakeMotor(robot.movementTestingSubsystem), + Commands.MovementTesting.neutralServoSpeed(robot.movementTestingSubsystem), + Commands.MotorAsServo.halt(robot.encodedMotorSubsystem) + ) ); } } diff --git a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/OpModes/TechnoManualControl.java b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/OpModes/TechnoManualControl.java index af94df08..bca383f1 100644 --- a/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/OpModes/TechnoManualControl.java +++ b/ForTeaching/src/main/java/org/firstinspires/ftc/forteaching/TechnoBot/OpModes/TechnoManualControl.java @@ -25,7 +25,7 @@ public void uponInit() { controls = new Controls(driverGamepad, robot, Alliance.RED); if (TheBot.Connected.Camera) CommandScheduler - .getInstance() + .scheduleInit(new VisionCommand(robot.visionSystem, Alliance.RED)); } } diff --git a/Sixteen750/build.gradle b/Sixteen750/build.gradle index bcf2bf23..7b8044bb 100644 --- a/Sixteen750/build.gradle +++ b/Sixteen750/build.gradle @@ -26,8 +26,8 @@ android { dependencies { implementation project(':FtcRobotController') // Uncomment this to use a local version of TechnoLib - // implementation project(':RobotLibrary') // FLIP: TechnoLibLocal - // implementation project(':Path') // FLIP: TechnoLibLocal - // implementation project(':Vision') // FLIP: TechnoLibLocal + implementation project(':RobotLibrary') // FLIP: TechnoLibLocal + implementation project(':Path') // FLIP: TechnoLibLocal + implementation project(':Vision') // FLIP: TechnoLibLocal annotationProcessor files('lib/OpModeAnnotationProcessor.jar') } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/ControlsDriver.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/ControlsDriver.java index 659b3e94..b83e5f57 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/ControlsDriver.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/ControlsDriver.java @@ -71,7 +71,7 @@ public ControlsDriver(CommandGamepad g, Robot r, Robot.SubsystemCombo combo) { public void bindMecanumDriveControls() { // Probably not a good idea to bind the drive controls to more than one gamepad CommandScheduler - .getInstance() + .scheduleJoystick(new MecanumDriveCommand(robot.mecanumDriveSubsystem, gamepad.leftStick, gamepad.rightStick)); gamepad.leftStickButton.whenPressed(new ApplyTurboModeCommand(robot.mecanumDriveSubsystem)); gamepad.rightStickButton.whenPressed(new ApplyTurboModeCommand(robot.mecanumDriveSubsystem)); @@ -80,7 +80,7 @@ public void bindMecanumDriveControls() { public void bindVisionCommand(){ CommandScheduler - .getInstance() + .scheduleForState(new VisionDuringTeleCommand(robot.visionSubsystem, gamepad.ps_share), CommandOpMode.OpModeState.RUN); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/auto/LeftJustPark.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/auto/LeftJustPark.java index 07651e62..b1e51ea1 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/auto/LeftJustPark.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/auto/LeftJustPark.java @@ -25,15 +25,15 @@ public void uponInit() { hardware = new Hardware(hardwareMap, Robot.SubsystemCombo.VISION_M_DRIVE); robot = new Robot(hardware, Robot.SubsystemCombo.VISION_M_DRIVE, Alliance.BLUE, StartingPosition.AWAY); robot.mecanumDriveSubsystem.setPoseEstimate(AutoConstantsBlue.Away.START.toPose()); - CommandScheduler.getInstance() + CommandScheduler .scheduleForState( new SequentialCommandGroup( new LeftParkingSelectionCommandJustPark( robot.visionSubsystem, robot.mecanumDriveSubsystem), - CommandScheduler.getInstance()::terminateOpMode), + CommandScheduler::terminateOpMode), CommandOpMode.OpModeState.RUN); if (Robot.RobotConstant.CAMERA_ENABLED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSubsystem)); + CommandScheduler.scheduleInit(new VisionCommand(robot.visionSubsystem)); } } } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/auto/RightJustPark.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/auto/RightJustPark.java index 53820a3c..010bfa2a 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/auto/RightJustPark.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/auto/RightJustPark.java @@ -25,15 +25,15 @@ public void uponInit() { hardware = new Hardware(hardwareMap, Robot.SubsystemCombo.VISION_M_DRIVE); robot = new Robot(hardware, Robot.SubsystemCombo.VISION_M_DRIVE, Alliance.BLUE, StartingPosition.HOME); robot.mecanumDriveSubsystem.setPoseEstimate(AutoConstantsBlue.Home.START.toPose()); - CommandScheduler.getInstance() + CommandScheduler .scheduleForState( new SequentialCommandGroup( new RightParkingSelectionCommandJustPark( robot.visionSubsystem, robot.mecanumDriveSubsystem), - CommandScheduler.getInstance()::terminateOpMode), + CommandScheduler::terminateOpMode), CommandOpMode.OpModeState.RUN); if (Robot.RobotConstant.CAMERA_ENABLED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSubsystem)); + CommandScheduler.scheduleInit(new VisionCommand(robot.visionSubsystem)); } } } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/diagnosis/TeleVisionTest.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/diagnosis/TeleVisionTest.java index 4aca0912..835cbc23 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/diagnosis/TeleVisionTest.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmode/diagnosis/TeleVisionTest.java @@ -35,7 +35,7 @@ public void uponInit() { coDriverControls = new ControlsCoDriver(codriverGamepad, robot, Robot.SubsystemCombo.DEFAULT); telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); if (false && Robot.RobotConstant.CAMERA_ENABLED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSubsystem)); + CommandScheduler.scheduleInit(new VisionCommand(robot.visionSubsystem)); } } diff --git a/Swerveteen750/build.gradle b/Swerveteen750/build.gradle index 678d7fcb..6d7e11d5 100644 --- a/Swerveteen750/build.gradle +++ b/Swerveteen750/build.gradle @@ -32,8 +32,8 @@ android { dependencies { implementation project(':FtcRobotController') // Uncomment this to use a local version of TechnoLib - // implementation project(':RobotLibrary') // FLIP: TechnoLibLocal - // implementation project(':Path') // FLIP: TechnoLibLocal - // implementation project(':Vision') // FLIP: TechnoLibLocal + implementation project(':RobotLibrary') // FLIP: TechnoLibLocal + implementation project(':Path') // FLIP: TechnoLibLocal + implementation project(':Vision') // FLIP: TechnoLibLocal annotationProcessor files('lib/OpModeAnnotationProcessor.jar') } diff --git a/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/ControlsDriver.java b/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/ControlsDriver.java index 7f886c18..e0aff457 100644 --- a/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/ControlsDriver.java +++ b/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/ControlsDriver.java @@ -70,7 +70,6 @@ public ControlsDriver(CommandGamepad g, Robot r, Robot.SubsystemCombo combo) { public void bindMecanumDriveControls() { // Probably not a good idea to bind the drive controls to more than one gamepad CommandScheduler - .getInstance() .scheduleJoystick(new MecanumDriveCommand(robot.mecanumDriveSubsystem, gamepad.leftStick, gamepad.rightStick)); gamepad.leftStickButton.whenPressed(new ApplyTurboModeCommand(robot.mecanumDriveSubsystem)); gamepad.rightStickButton.whenPressed(new ApplyTurboModeCommand(robot.mecanumDriveSubsystem)); diff --git a/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/auto/LeftJustPark.java b/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/auto/LeftJustPark.java index f396c258..68a81440 100644 --- a/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/auto/LeftJustPark.java +++ b/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/auto/LeftJustPark.java @@ -25,15 +25,15 @@ public void uponInit() { hardware = new Hardware(hardwareMap, Robot.SubsystemCombo.VISION_M_DRIVE); robot = new Robot(hardwareMap, hardware, Robot.SubsystemCombo.VISION_M_DRIVE, Alliance.BLUE, StartingPosition.AWAY); robot.mecanumDriveSubsystem.setPoseEstimate(AutoConstantsBlue.Away.START.toPose()); - CommandScheduler.getInstance() + CommandScheduler .scheduleForState( new SequentialCommandGroup( new LeftParkingSelectionCommandJustPark( robot.visionSubsystem, robot.mecanumDriveSubsystem), - CommandScheduler.getInstance()::terminateOpMode), + CommandScheduler::terminateOpMode), CommandOpMode.OpModeState.RUN); if (Robot.RobotConstant.CAMERA_ENABLED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSubsystem)); + CommandScheduler.scheduleInit(new VisionCommand(robot.visionSubsystem)); } } } diff --git a/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/auto/RightJustPark.java b/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/auto/RightJustPark.java index 7ad0adf0..cc275dd1 100644 --- a/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/auto/RightJustPark.java +++ b/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/auto/RightJustPark.java @@ -25,15 +25,15 @@ public void uponInit() { hardware = new Hardware(hardwareMap, Robot.SubsystemCombo.VISION_M_DRIVE); robot = new Robot(hardwareMap, hardware, Robot.SubsystemCombo.VISION_M_DRIVE, Alliance.BLUE, StartingPosition.HOME); robot.mecanumDriveSubsystem.setPoseEstimate(AutoConstantsBlue.Home.START.toPose()); - CommandScheduler.getInstance() + CommandScheduler .scheduleForState( new SequentialCommandGroup( new RightParkingSelectionCommandJustPark( robot.visionSubsystem, robot.mecanumDriveSubsystem), - CommandScheduler.getInstance()::terminateOpMode), + CommandScheduler::terminateOpMode), CommandOpMode.OpModeState.RUN); if (Robot.RobotConstant.CAMERA_ENABLED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSubsystem)); + CommandScheduler.scheduleInit(new VisionCommand(robot.visionSubsystem)); } } } diff --git a/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/teleop/RegularMecanumDrive.java b/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/teleop/RegularMecanumDrive.java index 84f3e666..55d5b2f6 100644 --- a/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/teleop/RegularMecanumDrive.java +++ b/Swerveteen750/src/main/java/org/firstinspires/ftc/swerveteen750/opmode/teleop/RegularMecanumDrive.java @@ -38,7 +38,7 @@ public void uponInit() { coDriverControls = new ControlsCoDriver(codriverGamepad, robot, Robot.SubsystemCombo.DEFAULT); telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); if (Robot.RobotConstant.CAMERA_ENABLED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSubsystem)); + CommandScheduler.scheduleInit(new VisionCommand(robot.visionSubsystem)); } } diff --git a/Twenty403/build.gradle b/Twenty403/build.gradle index e57bfe11..b0433daf 100644 --- a/Twenty403/build.gradle +++ b/Twenty403/build.gradle @@ -32,8 +32,8 @@ android { dependencies { implementation project(':FtcRobotController') // Uncomment this to use a local version of TechnoLib - // implementation project(':RobotLibrary') // FLIP: TechnoLibLocal - // implementation project(':Path') // FLIP: TechnoLibLocal - // implementation project(':Vision') // FLIP: TechnoLibLocal + implementation project(':RobotLibrary') // FLIP: TechnoLibLocal + implementation project(':Path') // FLIP: TechnoLibLocal + implementation project(':Vision') // FLIP: TechnoLibLocal annotationProcessor files('lib/OpModeAnnotationProcessor.jar') } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/AutoCmds.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/AutoCmds.java new file mode 100644 index 00000000..a184f04d --- /dev/null +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/AutoCmds.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.twenty403.command; + +import com.technototes.library.command.Command; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; + +public class AutoCmds { + + public static class Right { + + public static Command CycleLeft(Robot r) { + return new SequentialCommandGroup( + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.START_TO_W_JUNCTION + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_STACK_ONE + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)), + Commands.Claw.close(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.STACK_TO_W_JUNCTION_ONE + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_LEFT_PARK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) + ); + } + } +} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/Commands.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/Commands.java new file mode 100644 index 00000000..25df6cfc --- /dev/null +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/Commands.java @@ -0,0 +1,156 @@ +package org.firstinspires.ftc.twenty403.command; + +import android.util.Pair; +import com.technototes.library.command.ChoiceCommand; +import com.technototes.library.command.Command; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.SimpleCommand; +import com.technototes.library.command.SimpleRequiredCommand; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; +// This one has been converted to a local command as an example +// import org.firstinspires.ftc.twenty403.command.autonomous.right.AutoRightCycleLeft; +import org.firstinspires.ftc.twenty403.command.autonomous.right.AutoRightCycleMiddle; +import org.firstinspires.ftc.twenty403.command.autonomous.right.AutoRightCycleRight; +import org.firstinspires.ftc.twenty403.subsystem.ClawSubsystem; +import org.firstinspires.ftc.twenty403.subsystem.DrivebaseSubsystem; +import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; + +public class Commands { + + // If you prefer, we could name commands ClawOpen, instead of Claw.open (or Claw.Open) + // In addition, every autonomous command that just extends a command type, and only + // has a constructor could be moved into this format of command. + // The onlyk problem I see is that this file would become problematic for merge conflicts + // For an example: + public static class Auto { + + public static class Right { + + public static Command ParkingSelectionCycle(Robot r) { + return new ChoiceCommand( + // Note that by putting these things in their respective static classes, + // much of the auto code may be identical, as this "CycleLeft" function + // is calling the Auto.Right.CycleLeft function. We could call it that + // way if we wanted. I could also moving the Auto.Right into a completely + // separate file, so they're invoked as "AutoCmds.Right.ParkingSelectionCycle" + new Pair<>(r.visionSystem::left, CycleLeft(r)), + new Pair<>(r.visionSystem::middle, new AutoRightCycleMiddle(r)), + new Pair<>(r.visionSystem::right, new AutoRightCycleRight(r)) + ); + } + + public static Command CycleLeft(Robot r) { + // This is instead of the AutoRightCycleLeft command + // (it's literally a copy of the constructor of that thing) + return new SequentialCommandGroup( + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.START_TO_W_JUNCTION + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_STACK_ONE + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)), + Commands.Claw.close(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.STACK_TO_W_JUNCTION_ONE + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_LEFT_PARK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) + ); + } + } + } + + public static class Claw { + + public static Command open(ClawSubsystem claw) { + return new SimpleRequiredCommand<>(claw, ClawSubsystem::open); + } + + public static Command close(ClawSubsystem claw) { + return new SimpleRequiredCommand<>(claw, ClawSubsystem::close); + } + + public static Command toggleAutoClose(ClawSubsystem claw) { + return new SimpleRequiredCommand<>(claw, ClawSubsystem::toggleAutoClose); + } + } + + public static class Drive { + + public static Command fast(DrivebaseSubsystem drive) { + return new SimpleCommand(drive::fast); + } + + public static Command slow(DrivebaseSubsystem drive) { + return new SimpleCommand(drive::slow); + } + + public static Command zeroHeading(DrivebaseSubsystem drive) { + return new SimpleCommand(drive::setExternalHeading, 0.0); + } + + public static Command cancelTileRequest(DrivebaseSubsystem drive) { + return new SimpleCommand(drive::requestCancelled); + } + } + + public static class Lift { + + public static Command moveUp(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::moveUp); + } + + public static Command moveDown(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::moveDown); + } + + public static Command moveUp_OVERRIDE(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::moveUp_OVERRIDE); + } + + public static Command moveDown_OVERRIDE(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::moveDown_OVERRIDE); + } + + public static Command collect(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::collect); + } + + public static Command highJunction(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::highPole); + } + + public static Command midJunction(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::midPole); + } + + public static Command lowJunction(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::lowPole); + } + + public static Command groundJunction(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::groundJunction); + } + + public static Command intake(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::intakePos); + } + + public static Command setNewZero(LiftSubsystem lift) { + return new SimpleRequiredCommand<>(lift, LiftSubsystem::setNewZero); + } + } +} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/VisionCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/VisionCommand.java deleted file mode 100644 index d511b42b..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/VisionCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.twenty403.command; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.VisionSubsystem; - -public class VisionCommand implements Command { - - public VisionSubsystem subsystem; - - public VisionCommand(VisionSubsystem s) { - subsystem = s; - addRequirements(subsystem); - } - - @Override - public void initialize() { - subsystem.startVisionPipeline(); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean cancel) { - subsystem.stopVisionPipeline(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/VisionDuringTeleCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/VisionDuringTeleCommand.java deleted file mode 100644 index a74b2751..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/VisionDuringTeleCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.twenty403.command; - -import com.technototes.library.command.Command; -import java.util.function.BooleanSupplier; -import java.util.function.BooleanSupplier; -import org.firstinspires.ftc.twenty403.subsystem.VisionPipeline; -import org.firstinspires.ftc.twenty403.subsystem.VisionSubsystem; - -public class VisionDuringTeleCommand implements Command { - - public VisionSubsystem subsystem; - - public VisionDuringTeleCommand(VisionSubsystem s) { - subsystem = s; - addRequirements(subsystem); - } - - @Override - public void initialize() { - subsystem.startVisionPipeline(); - subsystem.pauseScanning(); - } - - @Override - public void execute() { - if (subsystem.visionPipeline.activeMode == VisionPipeline.Mode.Junction) { - subsystem.pauseScanning(); - } else { - subsystem.startJunctionScanning(); - } - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/AutoConstants.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/AutoConstants.java index 298eec5e..70bdb1c1 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/AutoConstants.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/AutoConstants.java @@ -1,246 +1,114 @@ -package org.firstinspires.ftc.twenty403.command.autonomous; - -import static java.lang.Math.toRadians; - -import java.util.function.Function; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.technototes.path.geometry.ConfigurablePose; -import com.technototes.path.trajectorysequence.TrajectorySequence; -import com.technototes.path.trajectorysequence.TrajectorySequenceBuilder; - -import org.firstinspires.ftc.twenty403.subsystem.OdoSubsystem; - -public class AutoConstants { - @Config - public static class Right { - public static double finalCycleStartTime = 20; - public static ConfigurablePose ZERO = new ConfigurablePose(0, 0, toRadians(90)); - public static ConfigurablePose START = new ConfigurablePose(36, -66, toRadians(90)); - public static ConfigurablePose STRAIGHT = new ConfigurablePose(36, -18, toRadians(90)); - public static ConfigurablePose LEFTSIDE = new ConfigurablePose(-12, -66, toRadians(90)); - public static ConfigurablePose TELESTART = new ConfigurablePose(0, 0, toRadians(90)); - public static ConfigurablePose FORWARD_MOVE = new ConfigurablePose(0, 24, toRadians(90)); - public static ConfigurablePose BACKWARD_MOVE = new ConfigurablePose(0, 24, toRadians(90)); - public static ConfigurablePose LEFT_MOVE = new ConfigurablePose(-24, 0, toRadians(90)); - public static ConfigurablePose RIGHT_MOVE = new ConfigurablePose(24, 0, toRadians(90)); - - public static ConfigurablePose STACK_ONE = new ConfigurablePose(66, -16, toRadians(0)); - public static ConfigurablePose STACK_TWO = new ConfigurablePose(68, -16, toRadians(0)); - public static ConfigurablePose STACK_THREE = new ConfigurablePose(73, -16, toRadians(0)); - //x: 66, y: -16, 0 - public static ConfigurablePose LEFT = new ConfigurablePose(18, -18, toRadians(90)); - public static ConfigurablePose MIDDLE = new ConfigurablePose(41, -16, toRadians(90)); - public static ConfigurablePose RIGHT = new ConfigurablePose(61, -17, toRadians(90)); - public static ConfigurablePose W_JUNCTION_ONE = new ConfigurablePose(29, -13, 1.9); - public static ConfigurablePose W_JUNCTION_TWO = new ConfigurablePose(32, -10, 1.9); - public static ConfigurablePose W_JUNCTION_THREE = new ConfigurablePose(37, -11, 2); - - - //public static ConfigurablePose BETWEEN_START_W_jUNCTION_ONE = new ConfigurablePose(40, -48, toRadians(180)); - public static ConfigurablePose BETWEEN_START_W_JUNCTION = new ConfigurablePose(41, -11, 4); - public static ConfigurablePose BETWEEN_START_W_JUNCTION_TWO = new ConfigurablePose(42, -11, 3.9); - public static ConfigurablePose BETWEEN_W_JUNCTION_STACK = new ConfigurablePose(43, -19, 1.5); - public static ConfigurablePose BETWEEN_STACK_W_JUNCTION = new ConfigurablePose(40, -15, 2.4); - public static ConfigurablePose BETWEEN_START_LEFT = new ConfigurablePose(15, -60, toRadians(90)); - public static ConfigurablePose BETWEEN_START_RIGHT = new ConfigurablePose(60, -60, toRadians(90)); - public static ConfigurablePose TERMINAL = new ConfigurablePose(61, -64, toRadians(180)); - // These are 'trajectory pieces' which should be named like this: - // {STARTING_POSITION}_TO_{ENDING_POSITION} - public static final Function, TrajectorySequence> - TELESTART_TO_FORWARD_MOVE = - b -> b.apply(TELESTART.toPose()) - .lineToLinearHeading(FORWARD_MOVE.toPose()) - .build(), - TELESTART_TO_BACKWARD_MOVE = - b -> b.apply(TELESTART.toPose()) - .lineToLinearHeading(BACKWARD_MOVE.toPose()) - .build(), - TELESTART_TO_LEFT_MOVE = - b -> b.apply(TELESTART.toPose()) - .lineToLinearHeading(LEFT_MOVE.toPose()) - .build(), - TELESTART_TO_RIGHT_MOVE = - b -> b.apply(TELESTART.toPose()) - .lineToLinearHeading(RIGHT_MOVE.toPose()) - .build(), - //Testing Straights - START_TO_STRAIGHT = - b -> b.apply(START.toPose()) - .lineTo(STRAIGHT.toPose().vec()) - .build(), - STRAIGHT_TO_START = - b -> b.apply(STRAIGHT.toPose()) - .lineTo(START.toPose().vec()) - .build(), - START_TO_LEFTSIDE = - b -> b.apply(START.toPose()) - .lineTo(LEFTSIDE.toPose().vec()) - .build(), - LEFTSIDE_TO_START = - b -> b.apply(LEFTSIDE.toPose()) - .lineTo(START.toPose().vec()) - .build(), - - START_TO_W_JUNCTION = - b -> b.apply(START.toPose()) - .lineToLinearHeading(BETWEEN_START_W_JUNCTION.toPose()) - .lineToLinearHeading(W_JUNCTION_ONE.toPose()) - .build(), - W_JUNCTION_TO_STACK_ONE = - b -> b.apply(W_JUNCTION_ONE.toPose()) - .lineToLinearHeading(BETWEEN_W_JUNCTION_STACK.toPose()) - .lineToLinearHeading(STACK_ONE.toPose()) - .build(), - W_JUNCTION_TO_STACK_TWO = - b -> b.apply(W_JUNCTION_TWO.toPose()) - .lineToLinearHeading(BETWEEN_W_JUNCTION_STACK.toPose()) - .lineToLinearHeading(STACK_TWO.toPose()) - .build(), - W_JUNCTION_TO_STACK_THREE = - b -> b.apply(W_JUNCTION_THREE.toPose()) - .lineToLinearHeading(BETWEEN_W_JUNCTION_STACK.toPose()) - .lineToLinearHeading(STACK_THREE.toPose()) - .build(), - STACK_TO_W_JUNCTION_ONE = - b -> b.apply(STACK_ONE.toPose()) - .lineToLinearHeading(BETWEEN_STACK_W_JUNCTION.toPose()) - .lineToLinearHeading(W_JUNCTION_ONE.toPose()) - .build(), - STACK_TO_W_JUNCTION_TWO = - b -> b.apply(STACK_TWO.toPose()) - .lineToLinearHeading(BETWEEN_STACK_W_JUNCTION.toPose()) - .lineToLinearHeading(W_JUNCTION_TWO.toPose()) - .build(), - STACK_TO_W_JUNCTION_THREE = - b -> b.apply(STACK_THREE.toPose()) - .lineToLinearHeading(BETWEEN_STACK_W_JUNCTION.toPose()) - .lineToLinearHeading(W_JUNCTION_THREE.toPose()) - .build(), - W_JUNCTION_TO_LEFT_PARK = - b -> b.apply(W_JUNCTION_THREE.toPose()) - .lineToLinearHeading(BETWEEN_STACK_W_JUNCTION.toPose()) - .lineToLinearHeading(LEFT.toPose()) - .build(), - W_JUNCTION_TO_MIDDLE_PARK = - b -> b.apply(W_JUNCTION_THREE.toPose()) - .lineToLinearHeading(BETWEEN_STACK_W_JUNCTION.toPose()) - .lineToLinearHeading(MIDDLE.toPose()) - .build(), - W_JUNCTION_TO_RIGHT_PARK = - b -> b.apply(W_JUNCTION_THREE.toPose()) - .lineToLinearHeading(BETWEEN_STACK_W_JUNCTION.toPose()) - .lineToLinearHeading(RIGHT.toPose()) - .build(), - - - START_TO_LEFT_PARK = - b -> b.apply(START.toPose()) - //.lineToLinearHeading(TERMINAL.toPose()) - .lineToLinearHeading(BETWEEN_START_LEFT.toPose()) - .lineToLinearHeading(LEFT.toPose()) - .build(), - START_TO_MIDDLE_PARK = - b -> b.apply(START.toPose()) - //.lineToLinearHeading(TERMINAL.toPose()) - //.lineToLinearHeading(START.toPose()) - .lineToLinearHeading(MIDDLE.toPose()) - .build(), - START_TO_RIGHT_PARK = - b -> b.apply(START.toPose()) - //.lineToLinearHeading(TERMINAL.toPose()) - .lineToLinearHeading(BETWEEN_START_RIGHT.toPose()) - .lineToLinearHeading(RIGHT.toPose()) - .build(); - - - } - - @Config - public static class Left { - public static ConfigurablePose START = new ConfigurablePose(-36, -66, toRadians(90)); - public static ConfigurablePose E_JUNCTION = new ConfigurablePose(-29, -10, 1.1); - public static ConfigurablePose E_JUNCTION_2 = new ConfigurablePose(-31, -8, 1.1); - public static ConfigurablePose STACK = new ConfigurablePose(-62, -17, toRadians(180)); - public static ConfigurablePose STACK2 = new ConfigurablePose(-65, -17, toRadians(180)); - public static ConfigurablePose LEFT = new ConfigurablePose(-59, -20, toRadians(90)); - public static ConfigurablePose LEFT2 = new ConfigurablePose(-58, -18, toRadians(180)); - //-58,-20,90 - public static ConfigurablePose MIDDLE = new ConfigurablePose(-35, -17, toRadians(90)); - public static ConfigurablePose RIGHT = new ConfigurablePose(-16, -15, toRadians(90)); - public static ConfigurablePose BETWEEN_START_E_JUNCTION = new ConfigurablePose(-36, -12, 5.9); - public static ConfigurablePose BETWEEN_E_JUNCTION_STACK = new ConfigurablePose(-36, -14, 3.14159); - public static ConfigurablePose BETWEEN_STACK_E_JUNCTION = new ConfigurablePose(-38, -13, 1.3089969389957472); - public static ConfigurablePose BETWEEN_START_LEFT = new ConfigurablePose(-58, -60, toRadians(90)); - public static ConfigurablePose BETWEEN_START_RIGHT = new ConfigurablePose(-14, -60, toRadians(90)); - public static ConfigurablePose TERMINAL = new ConfigurablePose(-61, -64, toRadians(180)); - - public static final Function, TrajectorySequence> - START_TO_E_JUNCTION = - b -> b.apply(START.toPose()) - .lineToLinearHeading(BETWEEN_START_E_JUNCTION.toPose()) - .lineToLinearHeading(E_JUNCTION.toPose()) - .build(), - E_JUNCTION_TO_STACK = - b -> b.apply(E_JUNCTION.toPose()) - .lineToLinearHeading(BETWEEN_E_JUNCTION_STACK.toPose()) - .lineToLinearHeading(STACK.toPose()) - .build(), - STACK_TO_E_JUNCTION = - b -> b.apply(STACK.toPose()) - .lineToLinearHeading(BETWEEN_STACK_E_JUNCTION.toPose()) - .lineToLinearHeading(E_JUNCTION.toPose()) - .build(), - STACK_TO_E_JUNCTION_TWO = - b -> b.apply(STACK.toPose()) - .lineToLinearHeading(BETWEEN_STACK_E_JUNCTION.toPose()) - .lineToLinearHeading(E_JUNCTION_2.toPose()) - .build(), - E_JUNCTION_TO_STACK_TWO = - b -> b.apply(E_JUNCTION_2.toPose()) - .lineToLinearHeading(BETWEEN_E_JUNCTION_STACK.toPose()) - .lineToLinearHeading(STACK2.toPose()) - .build(), - E_JUNCTION_TO_LEFT_PARK = - b -> b.apply(E_JUNCTION.toPose()) - .lineToLinearHeading(BETWEEN_STACK_E_JUNCTION.toPose()) - .lineToLinearHeading(LEFT.toPose()) - .build(), - E_JUNCTION_TO_MIDDLE_PARK = - b -> b.apply(E_JUNCTION.toPose()) - .lineToLinearHeading(BETWEEN_STACK_E_JUNCTION.toPose()) - .lineToLinearHeading(MIDDLE.toPose()) - .build(), - E_JUNCTION_TO_RIGHT_PARK = - b -> b.apply(E_JUNCTION.toPose()) - .lineToLinearHeading(BETWEEN_STACK_E_JUNCTION.toPose()) - .lineToLinearHeading(RIGHT.toPose()) - .build(), - LEFT_1_2 = - b -> b.apply(LEFT.toPose()) - .lineToLinearHeading(LEFT2.toPose()) - .lineToLinearHeading(LEFT.toPose()) - .build(), - - START_TO_LEFT_PARK = - b -> b.apply(START.toPose()) - //.lineToLinearHeading(TERMINAL.toPose()) - .lineToLinearHeading(BETWEEN_START_LEFT.toPose()) - .lineToLinearHeading(LEFT.toPose()) - .build(), - START_TO_MIDDLE_PARK = - b -> b.apply(START.toPose()) - //.lineToLinearHeading(TERMINAL.toPose()) - //.lineToLinearHeading(START.toPose()) - .lineToLinearHeading(MIDDLE.toPose()) - .build(), - - START_TO_RIGHT_PARK = - b -> b.apply(START.toPose()) - //.lineToLinearHeading(TERMINAL.toPose()) - .lineToLinearHeading(BETWEEN_START_RIGHT.toPose()) - .lineToLinearHeading(RIGHT.toPose()) - .build(); - } -} +package org.firstinspires.ftc.twenty403.command.autonomous; + +import static java.lang.Math.toRadians; + +import java.util.function.Function; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.technototes.path.geometry.ConfigurablePose; +import com.technototes.path.trajectorysequence.TrajectoryPath; +import com.technototes.path.trajectorysequence.TrajectorySequence; +import com.technototes.path.trajectorysequence.TrajectorySequenceBuilder; + +import org.firstinspires.ftc.twenty403.subsystem.OdoSubsystem; + +public class AutoConstants { + @Config + public static class Right { + public static double finalCycleStartTime = 20; + public static ConfigurablePose ZERO = new ConfigurablePose(0, 0, toRadians(90)); + public static ConfigurablePose START = new ConfigurablePose(36, -66, toRadians(90)); + public static ConfigurablePose STRAIGHT = new ConfigurablePose(36, -18, toRadians(90)); + public static ConfigurablePose LEFTSIDE = new ConfigurablePose(-12, -66, toRadians(90)); + public static ConfigurablePose TELESTART = new ConfigurablePose(0, 0, toRadians(90)); + public static ConfigurablePose FORWARD_MOVE = new ConfigurablePose(0, 24, toRadians(90)); + public static ConfigurablePose BACKWARD_MOVE = new ConfigurablePose(0, 24, toRadians(90)); + public static ConfigurablePose LEFT_MOVE = new ConfigurablePose(-24, 0, toRadians(90)); + public static ConfigurablePose RIGHT_MOVE = new ConfigurablePose(24, 0, toRadians(90)); + + public static ConfigurablePose STACK_ONE = new ConfigurablePose(66, -16, toRadians(0)); + public static ConfigurablePose STACK_TWO = new ConfigurablePose(68, -16, toRadians(0)); + public static ConfigurablePose STACK_THREE = new ConfigurablePose(73, -16, toRadians(0)); + //x: 66, y: -16, 0 + public static ConfigurablePose LEFT = new ConfigurablePose(18, -18, toRadians(90)); + public static ConfigurablePose MIDDLE = new ConfigurablePose(41, -16, toRadians(90)); + public static ConfigurablePose RIGHT = new ConfigurablePose(61, -17, toRadians(90)); + public static ConfigurablePose W_JUNCTION_ONE = new ConfigurablePose(29, -13, 1.9); + public static ConfigurablePose W_JUNCTION_TWO = new ConfigurablePose(32, -10, 1.9); + public static ConfigurablePose W_JUNCTION_THREE = new ConfigurablePose(37, -11, 2); + + + //public static ConfigurablePose BETWEEN_START_W_jUNCTION_ONE = new ConfigurablePose(40, -48, toRadians(180)); + public static ConfigurablePose BETWEEN_START_W_JUNCTION = new ConfigurablePose(41, -11, 4); + public static ConfigurablePose BETWEEN_START_W_JUNCTION_TWO = new ConfigurablePose(42, -11, 3.9); + public static ConfigurablePose BETWEEN_W_JUNCTION_STACK = new ConfigurablePose(43, -19, 1.5); + public static ConfigurablePose BETWEEN_STACK_W_JUNCTION = new ConfigurablePose(40, -15, 2.4); + public static ConfigurablePose BETWEEN_START_LEFT = new ConfigurablePose(15, -60, toRadians(90)); + public static ConfigurablePose BETWEEN_START_RIGHT = new ConfigurablePose(60, -60, toRadians(90)); + public static ConfigurablePose TERMINAL = new ConfigurablePose(61, -64, toRadians(180)); + // These are 'trajectory pieces' which should be named like this: + // {STARTING_POSITION}_TO_{ENDING_POSITION} + public static final TrajectoryPath + TELESTART_TO_FORWARD_MOVE = TrajectoryPath.lineToLinearHeading(TELESTART, FORWARD_MOVE), + TELESTART_TO_BACKWARD_MOVE = TrajectoryPath.lineToLinearHeading(TELESTART, BACKWARD_MOVE), + TELESTART_TO_LEFT_MOVE = TrajectoryPath.lineToLinearHeading(TELESTART, LEFT_MOVE), + TELESTART_TO_RIGHT_MOVE = TrajectoryPath.lineToLinearHeading(TELESTART, RIGHT_MOVE); + //Testing Straights + public static final TrajectoryPath + START_TO_STRAIGHT = TrajectoryPath.lineTo(START, STRAIGHT), + STRAIGHT_TO_START = TrajectoryPath.lineTo(STRAIGHT, START), + START_TO_LEFTSIDE = TrajectoryPath.lineTo(START, LEFTSIDE), + LEFTSIDE_TO_START = TrajectoryPath.lineTo(LEFTSIDE, START); + public static final TrajectoryPath + START_TO_W_JUNCTION = TrajectoryPath.linesToLinearHeadings(START, BETWEEN_START_W_JUNCTION, W_JUNCTION_ONE), + W_JUNCTION_TO_STACK_ONE = TrajectoryPath.linesToLinearHeadings(W_JUNCTION_ONE, BETWEEN_W_JUNCTION_STACK, STACK_ONE), + W_JUNCTION_TO_STACK_TWO = TrajectoryPath.linesToLinearHeadings(W_JUNCTION_TWO, BETWEEN_W_JUNCTION_STACK, STACK_TWO), + W_JUNCTION_TO_STACK_THREE = TrajectoryPath.linesToLinearHeadings(W_JUNCTION_THREE, BETWEEN_W_JUNCTION_STACK, STACK_THREE), + STACK_TO_W_JUNCTION_ONE = TrajectoryPath.linesToLinearHeadings(STACK_ONE, BETWEEN_STACK_W_JUNCTION, W_JUNCTION_ONE), + STACK_TO_W_JUNCTION_TWO = TrajectoryPath.linesToLinearHeadings(STACK_TWO, BETWEEN_STACK_W_JUNCTION, W_JUNCTION_TWO), + STACK_TO_W_JUNCTION_THREE = TrajectoryPath.linesToLinearHeadings(STACK_THREE, BETWEEN_STACK_W_JUNCTION, W_JUNCTION_THREE), + W_JUNCTION_TO_LEFT_PARK = TrajectoryPath.linesToLinearHeadings(W_JUNCTION_THREE, BETWEEN_STACK_W_JUNCTION, LEFT), + W_JUNCTION_TO_MIDDLE_PARK = TrajectoryPath.linesToLinearHeadings(W_JUNCTION_THREE, BETWEEN_STACK_W_JUNCTION, MIDDLE), + W_JUNCTION_TO_RIGHT_PARK = TrajectoryPath.linesToLinearHeadings(W_JUNCTION_THREE, BETWEEN_STACK_W_JUNCTION, RIGHT); + public static final TrajectoryPath + START_TO_LEFT_PARK = TrajectoryPath.linesToLinearHeadings(START, /* TERMINAL, */ BETWEEN_START_LEFT, LEFT), + START_TO_MIDDLE_PARK = TrajectoryPath.linesToLinearHeadings(START, /* TERMINAL, START, */ MIDDLE), + START_TO_RIGHT_PARK = TrajectoryPath.linesToLinearHeadings(START, /* TERMINAL, */ BETWEEN_START_RIGHT, RIGHT); + + } + + @Config + public static class Left { + public static ConfigurablePose START = new ConfigurablePose(-36, -66, toRadians(90)); + public static ConfigurablePose E_JUNCTION = new ConfigurablePose(-29, -10, 1.1); + public static ConfigurablePose E_JUNCTION_2 = new ConfigurablePose(-31, -8, 1.1); + public static ConfigurablePose STACK = new ConfigurablePose(-62, -17, toRadians(180)); + public static ConfigurablePose STACK2 = new ConfigurablePose(-65, -17, toRadians(180)); + public static ConfigurablePose LEFT = new ConfigurablePose(-59, -20, toRadians(90)); + public static ConfigurablePose LEFT2 = new ConfigurablePose(-58, -18, toRadians(180)); + //-58,-20,90 + public static ConfigurablePose MIDDLE = new ConfigurablePose(-35, -17, toRadians(90)); + public static ConfigurablePose RIGHT = new ConfigurablePose(-16, -15, toRadians(90)); + public static ConfigurablePose BETWEEN_START_E_JUNCTION = new ConfigurablePose(-36, -12, 5.9); + public static ConfigurablePose BETWEEN_E_JUNCTION_STACK = new ConfigurablePose(-36, -14, 3.14159); + public static ConfigurablePose BETWEEN_STACK_E_JUNCTION = new ConfigurablePose(-38, -13, 1.3089969389957472); + public static ConfigurablePose BETWEEN_START_LEFT = new ConfigurablePose(-58, -60, toRadians(90)); + public static ConfigurablePose BETWEEN_START_RIGHT = new ConfigurablePose(-14, -60, toRadians(90)); + public static ConfigurablePose TERMINAL = new ConfigurablePose(-61, -64, toRadians(180)); + + public static final TrajectoryPath + START_TO_E_JUNCTION = TrajectoryPath.linesToLinearHeadings(START, BETWEEN_START_E_JUNCTION, E_JUNCTION), + E_JUNCTION_TO_STACK = TrajectoryPath.linesToLinearHeadings(E_JUNCTION, BETWEEN_E_JUNCTION_STACK, STACK), + STACK_TO_E_JUNCTION = TrajectoryPath.linesToLinearHeadings(STACK, BETWEEN_STACK_E_JUNCTION, E_JUNCTION), + STACK_TO_E_JUNCTION_TWO = TrajectoryPath.linesToLinearHeadings(STACK, BETWEEN_STACK_E_JUNCTION, E_JUNCTION_2), + E_JUNCTION_TO_STACK_TWO = TrajectoryPath.linesToLinearHeadings(E_JUNCTION_2, BETWEEN_E_JUNCTION_STACK, STACK2), + E_JUNCTION_TO_LEFT_PARK = TrajectoryPath.linesToLinearHeadings(E_JUNCTION, BETWEEN_STACK_E_JUNCTION, LEFT), + E_JUNCTION_TO_MIDDLE_PARK = TrajectoryPath.linesToLinearHeadings(E_JUNCTION, BETWEEN_STACK_E_JUNCTION, MIDDLE), + E_JUNCTION_TO_RIGHT_PARK = TrajectoryPath.linesToLinearHeadings(E_JUNCTION, BETWEEN_STACK_E_JUNCTION, RIGHT), + LEFT_1_2 = TrajectoryPath.linesToLinearHeadings(LEFT, LEFT2, LEFT), + START_TO_LEFT_PARK = TrajectoryPath.linesToLinearHeadings(START, BETWEEN_START_LEFT, LEFT), + START_TO_MIDDLE_PARK = TrajectoryPath.lineToLinearHeading(START, MIDDLE), + START_TO_RIGHT_PARK = TrajectoryPath.linesToLinearHeadings(START, BETWEEN_START_RIGHT, RIGHT); + } +} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/AutoTestDrivebaseCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/AutoTestDrivebaseCommand.java index 25185f53..a02adfb6 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/AutoTestDrivebaseCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/AutoTestDrivebaseCommand.java @@ -15,7 +15,7 @@ public AutoTestDrivebaseCommand(PathingMecanumDrivebaseSubsystem drive) { new TrajectorySequenceCommand(drive, AutoConstants.Right.START_TO_W_JUNCTION), new TrajectorySequenceCommand(drive, AutoConstants.Right.W_JUNCTION_TO_STACK_ONE), new TrajectorySequenceCommand(drive, AutoConstants.Right.STACK_TO_W_JUNCTION_ONE), - CommandScheduler.getInstance()::terminateOpMode + CommandScheduler::terminateOpMode // .alongWith(new ConeReadyToScoreCommand(cone)), // new ClawOpenCommand(claw), // new AutoRightConeStackCommand(drive, lift, claw), diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftConeStackCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftConeStackCommand.java deleted file mode 100644 index 6a2271d3..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftConeStackCommand.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.autonomous.left; - -import com.technototes.library.command.SequentialCommandGroup; -import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; - -public class AutoLeftConeStackCommand extends SequentialCommandGroup { - - public AutoLeftConeStackCommand(Robot r) { - super( - /*new TrajectorySequenceCommand(drive, Robot.Trajectories.LEFT_STACK) - .alongWith(new ConeReadyToIntakeCommand(cone)),*/ - new ClawCloseCommand(r.clawSubsystem), - /*new TrajectorySequenceCommand(drive, Robot.Trajectories.HIGH_JUNCTION_LEFT) - .alongWith(new ConeReadyToScoreCommand(cone)),*/ - new ClawOpenCommand(r.clawSubsystem) - ); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleLeft.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleLeft.java index 2b756f9d..1425a1dd 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleLeft.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleLeft.java @@ -3,11 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftCycleLeft extends SequentialCommandGroup { @@ -17,27 +14,25 @@ public AutoLeftCycleLeft(Robot r) { r.drivebaseSubsystem, AutoConstants.Left.START_TO_E_JUNCTION ) - .alongWith( - new LiftHighJunctionCommand(r.liftSubsystem), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Left.E_JUNCTION_TO_STACK - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)), - new ClawCloseCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Left.STACK_TO_E_JUNCTION - ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Left.E_JUNCTION_TO_LEFT_PARK - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)) - ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Left.E_JUNCTION_TO_STACK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)), + Commands.Claw.close(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Left.STACK_TO_E_JUNCTION + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Left.E_JUNCTION_TO_LEFT_PARK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleMiddle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleMiddle.java index 6844a400..fa328802 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleMiddle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleMiddle.java @@ -3,11 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftCycleMiddle extends SequentialCommandGroup { @@ -17,27 +14,25 @@ public AutoLeftCycleMiddle(Robot r) { r.drivebaseSubsystem, AutoConstants.Left.START_TO_E_JUNCTION ) - .alongWith( - new LiftHighJunctionCommand(r.liftSubsystem), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Left.E_JUNCTION_TO_STACK - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)), - new ClawCloseCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Left.STACK_TO_E_JUNCTION - ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Left.E_JUNCTION_TO_MIDDLE_PARK - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)) - ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Left.E_JUNCTION_TO_STACK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)), + Commands.Claw.close(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Left.STACK_TO_E_JUNCTION + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Left.E_JUNCTION_TO_MIDDLE_PARK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleRight.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleRight.java index 64d3c4f6..10865f39 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleRight.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftCycleRight.java @@ -3,11 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftCycleRight extends SequentialCommandGroup { @@ -17,27 +14,25 @@ public AutoLeftCycleRight(Robot r) { r.drivebaseSubsystem, AutoConstants.Left.START_TO_E_JUNCTION ) - .alongWith( - new LiftHighJunctionCommand(r.liftSubsystem), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Left.E_JUNCTION_TO_STACK - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)), - new ClawCloseCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Left.STACK_TO_E_JUNCTION - ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Left.E_JUNCTION_TO_RIGHT_PARK - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)) - ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Left.E_JUNCTION_TO_STACK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)), + Commands.Claw.close(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Left.STACK_TO_E_JUNCTION + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Left.E_JUNCTION_TO_RIGHT_PARK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftFullCycle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftFullCycle.java index 60e616c8..efd2dc86 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftFullCycle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftFullCycle.java @@ -9,23 +9,21 @@ import java.util.function.DoubleSupplier; import java.util.function.Function; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.autonomous.right.AutoRightSingleCycleOne; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.drive.SlowCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftIntakeCommand; public class AutoLeftFullCycle extends SequentialCommandGroup { public AutoLeftFullCycle( Robot r, - Function, TrajectorySequence> parkingDestination, + Function< + Function, + TrajectorySequence + > parkingDestination, DoubleSupplier currOpModeRunTime ) { super( - new ClawCloseCommand(r.clawSubsystem), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Left.START_TO_E_JUNCTION @@ -33,10 +31,10 @@ public AutoLeftFullCycle( .alongWith( new SequentialCommandGroup( new WaitCommand(0.2), - new LiftHighJunctionCommand(r.liftSubsystem) + Commands.Lift.highJunction(r.liftSubsystem) ) ), - new ClawOpenCommand(r.clawSubsystem), + Commands.Claw.open(r.clawSubsystem), new AutoLeftSingleCycle(r), new AutoLeftSingleCycleTwo(r), new AutoLeftSingleCycleTwo(r), @@ -44,7 +42,7 @@ public AutoLeftFullCycle( //new AutoRightSingleCycleOne(r), //new AutoRightSingleCycleOne(r), new TrajectorySequenceCommand(r.drivebaseSubsystem, parkingDestination) - .alongWith(new LiftIntakeCommand(r.liftSubsystem)) + .alongWith(Commands.Lift.intake(r.liftSubsystem)) //new SlowCommand(r.drivebaseSubsystem) ); // new AutoRightSingleCycle(drivebaseSubsystem, liftSubsystem, clawSubsystem), diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftJunctionStackCycle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftJunctionStackCycle.java index 8e545d57..25e678d2 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftJunctionStackCycle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftJunctionStackCycle.java @@ -3,11 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftJunctionStackCycle extends SequentialCommandGroup { @@ -17,14 +14,14 @@ public AutoLeftJunctionStackCycle(Robot robot) { robot.drivebaseSubsystem, AutoConstants.Left.E_JUNCTION_TO_STACK ) - .alongWith(new LiftCollectCommand(robot.liftSubsystem)), - new ClawCloseCommand(robot.clawSubsystem), + .alongWith(Commands.Lift.collect(robot.liftSubsystem)), + Commands.Claw.close(robot.clawSubsystem), new TrajectorySequenceCommand( robot.drivebaseSubsystem, AutoConstants.Left.STACK_TO_E_JUNCTION ) - .alongWith(new LiftHighJunctionCommand(robot.liftSubsystem)), - new ClawOpenCommand(robot.clawSubsystem) + .alongWith(Commands.Lift.highJunction(robot.liftSubsystem)), + Commands.Claw.open(robot.clawSubsystem) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkLeft.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkLeft.java index 49968db1..401152bd 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkLeft.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkLeft.java @@ -15,7 +15,7 @@ public AutoLeftParkLeft(Robot r) { r.drivebaseSubsystem, AutoConstants.Left.START_TO_LEFT_PARK ), - CommandScheduler.getInstance()::terminateOpMode + CommandScheduler::terminateOpMode ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkMiddle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkMiddle.java index 2301f5a2..e2e665a2 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkMiddle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkMiddle.java @@ -24,7 +24,7 @@ public AutoLeftParkMiddle(Robot r) { r.drivebaseSubsystem, AutoConstants.Left.START_TO_MIDDLE_PARK ), - CommandScheduler.getInstance()::terminateOpMode + CommandScheduler::terminateOpMode ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionCycleCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionCycleCommand.java index ae29ab34..f96866f0 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionCycleCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionCycleCommand.java @@ -8,9 +8,9 @@ public class AutoLeftParkingSelectionCycleCommand extends ChoiceCommand { public AutoLeftParkingSelectionCycleCommand(Robot r) { super( - new Pair<>(r.visionSystem.visionPipeline::left, new AutoLeftCycleLeft(r)), - new Pair<>(r.visionSystem.visionPipeline::middle, new AutoLeftCycleMiddle(r)), - new Pair<>(r.visionSystem.visionPipeline::right, new AutoLeftCycleRight(r)) + new Pair<>(r.visionSystem::left, new AutoLeftCycleLeft(r)), + new Pair<>(r.visionSystem::middle, new AutoLeftCycleMiddle(r)), + new Pair<>(r.visionSystem::right, new AutoLeftCycleRight(r)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionFullCycleCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionFullCycleCommand.java index b4231148..c8b3b9fa 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionFullCycleCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionFullCycleCommand.java @@ -11,7 +11,7 @@ public class AutoLeftParkingSelectionFullCycleCommand extends ChoiceCommand { public AutoLeftParkingSelectionFullCycleCommand(Robot robot, DoubleSupplier currOpModeRunTime) { super( new Pair<>( - robot.visionSystem.visionPipeline::left, + robot.visionSystem::left, new AutoLeftFullCycle( robot, AutoConstants.Left.E_JUNCTION_TO_LEFT_PARK, @@ -19,7 +19,7 @@ public AutoLeftParkingSelectionFullCycleCommand(Robot robot, DoubleSupplier curr ) ), new Pair<>( - robot.visionSystem.visionPipeline::middle, + robot.visionSystem::middle, new AutoLeftFullCycle( robot, AutoConstants.Left.E_JUNCTION_TO_MIDDLE_PARK, @@ -27,7 +27,7 @@ public AutoLeftParkingSelectionFullCycleCommand(Robot robot, DoubleSupplier curr ) ), new Pair<>( - robot.visionSystem.visionPipeline::right, + robot.visionSystem::right, new AutoLeftFullCycle( robot, AutoConstants.Left.E_JUNCTION_TO_RIGHT_PARK, diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionJustParkCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionJustParkCommand.java index c077eacb..cc0826f6 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionJustParkCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionJustParkCommand.java @@ -8,9 +8,9 @@ public class AutoLeftParkingSelectionJustParkCommand extends ChoiceCommand { public AutoLeftParkingSelectionJustParkCommand(Robot r) { super( - new Pair<>(r.visionSystem.visionPipeline::left, new AutoLeftParkLeft(r)), - new Pair<>(r.visionSystem.visionPipeline::middle, new AutoLeftParkMiddle(r)), - new Pair<>(r.visionSystem.visionPipeline::right, new AutoLeftParkRight(r)) + new Pair<>(r.visionSystem::left, new AutoLeftParkLeft(r)), + new Pair<>(r.visionSystem::middle, new AutoLeftParkMiddle(r)), + new Pair<>(r.visionSystem::right, new AutoLeftParkRight(r)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionPreLoadCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionPreLoadCommand.java index 1555d620..01d00598 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionPreLoadCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftParkingSelectionPreLoadCommand.java @@ -8,9 +8,9 @@ public class AutoLeftParkingSelectionPreLoadCommand extends ChoiceCommand { public AutoLeftParkingSelectionPreLoadCommand(Robot r) { super( - new Pair<>(r.visionSystem.visionPipeline::left, new AutoLeftPreLoadLeft(r)), - new Pair<>(r.visionSystem.visionPipeline::middle, new AutoLeftPreLoadMiddle(r)), - new Pair<>(r.visionSystem.visionPipeline::right, new AutoLeftPreLoadRight(r)) + new Pair<>(r.visionSystem::left, new AutoLeftPreLoadLeft(r)), + new Pair<>(r.visionSystem::middle, new AutoLeftPreLoadMiddle(r)), + new Pair<>(r.visionSystem::right, new AutoLeftPreLoadRight(r)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadLeft.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadLeft.java index a4e7e052..2e05d196 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadLeft.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadLeft.java @@ -3,10 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftPreLoadLeft extends SequentialCommandGroup { @@ -16,13 +14,13 @@ public AutoLeftPreLoadLeft(Robot r) { r.drivebaseSubsystem, AutoConstants.Left.START_TO_E_JUNCTION ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Left.E_JUNCTION_TO_LEFT_PARK ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadMiddle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadMiddle.java index 3adabe39..377a322e 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadMiddle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadMiddle.java @@ -3,10 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftPreLoadMiddle extends SequentialCommandGroup { @@ -16,13 +14,13 @@ public AutoLeftPreLoadMiddle(Robot r) { r.drivebaseSubsystem, AutoConstants.Left.START_TO_E_JUNCTION ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Left.E_JUNCTION_TO_MIDDLE_PARK ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadRight.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadRight.java index 6a804844..32d68224 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadRight.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftPreLoadRight.java @@ -3,10 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftPreLoadRight extends SequentialCommandGroup { @@ -16,13 +14,13 @@ public AutoLeftPreLoadRight(Robot r) { r.drivebaseSubsystem, AutoConstants.Left.START_TO_E_JUNCTION ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Left.E_JUNCTION_TO_RIGHT_PARK ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftSingleCycle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftSingleCycle.java index 2c004a55..8c16fbd4 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftSingleCycle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftSingleCycle.java @@ -4,11 +4,8 @@ import com.technototes.library.command.WaitCommand; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftSingleCycle extends SequentialCommandGroup { @@ -21,16 +18,16 @@ public AutoLeftSingleCycle(Robot r) { .alongWith( new SequentialCommandGroup( new WaitCommand(0.4), - new LiftCollectCommand(r.liftSubsystem) + Commands.Lift.collect(r.liftSubsystem) ) ), - new ClawCloseCommand(r.clawSubsystem), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Left.STACK_TO_E_JUNCTION ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftSingleCycleTwo.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftSingleCycleTwo.java index aa6200ac..0c721bb6 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftSingleCycleTwo.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftSingleCycleTwo.java @@ -4,11 +4,8 @@ import com.technototes.library.command.WaitCommand; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftSingleCycleTwo extends SequentialCommandGroup { @@ -21,16 +18,16 @@ public AutoLeftSingleCycleTwo(Robot r) { .alongWith( new SequentialCommandGroup( new WaitCommand(0.4), - new LiftCollectCommand(r.liftSubsystem) + Commands.Lift.collect(r.liftSubsystem) ) ), - new ClawCloseCommand(r.clawSubsystem), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Left.STACK_TO_E_JUNCTION_TWO ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftTest.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftTest.java index 58a38e4e..98650465 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftTest.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/left/AutoLeftTest.java @@ -4,9 +4,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoLeftTest extends SequentialCommandGroup { @@ -16,9 +15,9 @@ public AutoLeftTest(Robot r) { r.drivebaseSubsystem, AutoConstants.Right.START_TO_W_JUNCTION ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), - CommandScheduler.getInstance()::terminateOpMode + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + CommandScheduler::terminateOpMode ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightConeStackCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightConeStackCommand.java index c316e3d2..51fae7ba 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightConeStackCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightConeStackCommand.java @@ -1,20 +1,20 @@ package org.firstinspires.ftc.twenty403.command.autonomous.right; import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.path.trajectorysequence.TrajectoryPath; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; +import org.firstinspires.ftc.twenty403.command.Commands; public class AutoRightConeStackCommand extends SequentialCommandGroup { public AutoRightConeStackCommand(Robot r) { super( - /*new TrajectorySequenceCommand(drive, Robot.Trajectories.RIGHT_STACK) - .alongWith(new ConeReadyToIntakeCommand(cone)),*/ - new ClawCloseCommand(r.clawSubsystem), - /*new TrajectorySequenceCommand(drive, Robot.Trajectories.HIGH_JUNCTION_RIGHT) - .alongWith(new ConeReadyToScoreCommand(cone)),*/ - new ClawOpenCommand(r.clawSubsystem) + /* new TrajectorySequenceCommand(drive, Robot.Trajectories.RIGHT_STACK) + .alongWith(new ConeReadyToIntakeCommand(cone)), */ + Commands.Claw.close(r.clawSubsystem), + /* new TrajectorySequenceCommand(drive, Robot.Trajectories.HIGH_JUNCTION_RIGHT) + .alongWith(new ConeReadyToScoreCommand(cone)), */ + Commands.Claw.open(r.clawSubsystem) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleLeft.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleLeft.java index 7b26859c..7c09bfda 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleLeft.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleLeft.java @@ -3,11 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoRightCycleLeft extends SequentialCommandGroup { @@ -17,27 +14,25 @@ public AutoRightCycleLeft(Robot r) { r.drivebaseSubsystem, AutoConstants.Right.START_TO_W_JUNCTION ) - .alongWith( - new LiftHighJunctionCommand(r.liftSubsystem), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Right.W_JUNCTION_TO_STACK_ONE - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)), - new ClawCloseCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Right.STACK_TO_W_JUNCTION_ONE - ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Right.W_JUNCTION_TO_LEFT_PARK - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)) - ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_STACK_ONE + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)), + Commands.Claw.close(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.STACK_TO_W_JUNCTION_ONE + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_LEFT_PARK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleMiddle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleMiddle.java index 91722795..b7c6ec0a 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleMiddle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleMiddle.java @@ -3,11 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoRightCycleMiddle extends SequentialCommandGroup { @@ -17,27 +14,25 @@ public AutoRightCycleMiddle(Robot r) { r.drivebaseSubsystem, AutoConstants.Right.START_TO_W_JUNCTION ) - .alongWith( - new LiftHighJunctionCommand(r.liftSubsystem), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Right.W_JUNCTION_TO_STACK_ONE - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)), - new ClawCloseCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Right.STACK_TO_W_JUNCTION_ONE - ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Right.W_JUNCTION_TO_MIDDLE_PARK - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)) - ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_STACK_ONE + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)), + Commands.Claw.close(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.STACK_TO_W_JUNCTION_ONE + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_MIDDLE_PARK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleRight.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleRight.java index 9f5dda6f..d87682a1 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleRight.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightCycleRight.java @@ -3,11 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoRightCycleRight extends SequentialCommandGroup { @@ -17,27 +14,25 @@ public AutoRightCycleRight(Robot r) { r.drivebaseSubsystem, AutoConstants.Right.START_TO_W_JUNCTION ) - .alongWith( - new LiftHighJunctionCommand(r.liftSubsystem), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Right.W_JUNCTION_TO_STACK_ONE - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)), - new ClawCloseCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Right.STACK_TO_W_JUNCTION_ONE - ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), - new TrajectorySequenceCommand( - r.drivebaseSubsystem, - AutoConstants.Right.W_JUNCTION_TO_RIGHT_PARK - ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)) - ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_STACK_ONE + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)), + Commands.Claw.close(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.STACK_TO_W_JUNCTION_ONE + ) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.Right.W_JUNCTION_TO_RIGHT_PARK + ) + .alongWith(Commands.Lift.collect(r.liftSubsystem)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightFullCycle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightFullCycle.java index 950b35ee..63904d09 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightFullCycle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightFullCycle.java @@ -9,13 +9,8 @@ import java.util.function.DoubleSupplier; import java.util.function.Function; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.drive.SlowCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftIntakeCommand; -import org.firstinspires.ftc.twenty403.subsystem.DrivebaseSubsystem; // autonomous for right // parks in left position @@ -24,22 +19,22 @@ public class AutoRightFullCycle extends SequentialCommandGroup { public AutoRightFullCycle( Robot r, - Function, TrajectorySequence> parkingDestination, + Function< + Function, + TrajectorySequence + > parkingDestination, DoubleSupplier currOpModeRunTime ) { super( - new ClawCloseCommand(r.clawSubsystem), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.START_TO_W_JUNCTION ) .alongWith( - new SequentialCommandGroup( - new WaitCommand(0.2), - new LiftHighJunctionCommand(r.liftSubsystem) - ) + new WaitCommand(0.2).andThen(Commands.Lift.highJunction(r.liftSubsystem)) ), - new ClawOpenCommand(r.clawSubsystem), + Commands.Claw.open(r.clawSubsystem), new AutoRightSingleCycleOne(r), new AutoRightSingleCycleTwo(r), new AutoRightSingleCycleTwo(r), @@ -47,7 +42,7 @@ public AutoRightFullCycle( //new AutoRightSingleCycleOne(r), //new AutoRightSingleCycleOne(r), new TrajectorySequenceCommand(r.drivebaseSubsystem, parkingDestination) - .alongWith(new LiftIntakeCommand(r.liftSubsystem)) + .alongWith(Commands.Lift.intake(r.liftSubsystem)) //new SlowCommand(r.drivebaseSubsystem) ); // new AutoRightSingleCycle(drivebaseSubsystem, liftSubsystem, clawSubsystem), diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingLeft.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingLeft.java index e39d91bf..e516c21a 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingLeft.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingLeft.java @@ -3,14 +3,14 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; public class AutoRightParkingLeft extends SequentialCommandGroup { public AutoRightParkingLeft(Robot r) { super( - new ClawCloseCommand(r.clawSubsystem), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.START_TO_LEFT_PARK diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingMiddle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingMiddle.java index 1f31bdaf..adb231ea 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingMiddle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingMiddle.java @@ -3,14 +3,14 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; public class AutoRightParkingMiddle extends SequentialCommandGroup { public AutoRightParkingMiddle(Robot r) { super( - new ClawCloseCommand(r.clawSubsystem), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.START_TO_MIDDLE_PARK diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingRight.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingRight.java index 62c20671..b1dcd67b 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingRight.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingRight.java @@ -3,14 +3,14 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; public class AutoRightParkingRight extends SequentialCommandGroup { public AutoRightParkingRight(Robot r) { super( - new ClawCloseCommand(r.clawSubsystem), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.START_TO_RIGHT_PARK diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionCycleCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionCycleCommand.java index 5ead792e..ed024326 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionCycleCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionCycleCommand.java @@ -8,9 +8,9 @@ public class AutoRightParkingSelectionCycleCommand extends ChoiceCommand { public AutoRightParkingSelectionCycleCommand(Robot r) { super( - new Pair<>(r.visionSystem.visionPipeline::left, new AutoRightCycleLeft(r)), - new Pair<>(r.visionSystem.visionPipeline::middle, new AutoRightCycleMiddle(r)), - new Pair<>(r.visionSystem.visionPipeline::right, new AutoRightCycleRight(r)) + new Pair<>(r.visionSystem::left, new AutoRightCycleLeft(r)), + new Pair<>(r.visionSystem::middle, new AutoRightCycleMiddle(r)), + new Pair<>(r.visionSystem::right, new AutoRightCycleRight(r)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionFullCycleCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionFullCycleCommand.java index dfb78ed4..7172872b 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionFullCycleCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionFullCycleCommand.java @@ -14,7 +14,7 @@ public AutoRightParkingSelectionFullCycleCommand( ) { super( new Pair<>( - robot.visionSystem.visionPipeline::left, + robot.visionSystem::left, new AutoRightFullCycle( robot, AutoConstants.Right.W_JUNCTION_TO_LEFT_PARK, @@ -22,7 +22,7 @@ public AutoRightParkingSelectionFullCycleCommand( ) ), new Pair<>( - robot.visionSystem.visionPipeline::middle, + robot.visionSystem::middle, new AutoRightFullCycle( robot, AutoConstants.Right.W_JUNCTION_TO_MIDDLE_PARK, @@ -30,7 +30,7 @@ public AutoRightParkingSelectionFullCycleCommand( ) ), new Pair<>( - robot.visionSystem.visionPipeline::right, + robot.visionSystem::right, new AutoRightFullCycle( robot, AutoConstants.Right.W_JUNCTION_TO_RIGHT_PARK, diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionJustParkCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionJustParkCommand.java index 63857983..f4be8f09 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionJustParkCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionJustParkCommand.java @@ -8,9 +8,9 @@ public class AutoRightParkingSelectionJustParkCommand extends ChoiceCommand { public AutoRightParkingSelectionJustParkCommand(Robot r) { super( - new Pair<>(r.visionSystem.visionPipeline::left, new AutoRightParkingLeft(r)), - new Pair<>(r.visionSystem.visionPipeline::middle, new AutoRightParkingMiddle(r)), - new Pair<>(r.visionSystem.visionPipeline::right, new AutoRightParkingRight(r)) + new Pair<>(r.visionSystem::left, new AutoRightParkingLeft(r)), + new Pair<>(r.visionSystem::middle, new AutoRightParkingMiddle(r)), + new Pair<>(r.visionSystem::right, new AutoRightParkingRight(r)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionPreLoadCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionPreLoadCommand.java index 0df017bc..c1e6d06a 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionPreLoadCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightParkingSelectionPreLoadCommand.java @@ -8,9 +8,9 @@ public class AutoRightParkingSelectionPreLoadCommand extends ChoiceCommand { public AutoRightParkingSelectionPreLoadCommand(Robot r) { super( - new Pair<>(r.visionSystem.visionPipeline::left, new AutoRightPreLoadLeft(r)), - new Pair<>(r.visionSystem.visionPipeline::middle, new AutoRightPreLoadMiddle(r)), - new Pair<>(r.visionSystem.visionPipeline::right, new AutoRightPreLoadRight(r)) + new Pair<>(r.visionSystem::left, new AutoRightPreLoadLeft(r)), + new Pair<>(r.visionSystem::middle, new AutoRightPreLoadMiddle(r)), + new Pair<>(r.visionSystem::right, new AutoRightPreLoadRight(r)) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadLeft.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadLeft.java index 7c53172a..36058e36 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadLeft.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadLeft.java @@ -4,11 +4,8 @@ import com.technototes.library.command.WaitCommand; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftIntakeCommand; public class AutoRightPreLoadLeft extends SequentialCommandGroup { @@ -18,8 +15,8 @@ public AutoRightPreLoadLeft(Robot r) { r.drivebaseSubsystem, AutoConstants.Right.START_TO_W_JUNCTION ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.W_JUNCTION_TO_LEFT_PARK @@ -27,7 +24,7 @@ public AutoRightPreLoadLeft(Robot r) { .alongWith( new SequentialCommandGroup( new WaitCommand(0.6), - new LiftIntakeCommand(r.liftSubsystem) + Commands.Lift.intake(r.liftSubsystem) ) ) ); diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadMiddle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadMiddle.java index 3d2378bb..99540d80 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadMiddle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadMiddle.java @@ -4,11 +4,8 @@ import com.technototes.library.command.WaitCommand; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftIntakeCommand; public class AutoRightPreLoadMiddle extends SequentialCommandGroup { @@ -18,8 +15,8 @@ public AutoRightPreLoadMiddle(Robot r) { r.drivebaseSubsystem, AutoConstants.Right.START_TO_W_JUNCTION ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.W_JUNCTION_TO_MIDDLE_PARK @@ -27,7 +24,7 @@ public AutoRightPreLoadMiddle(Robot r) { .alongWith( new SequentialCommandGroup( new WaitCommand(0.6), - new LiftIntakeCommand(r.liftSubsystem) + Commands.Lift.intake(r.liftSubsystem) ) ) ); diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadRight.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadRight.java index baba4a3b..cc55136f 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadRight.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightPreLoadRight.java @@ -4,11 +4,8 @@ import com.technototes.library.command.WaitCommand; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftIntakeCommand; public class AutoRightPreLoadRight extends SequentialCommandGroup { @@ -18,8 +15,8 @@ public AutoRightPreLoadRight(Robot r) { r.drivebaseSubsystem, AutoConstants.Right.START_TO_W_JUNCTION ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem), + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.W_JUNCTION_TO_RIGHT_PARK @@ -27,7 +24,7 @@ public AutoRightPreLoadRight(Robot r) { .alongWith( new SequentialCommandGroup( new WaitCommand(0.6), - new LiftIntakeCommand(r.liftSubsystem) + Commands.Lift.intake(r.liftSubsystem) ) ) ); diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleOne.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleOne.java index d7bbaae8..24750973 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleOne.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleOne.java @@ -4,11 +4,8 @@ import com.technototes.library.command.WaitCommand; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoRightSingleCycleOne extends SequentialCommandGroup { @@ -21,16 +18,16 @@ public AutoRightSingleCycleOne(Robot r) { .alongWith( new SequentialCommandGroup( new WaitCommand(0.4), - new LiftCollectCommand(r.liftSubsystem) + Commands.Lift.collect(r.liftSubsystem) ) ), - new ClawCloseCommand(r.clawSubsystem), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.STACK_TO_W_JUNCTION_ONE ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleThree.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleThree.java index 2a110791..21617165 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleThree.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleThree.java @@ -3,11 +3,8 @@ import com.technototes.library.command.SequentialCommandGroup; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoRightSingleCycleThree extends SequentialCommandGroup { @@ -17,14 +14,14 @@ public AutoRightSingleCycleThree(Robot r) { r.drivebaseSubsystem, AutoConstants.Right.W_JUNCTION_TO_STACK_THREE ) - .alongWith(new LiftCollectCommand(r.liftSubsystem)), - new ClawCloseCommand(r.clawSubsystem), + .alongWith(Commands.Lift.collect(r.liftSubsystem)), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.STACK_TO_W_JUNCTION_THREE ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleTwo.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleTwo.java index bf69d779..a0ab19ef 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleTwo.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/autonomous/right/AutoRightSingleCycleTwo.java @@ -4,11 +4,8 @@ import com.technototes.library.command.WaitCommand; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftCollectCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; public class AutoRightSingleCycleTwo extends SequentialCommandGroup { @@ -21,16 +18,16 @@ public AutoRightSingleCycleTwo(Robot r) { .alongWith( new SequentialCommandGroup( new WaitCommand(0.4), - new LiftCollectCommand(r.liftSubsystem) + Commands.Lift.collect(r.liftSubsystem) ) ), - new ClawCloseCommand(r.clawSubsystem), + Commands.Claw.close(r.clawSubsystem), new TrajectorySequenceCommand( r.drivebaseSubsystem, AutoConstants.Right.STACK_TO_W_JUNCTION_TWO ) - .alongWith(new LiftHighJunctionCommand(r.liftSubsystem)), - new ClawOpenCommand(r.clawSubsystem) + .alongWith(Commands.Lift.highJunction(r.liftSubsystem)), + Commands.Claw.open(r.clawSubsystem) ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawAutoCloseToggleCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawAutoCloseToggleCommand.java deleted file mode 100644 index f4966f71..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawAutoCloseToggleCommand.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.claw; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.ClawSubsystem; -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class ClawAutoCloseToggleCommand implements Command { - - private ClawSubsystem subsystem; - - public ClawAutoCloseToggleCommand(ClawSubsystem s) { - subsystem = s; - - addRequirements(s); - } - - @Override - public void execute() { - subsystem.toggleAutoClose(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawAutoCloseWithLift.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawAutoCloseWithLift.java deleted file mode 100644 index dc37b3d0..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawAutoCloseWithLift.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.claw; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.lift.LiftUpCommand; -import org.firstinspires.ftc.twenty403.subsystem.ClawSubsystem; -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class ClawAutoCloseWithLift extends SequentialCommandGroup { - - public ClawAutoCloseWithLift(ClawSubsystem c, LiftSubsystem l) { - super(new ClawCloseCommand(c), new WaitCommand(.2), new LiftUpCommand(l)); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawCloseCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawCloseCommand.java deleted file mode 100644 index 95ce5140..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawCloseCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.claw; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.ClawSubsystem; - -public class ClawCloseCommand implements Command { - - private ClawSubsystem subsystem; - - public ClawCloseCommand(ClawSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - subsystem.close(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawOpenCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawOpenCommand.java deleted file mode 100644 index fd93ec1f..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/claw/ClawOpenCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.claw; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.ClawSubsystem; - -public class ClawOpenCommand implements Command { - - private ClawSubsystem subsystem; - - public ClawOpenCommand(ClawSubsystem s) { - this.subsystem = s; - addRequirements(this.subsystem); // Keeps robot from breaking - } - - @Override - public void execute() { - this.subsystem.open(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/AutoSpeedCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/AutoSpeedCommand.java deleted file mode 100644 index 95113f11..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/AutoSpeedCommand.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.drive; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.DrivebaseSubsystem; - -public class AutoSpeedCommand implements Command { - - public DrivebaseSubsystem subsystem; - - public AutoSpeedCommand(DrivebaseSubsystem s) { - subsystem = s; - } - - @Override - public void execute() { - subsystem.auto(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/DriveCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/DriveCommand.java index f61be63e..dee739f5 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/DriveCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/DriveCommand.java @@ -3,19 +3,13 @@ import com.acmerobotics.roadrunner.drive.DriveSignal; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.qualcomm.robotcore.util.Range; import com.technototes.library.command.Command; import com.technototes.library.control.Stick; import com.technototes.library.logger.Loggable; import com.technototes.library.util.MathUtils; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.twenty403.subsystem.DrivebaseSubsystem; -import org.firstinspires.ftc.twenty403.subsystem.VisionPipeline; -import org.firstinspires.ftc.twenty403.subsystem.VisionPipeline.VisionConstants.JunctionDetection; -import org.firstinspires.ftc.twenty403.subsystem.VisionSubsystem; public class DriveCommand implements Command, Loggable { @@ -24,15 +18,13 @@ public class DriveCommand implements Command, Loggable { public DoubleSupplier x, y, r; public BooleanSupplier straight; public BooleanSupplier watchTrigger; - public VisionPipeline visionPipeline; public DriveCommand( DrivebaseSubsystem sub, Stick stick1, Stick stick2, BooleanSupplier straighten, - BooleanSupplier watchAndAlign, - VisionPipeline vp + BooleanSupplier watchAndAlign ) { addRequirements(sub, sub.odometry); subsystem = sub; @@ -41,7 +33,6 @@ public DriveCommand( r = stick2.getXSupplier(); straight = straighten; watchTrigger = watchAndAlign; - visionPipeline = vp; } public DriveCommand( @@ -50,11 +41,11 @@ public DriveCommand( Stick stick2, BooleanSupplier straighten ) { - this(sub, stick1, stick2, straighten, null, null); + this(sub, stick1, stick2, straighten, null); } public DriveCommand(DrivebaseSubsystem sub, Stick stick1, Stick stick2) { - this(sub, stick1, stick2, null, null, null); + this(sub, stick1, stick2, null, null); } private double getRotation(double headingInRads) { @@ -111,56 +102,6 @@ private void autoAlign45() { double rotPower = getRotationClosest45(curHeading); if (Math.abs(rotPower) > .000001) { subsystem.setWeightedDrivePower(new Pose2d(0, 0, rotPower)); - } else { - // Look for the current junctionX/junctionY - double jx = visionPipeline.getJunctionX(); - double jy = visionPipeline.getJunctionY(); - if (jx == 0.0 && jy == 0.0) { - return; - } - if (jx > JunctionDetection.OnlyXRight.RIGHT_CENTER) { - subsystem.setWeightedDrivePower( - new Pose2d( - // moving only x, to the right - Range.clip( - (jx - JunctionDetection.OnlyXRight.RIGHT_CENTER) / - JunctionDetection.OnlyXRight.RIGHT_RANGE, - 0, - 0.3 - ), - 0, - 0 - ) - ); - } else if (jx < JunctionDetection.OnlyXLeft.LEFT_CENTER) { - subsystem.setWeightedDrivePower( - new Pose2d( - // moving only x, to the left - Range.clip( - (jx - JunctionDetection.OnlyXLeft.LEFT_CENTER) / - JunctionDetection.OnlyXLeft.LEFT_RANGE, - -0.3, - 0 - ), - 0, - 0 - ) - ); - } else if (jy < JunctionDetection.OnlyYForward.FORWARD_CENTER && jy > 10) { - subsystem.setWeightedDrivePower( - new Pose2d( - 0, - // Moving y forward - Range.clip( - (jy - JunctionDetection.OnlyYForward.FORWARD_CENTER) / - JunctionDetection.OnlyYForward.FORWARD_RANGE, - -0.3, - 0 - ), - 0 - ) - ); - } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/ResetGyroCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/ResetGyroCommand.java deleted file mode 100644 index 727a35a9..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/ResetGyroCommand.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.drive; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.DrivebaseSubsystem; - -public class ResetGyroCommand implements Command { - - public DrivebaseSubsystem subsystem; - - public ResetGyroCommand(DrivebaseSubsystem s) { - subsystem = s; - } - - @Override - public void execute() { - subsystem.setExternalHeading(0); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/SetSpeedCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/SetSpeedCommand.java deleted file mode 100644 index 7cd73439..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/SetSpeedCommand.java +++ /dev/null @@ -1,28 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.drive; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.DrivebaseSubsystem; - -public class SetSpeedCommand implements Command { - - public DrivebaseSubsystem subsystem; - - public SetSpeedCommand(DrivebaseSubsystem s) { - subsystem = s; - } - - @Override - public void execute() { - subsystem.speed = 0.5; - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean cancel) { - subsystem.speed = 1; - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/SlowCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/SlowCommand.java deleted file mode 100644 index 36899f2f..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/SlowCommand.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.drive; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.DrivebaseSubsystem; - -public class SlowCommand implements Command { - - public DrivebaseSubsystem subsystem; - - public SlowCommand(DrivebaseSubsystem s) { - subsystem = s; - } - - @Override - public void execute() { - subsystem.slow(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileAbortCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileAbortCommand.java deleted file mode 100644 index 450f5061..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileAbortCommand.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.drive; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.Robot; - -public class TileAbortCommand implements Command { - - Robot robot; - - public TileAbortCommand(Robot r) { - robot = r; - } - - @Override - public void execute() { - robot.drivebaseSubsystem.requestCancelled(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileMoveCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileMoveCommand.java index bb600908..2a1fd5fc 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileMoveCommand.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileMoveCommand.java @@ -1,10 +1,7 @@ package org.firstinspires.ftc.twenty403.command.drive; -import com.acmerobotics.roadrunner.geometry.Pose2d; import com.technototes.library.command.Command; -import com.technototes.path.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; public class TileMoveCommand implements Command { @@ -36,4 +33,11 @@ public void execute() { break; } } + + public enum TileMoving { + Up, + Down, + Left, + Right, + } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileMoving.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileMoving.java deleted file mode 100644 index 58e2d405..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TileMoving.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.drive; - -public enum TileMoving { - Up, - Down, - Left, - Right, -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TurboCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TurboCommand.java deleted file mode 100644 index 2a6c30f9..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/drive/TurboCommand.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.drive; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.DrivebaseSubsystem; - -public class TurboCommand implements Command { - - public DrivebaseSubsystem subsystem; - - public TurboCommand(DrivebaseSubsystem s) { - subsystem = s; - } - - @Override - public void execute() { - subsystem.fast(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/JunctionHeight.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/JunctionHeight.java deleted file mode 100644 index 6bb8d9ec..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/JunctionHeight.java +++ /dev/null @@ -1,9 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -public enum JunctionHeight { - Intake, - Ground, - Low, - Medium, - High, -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftCollectCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftCollectCommand.java deleted file mode 100644 index bd8cb2b7..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftCollectCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftCollectCommand implements Command { - - private LiftSubsystem liftSubsystem; - - public LiftCollectCommand(LiftSubsystem ls) { - liftSubsystem = ls; - addRequirements(ls); - } - - @Override - public void execute() { - liftSubsystem.collect(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftDownCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftDownCommand.java deleted file mode 100644 index 84aca776..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftDownCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftDownCommand implements Command { - - private LiftSubsystem liftSubsystem; - - public LiftDownCommand(LiftSubsystem ls) { - liftSubsystem = ls; - addRequirements(ls); - } - - @Override - public void execute() { - liftSubsystem.moveDown(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftGroundJunctionCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftGroundJunctionCommand.java deleted file mode 100644 index 8eee4da5..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftGroundJunctionCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftGroundJunctionCommand extends LiftHeightCommand { - - public LiftGroundJunctionCommand(LiftSubsystem ls) { - super(ls, JunctionHeight.Ground); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftHeightCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftHeightCommand.java deleted file mode 100644 index 4251c44b..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftHeightCommand.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftHeightCommand implements Command { - - LiftSubsystem lift; - JunctionHeight height; - - public LiftHeightCommand(LiftSubsystem ls, JunctionHeight ht) { - lift = ls; - height = ht; - addRequirements(ls); - } - - @Override - public void execute() { - switch (height) { - case Intake: - lift.intakePos(); - break; - case Ground: - lift.groundJunction(); - break; - case Low: - lift.lowPole(); - break; - case Medium: - lift.midPole(); - break; - case High: - lift.highPole(); - break; - } - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftHighJunctionCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftHighJunctionCommand.java deleted file mode 100644 index 287db63b..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftHighJunctionCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftHighJunctionCommand extends LiftHeightCommand { - - public LiftHighJunctionCommand(LiftSubsystem ls) { - super(ls, JunctionHeight.High); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftIntakeCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftIntakeCommand.java deleted file mode 100644 index 24696dc6..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftIntakeCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftIntakeCommand extends LiftHeightCommand { - - public LiftIntakeCommand(LiftSubsystem ls) { - super(ls, JunctionHeight.Intake); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftLowJunctionCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftLowJunctionCommand.java deleted file mode 100644 index 0cf1997b..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftLowJunctionCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftLowJunctionCommand extends LiftHeightCommand { - - public LiftLowJunctionCommand(LiftSubsystem ls) { - super(ls, JunctionHeight.Low); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftMidJunctionCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftMidJunctionCommand.java deleted file mode 100644 index 32b72698..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftMidJunctionCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftMidJunctionCommand extends LiftHeightCommand { - - public LiftMidJunctionCommand(LiftSubsystem ls) { - super(ls, JunctionHeight.Medium); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftMoveDownOverrideCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftMoveDownOverrideCommand.java deleted file mode 100644 index 91778727..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftMoveDownOverrideCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftMoveDownOverrideCommand implements Command { - - private LiftSubsystem subsystem; - - public LiftMoveDownOverrideCommand(LiftSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - subsystem.moveDown_OVERRIDE(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftMoveUpOverrideCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftMoveUpOverrideCommand.java deleted file mode 100644 index e9a9a476..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftMoveUpOverrideCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftMoveUpOverrideCommand implements Command { - - private LiftSubsystem subsystem; - - public LiftMoveUpOverrideCommand(LiftSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - subsystem.moveUp_OVERRIDE(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftSetZeroCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftSetZeroCommand.java deleted file mode 100644 index 1c3646b4..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftSetZeroCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftSetZeroCommand implements Command { - - private LiftSubsystem liftSubsystem; - - public LiftSetZeroCommand(LiftSubsystem ls) { - liftSubsystem = ls; - addRequirements(ls); - } - - @Override - public void execute() { - liftSubsystem.setNewZero(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftUpCommand.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftUpCommand.java deleted file mode 100644 index 7a5cd55f..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/command/lift/LiftUpCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.twenty403.command.lift; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.twenty403.subsystem.LiftSubsystem; - -public class LiftUpCommand implements Command { - - private LiftSubsystem liftSubsystem; - - public LiftUpCommand(LiftSubsystem ls) { - liftSubsystem = ls; - addRequirements(ls); - } - - @Override - public void execute() { - liftSubsystem.moveUp(); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlDriver.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlDriver.java index 9afc3a36..51124ed5 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlDriver.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlDriver.java @@ -1,28 +1,15 @@ package org.firstinspires.ftc.twenty403.controls; -import android.service.quicksettings.Tile; +import com.technototes.library.command.Command; import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.control.CommandButton; import com.technototes.library.control.CommandButton; import com.technototes.library.control.CommandGamepad; -import com.technototes.library.control.CommandGamepad; -import com.technototes.library.control.Stick; import com.technototes.library.control.Stick; -import com.technototes.library.structure.CommandOpMode; -import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionDuringTeleCommand; -import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; -import org.firstinspires.ftc.twenty403.command.claw.ClawAutoCloseToggleCommand; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.drive.DriveCommand; -import org.firstinspires.ftc.twenty403.command.drive.ResetGyroCommand; -import org.firstinspires.ftc.twenty403.command.drive.SlowCommand; -import org.firstinspires.ftc.twenty403.command.drive.TileAbortCommand; import org.firstinspires.ftc.twenty403.command.drive.TileMoveCommand; -import org.firstinspires.ftc.twenty403.command.drive.TileMoving; -import org.firstinspires.ftc.twenty403.command.drive.TurboCommand; -import org.firstinspires.ftc.twenty403.subsystem.VisionPipeline; +import org.firstinspires.ftc.twenty403.subsystem.ClawSubsystem; public class ControlDriver { @@ -34,13 +21,11 @@ public class ControlDriver { public CommandButton resetGyroButton, driveStraight, turboButton, autoAlign; public CommandButton clawToggleAutoCloseButton; public CommandButton override; - VisionPipeline visionPipeline; public ControlDriver(CommandGamepad g, Robot r) { this.robot = r; gamepad = g; override = g.leftTrigger.getAsButton(0.5); - visionPipeline = robot.visionSystem.visionPipeline; AssignNamedControllerButton(); @@ -50,9 +35,6 @@ public ControlDriver(CommandGamepad g, Robot r) { if (Robot.RobotConstant.CLAW_CONNECTED) { bindClawControls(); } - if (Robot.RobotConstant.CAMERA_CONNECTED) { - bindVisionCommand(); - } } private void AssignNamedControllerButton() { @@ -73,34 +55,27 @@ private void AssignNamedControllerButton() { } public void bindDriveControls() { - CommandScheduler - .getInstance() - .scheduleJoystick( - new DriveCommand( - robot.drivebaseSubsystem, - driveLeftStick, - driveRightStick, - driveStraight, - autoAlign, - visionPipeline - ) - ); - turboButton.whenPressed(new TurboCommand(robot.drivebaseSubsystem)); - turboButton.whenReleased(new SlowCommand(robot.drivebaseSubsystem)); - resetGyroButton.whenPressed(new ResetGyroCommand(robot.drivebaseSubsystem)); - - tileRight.whenPressed(new TileMoveCommand(robot, TileMoving.Right)); - tileLeft.whenPressed(new TileMoveCommand(robot, TileMoving.Left)); - tileUp.whenPressed(new TileMoveCommand(robot, TileMoving.Up)); - tileDown.whenPressed(new TileMoveCommand(robot, TileMoving.Down)); - tileAbort.whenPressed(new TileAbortCommand(robot)); - } + CommandScheduler.scheduleJoystick( + new DriveCommand( + robot.drivebaseSubsystem, + driveLeftStick, + driveRightStick, + driveStraight, + autoAlign + ) + ); + turboButton.whenPressed(Commands.Drive.fast(robot.drivebaseSubsystem)); + turboButton.whenReleased(Commands.Drive.slow(robot.drivebaseSubsystem)); + resetGyroButton.whenPressed(Commands.Drive.zeroHeading(robot.drivebaseSubsystem)); - public void bindVisionCommand() { - gamepad.ps_share.whenPressed(new VisionDuringTeleCommand(robot.visionSystem)); + tileRight.whenPressed(new TileMoveCommand(robot, TileMoveCommand.TileMoving.Right)); + tileLeft.whenPressed(new TileMoveCommand(robot, TileMoveCommand.TileMoving.Left)); + tileUp.whenPressed(new TileMoveCommand(robot, TileMoveCommand.TileMoving.Up)); + tileDown.whenPressed(new TileMoveCommand(robot, TileMoveCommand.TileMoving.Down)); + tileAbort.whenPressed(Commands.Drive.cancelTileRequest(robot.drivebaseSubsystem)); } public void bindClawControls() { - clawToggleAutoCloseButton.whenPressed(new ClawAutoCloseToggleCommand(robot.clawSubsystem)); + clawToggleAutoCloseButton.whenPressed(Commands.Claw.toggleAutoClose(robot.clawSubsystem)); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlOperator.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlOperator.java index 3d5b9d09..721f3683 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlOperator.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlOperator.java @@ -5,19 +5,7 @@ import com.technototes.library.control.CommandGamepad; import com.technototes.library.control.Stick; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.claw.ClawAutoCloseToggleCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftDownCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftGroundJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftIntakeCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftLowJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftMidJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftMoveDownOverrideCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftMoveUpOverrideCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftSetZeroCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftUpCommand; +import org.firstinspires.ftc.twenty403.command.Commands; public class ControlOperator { @@ -60,40 +48,40 @@ private void AssignNamedControllerButton() { } public void bindClawControls() { - clawOpenButton.whenPressed(new ClawOpenCommand(robot.clawSubsystem)); - clawCloseButton.whenReleased(new ClawCloseCommand(robot.clawSubsystem)); + clawOpenButton.whenPressed(Commands.Claw.open(robot.clawSubsystem)); + clawCloseButton.whenReleased(Commands.Claw.close(robot.clawSubsystem)); } public void bindLiftControls() { - liftUpButton.whenPressed(new LiftUpCommand(robot.liftSubsystem)); - liftDownButton.whenPressed(new LiftDownCommand(robot.liftSubsystem)); - liftIntakePos.whenPressed(new LiftIntakeCommand(robot.liftSubsystem)); + liftUpButton.whenPressed(Commands.Lift.moveUp(robot.liftSubsystem)); + liftDownButton.whenPressed(Commands.Lift.moveDown(robot.liftSubsystem)); + liftIntakePos.whenPressed(Commands.Lift.intake(robot.liftSubsystem)); liftHighOrOverrideZero.whenPressed( new ConditionalCommand( override, - new LiftSetZeroCommand(robot.liftSubsystem), - new LiftHighJunctionCommand(robot.liftSubsystem) + Commands.Lift.setNewZero(robot.liftSubsystem), + Commands.Lift.highJunction(robot.liftSubsystem) ) ); liftMediumOrToggleAutoClose.whenPressed( new ConditionalCommand( override, - new ClawAutoCloseToggleCommand(robot.clawSubsystem), - new LiftMidJunctionCommand(robot.liftSubsystem) + Commands.Claw.toggleAutoClose(robot.clawSubsystem), + Commands.Lift.midJunction(robot.liftSubsystem) ) ); liftGroundOrOverrideDown.whenPressed( new ConditionalCommand( override, - new LiftMoveDownOverrideCommand(robot.liftSubsystem), - new LiftGroundJunctionCommand(robot.liftSubsystem) + Commands.Lift.moveDown_OVERRIDE(robot.liftSubsystem), + Commands.Lift.groundJunction(robot.liftSubsystem) ) ); liftLowOrOverrideUp.whenPressed( new ConditionalCommand( override, - new LiftMoveUpOverrideCommand(robot.liftSubsystem), - new LiftLowJunctionCommand(robot.liftSubsystem) + Commands.Lift.moveUp_OVERRIDE(robot.liftSubsystem), + Commands.Lift.lowJunction(robot.liftSubsystem) ) ); } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlSingle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlSingle.java index 64b82dc8..1b5467b8 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlSingle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/ControlSingle.java @@ -6,22 +6,8 @@ import com.technototes.library.control.CommandGamepad; import com.technototes.library.control.Stick; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawOpenCommand; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.drive.DriveCommand; -import org.firstinspires.ftc.twenty403.command.drive.ResetGyroCommand; -import org.firstinspires.ftc.twenty403.command.drive.SlowCommand; -import org.firstinspires.ftc.twenty403.command.drive.TurboCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftDownCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftGroundJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftHighJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftIntakeCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftLowJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftMidJunctionCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftMoveDownOverrideCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftMoveUpOverrideCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftSetZeroCommand; -import org.firstinspires.ftc.twenty403.command.lift.LiftUpCommand; public class ControlSingle { @@ -82,54 +68,51 @@ private void AssignNamedControllerButton() { } public void bindDriveControls() { - CommandScheduler - .getInstance() - .scheduleJoystick( - new DriveCommand( - robot.drivebaseSubsystem, - driveLeftStick, - driveRightStick, - driveStraight, - watchButton, - robot.visionSystem.visionPipeline - ) - ); - turboButton.whenPressed(new TurboCommand(robot.drivebaseSubsystem)); - turboButton.whenReleased(new SlowCommand(robot.drivebaseSubsystem)); + CommandScheduler.scheduleJoystick( + new DriveCommand( + robot.drivebaseSubsystem, + driveLeftStick, + driveRightStick, + driveStraight, + watchButton + ) + ); + turboButton.whenPressed(robot.drivebaseSubsystem::fast); + turboButton.whenReleased(robot.drivebaseSubsystem::slow); // TODO: We probably want buttons to reset the Gyro... - resetGyroButton.whenPressed(new ResetGyroCommand(robot.drivebaseSubsystem)); + resetGyroButton.whenPressed(Commands.Drive.zeroHeading(robot.drivebaseSubsystem)); } public void bindClawControls() { // TODO: Name & Bind claw controls - clawOpenButton.whenPressed(new ClawOpenCommand(robot.clawSubsystem)); - clawCloseButton.whenReleased(new ClawCloseCommand(robot.clawSubsystem)); + clawOpenButton.whenPressed(Commands.Claw.open(robot.clawSubsystem)); + clawCloseButton.whenReleased(Commands.Claw.close(robot.clawSubsystem)); } public void bindLiftControls() { // TODO: Name & Bind lift controls - liftUpButton.whenPressed(new LiftUpCommand(robot.liftSubsystem)); - liftDownButton.whenPressed(new LiftDownCommand(robot.liftSubsystem)); - liftIntakePos.whenPressed(new LiftIntakeCommand(robot.liftSubsystem)); + liftUpButton.whenPressed(Commands.Lift.moveUp(robot.liftSubsystem)); + liftDownButton.whenPressed(Commands.Lift.moveDown(robot.liftSubsystem)); + liftIntakePos.whenPressed(Commands.Lift.intake(robot.liftSubsystem)); liftOverrideZeroButton.whenPressed( - new ConditionalCommand(override, new LiftSetZeroCommand(robot.liftSubsystem)) + new ConditionalCommand(override, Commands.Lift.setNewZero(robot.liftSubsystem)) ); liftGroundOrOverrideDown.whenPressed( new ConditionalCommand( override, - new LiftMoveDownOverrideCommand(robot.liftSubsystem), - new LiftGroundJunctionCommand(robot.liftSubsystem) + Commands.Lift.moveDown_OVERRIDE(robot.liftSubsystem), + Commands.Lift.groundJunction(robot.liftSubsystem) ) ); liftLowOrOverrideUp.whenPressed( new ConditionalCommand( override, - new LiftMoveUpOverrideCommand(robot.liftSubsystem), - new LiftLowJunctionCommand(robot.liftSubsystem) + Commands.Lift.moveUp_OVERRIDE(robot.liftSubsystem), + Commands.Lift.lowJunction(robot.liftSubsystem) ) ); - liftMedium.whenPressed(new LiftMidJunctionCommand(robot.liftSubsystem)); - liftHigh.whenPressed(new LiftHighJunctionCommand(robot.liftSubsystem)); + liftMedium.whenPressed(Commands.Lift.midJunction(robot.liftSubsystem)); + liftHigh.whenPressed(Commands.Lift.highJunction(robot.liftSubsystem)); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/DualBlue.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/DualBlue.java index fe26d677..34a6f817 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/DualBlue.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/DualBlue.java @@ -8,9 +8,9 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; -import org.firstinspires.ftc.twenty403.command.drive.ResetGyroCommand; import org.firstinspires.ftc.twenty403.controls.ControlDriver; import org.firstinspires.ftc.twenty403.controls.ControlOperator; @@ -31,8 +31,9 @@ public void uponInit() { controlsDriver = new ControlDriver(driverGamepad, robot); controlsOperator = new ControlOperator(codriverGamepad, robot); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Right.TELESTART.toPose()); - CommandScheduler - .getInstance() - .scheduleForState(new ResetGyroCommand(robot.drivebaseSubsystem), OpModeState.INIT); + CommandScheduler.scheduleForState( + Commands.Drive.zeroHeading(robot.drivebaseSubsystem), + OpModeState.INIT + ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/DualRed.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/DualRed.java index 58af72e7..f6cc4572 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/DualRed.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/DualRed.java @@ -8,9 +8,9 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; -import org.firstinspires.ftc.twenty403.command.drive.ResetGyroCommand; import org.firstinspires.ftc.twenty403.controls.ControlDriver; import org.firstinspires.ftc.twenty403.controls.ControlOperator; @@ -31,8 +31,9 @@ public void uponInit() { controlsDriver = new ControlDriver(driverGamepad, robot); controlsOperator = new ControlOperator(codriverGamepad, robot); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Right.TELESTART.toPose()); - CommandScheduler - .getInstance() - .scheduleForState(new ResetGyroCommand(robot.drivebaseSubsystem), OpModeState.INIT); + CommandScheduler.scheduleForState( + Commands.Drive.zeroHeading(robot.drivebaseSubsystem), + OpModeState.INIT + ); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftFullCycle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftFullCycle.java index 6bfac95e..4538b478 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftFullCycle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftFullCycle.java @@ -9,14 +9,10 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionCommand; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; import org.firstinspires.ftc.twenty403.command.autonomous.left.AutoLeftParkingSelectionFullCycleCommand; -import org.firstinspires.ftc.twenty403.command.autonomous.right.AutoRightParkingSelectionFullCycleCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.drive.AutoSpeedCommand; -import org.firstinspires.ftc.twenty403.command.drive.TurboCommand; import org.firstinspires.ftc.twenty403.controls.ControlSingle; @Autonomous(name = "LeftFullCycle") @@ -34,27 +30,19 @@ public void uponInit() { robot = new Robot(hardware, Alliance.NONE, StartingPosition.LEFT); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Left.START.toPose()); // ElapsedTimeHelper timeout = new ElapsedTimeHelper(() -> this.getOpModeRuntime(), 25); - CommandScheduler - .getInstance() - .scheduleForState( - new SequentialCommandGroup( - //new TurboCommand(robot.drivebaseSubsystem), - new ClawCloseCommand(robot.clawSubsystem), - new AutoLeftParkingSelectionFullCycleCommand( - robot, - () -> this.getOpModeRuntime() - ), - CommandScheduler.getInstance()::terminateOpMode - ), - CommandOpMode.OpModeState.RUN - ); + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + //new TurboCommand(robot.drivebaseSubsystem), + Commands.Claw.close(robot.clawSubsystem), + new AutoLeftParkingSelectionFullCycleCommand(robot, () -> this.getOpModeRuntime()), + CommandScheduler::terminateOpMode + ), + CommandOpMode.OpModeState.RUN + ); if (Robot.RobotConstant.CAMERA_CONNECTED) { - CommandScheduler - .getInstance() - .scheduleInit( - new ClawCloseCommand(robot.clawSubsystem) - .andThen(new VisionCommand(robot.visionSystem)) - ); + CommandScheduler.scheduleInit( + Commands.Claw.close(robot.clawSubsystem).andThen(robot.visionSystem.runVision) + ); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftJustPark.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftJustPark.java index 959bc6ac..d38384c4 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftJustPark.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftJustPark.java @@ -9,7 +9,6 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionCommand; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; import org.firstinspires.ftc.twenty403.command.autonomous.left.AutoLeftParkingSelectionJustParkCommand; @@ -29,17 +28,15 @@ public void uponInit() { hardware = new Hardware(hardwareMap); robot = new Robot(hardware, Alliance.NONE, StartingPosition.LEFT); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Left.START.toPose()); - CommandScheduler - .getInstance() - .scheduleForState( - new SequentialCommandGroup( - new AutoLeftParkingSelectionJustParkCommand(robot), - CommandScheduler.getInstance()::terminateOpMode - ), - CommandOpMode.OpModeState.RUN - ); + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + new AutoLeftParkingSelectionJustParkCommand(robot), + CommandScheduler::terminateOpMode + ), + CommandOpMode.OpModeState.RUN + ); if (Robot.RobotConstant.CAMERA_CONNECTED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSystem)); + CommandScheduler.scheduleInit(robot.visionSystem.runVision); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftPreLoad.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftPreLoad.java index c0399eef..763605da 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftPreLoad.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/LeftPreLoad.java @@ -9,7 +9,6 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionCommand; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; import org.firstinspires.ftc.twenty403.command.autonomous.left.AutoLeftParkingSelectionPreLoadCommand; @@ -29,17 +28,15 @@ public void uponInit() { hardware = new Hardware(hardwareMap); robot = new Robot(hardware, Alliance.NONE, StartingPosition.LEFT); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Left.START.toPose()); - CommandScheduler - .getInstance() - .scheduleForState( - new SequentialCommandGroup( - new AutoLeftParkingSelectionPreLoadCommand(robot), - CommandScheduler.getInstance()::terminateOpMode - ), - CommandOpMode.OpModeState.RUN - ); + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + new AutoLeftParkingSelectionPreLoadCommand(robot), + CommandScheduler::terminateOpMode + ), + CommandOpMode.OpModeState.RUN + ); if (Robot.RobotConstant.CAMERA_CONNECTED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSystem)); + CommandScheduler.scheduleInit(robot.visionSystem.runVision); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightFullCycle.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightFullCycle.java index 41b1ee76..af854319 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightFullCycle.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightFullCycle.java @@ -9,13 +9,10 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionCommand; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; import org.firstinspires.ftc.twenty403.command.autonomous.right.AutoRightParkingSelectionFullCycleCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; -import org.firstinspires.ftc.twenty403.command.drive.AutoSpeedCommand; -import org.firstinspires.ftc.twenty403.command.drive.TurboCommand; import org.firstinspires.ftc.twenty403.controls.ControlSingle; @Autonomous(name = "Right Full Cycle") @@ -33,27 +30,19 @@ public void uponInit() { robot = new Robot(hardware, Alliance.NONE, StartingPosition.RIGHT); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Right.START.toPose()); // ElapsedTimeHelper timeout = new ElapsedTimeHelper(() -> this.getOpModeRuntime(), 25); - CommandScheduler - .getInstance() - .scheduleForState( - new SequentialCommandGroup( - //new TurboCommand(robot.drivebaseSubsystem), - new ClawCloseCommand(robot.clawSubsystem), - new AutoRightParkingSelectionFullCycleCommand( - robot, - () -> this.getOpModeRuntime() - ), - CommandScheduler.getInstance()::terminateOpMode - ), - CommandOpMode.OpModeState.RUN - ); + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + //new TurboCommand(robot.drivebaseSubsystem), + Commands.Claw.close(robot.clawSubsystem), + new AutoRightParkingSelectionFullCycleCommand(robot, () -> this.getOpModeRuntime()), + CommandScheduler::terminateOpMode + ), + CommandOpMode.OpModeState.RUN + ); if (Robot.RobotConstant.CAMERA_CONNECTED) { - CommandScheduler - .getInstance() - .scheduleInit( - new ClawCloseCommand(robot.clawSubsystem) - .andThen(new VisionCommand(robot.visionSystem)) - ); + CommandScheduler.scheduleInit( + Commands.Claw.close(robot.clawSubsystem).andThen(robot.visionSystem.runVision) + ); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightJustPark.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightJustPark.java index e70c1610..7dd5f280 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightJustPark.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightJustPark.java @@ -9,7 +9,6 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionCommand; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; import org.firstinspires.ftc.twenty403.command.autonomous.right.AutoRightParkingSelectionJustParkCommand; @@ -29,17 +28,15 @@ public void uponInit() { hardware = new Hardware(hardwareMap); robot = new Robot(hardware, Alliance.NONE, StartingPosition.RIGHT); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Right.START.toPose()); - CommandScheduler - .getInstance() - .scheduleForState( - new SequentialCommandGroup( - new AutoRightParkingSelectionJustParkCommand(robot), - CommandScheduler.getInstance()::terminateOpMode - ), - CommandOpMode.OpModeState.RUN - ); + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + new AutoRightParkingSelectionJustParkCommand(robot), + CommandScheduler::terminateOpMode + ), + CommandOpMode.OpModeState.RUN + ); if (Robot.RobotConstant.CAMERA_CONNECTED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSystem)); + CommandScheduler.scheduleInit(robot.visionSystem.runVision); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightPreLoad.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightPreLoad.java index 3540f5d7..8b8c77df 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightPreLoad.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/RightPreLoad.java @@ -9,11 +9,10 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionCommand; +import org.firstinspires.ftc.twenty403.command.Commands; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; import org.firstinspires.ftc.twenty403.command.autonomous.right.AutoRightParkingSelectionPreLoadCommand; -import org.firstinspires.ftc.twenty403.command.claw.ClawCloseCommand; import org.firstinspires.ftc.twenty403.controls.ControlSingle; @Autonomous(name = "Right PreLoad") @@ -30,18 +29,16 @@ public void uponInit() { hardware = new Hardware(hardwareMap); robot = new Robot(hardware, Alliance.NONE, StartingPosition.RIGHT); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Right.START.toPose()); - CommandScheduler - .getInstance() - .scheduleForState( - new SequentialCommandGroup( - new ClawCloseCommand(robot.clawSubsystem), - new AutoRightParkingSelectionPreLoadCommand(robot), - CommandScheduler.getInstance()::terminateOpMode - ), - CommandOpMode.OpModeState.RUN - ); + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + Commands.Claw.close(robot.clawSubsystem), + new AutoRightParkingSelectionPreLoadCommand(robot), + CommandScheduler::terminateOpMode + ), + CommandOpMode.OpModeState.RUN + ); if (Robot.RobotConstant.CAMERA_CONNECTED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSystem)); + CommandScheduler.scheduleInit(robot.visionSystem.runVision); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/AutoTester.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/AutoTester.java index 4ce1023f..f1262b52 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/AutoTester.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/AutoTester.java @@ -7,7 +7,6 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionCommand; import org.firstinspires.ftc.twenty403.command.autonomous.AutoConstants; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; import org.firstinspires.ftc.twenty403.command.autonomous.left.AutoLeftTest; @@ -27,11 +26,9 @@ public void uponInit() { hardware = new Hardware(hardwareMap); robot = new Robot(hardware, Alliance.BLUE, StartingPosition.RIGHT); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Right.START.toPose()); - CommandScheduler - .getInstance() - .scheduleForState(new AutoLeftTest(robot), CommandOpMode.OpModeState.RUN); + CommandScheduler.scheduleForState(new AutoLeftTest(robot), CommandOpMode.OpModeState.RUN); if (Robot.RobotConstant.CAMERA_CONNECTED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSystem)); + CommandScheduler.scheduleInit(robot.visionSystem.runVision); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/DriveMotorTest.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/DriveMotorTest.java index d4bebb6b..c71526bd 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/DriveMotorTest.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/DriveMotorTest.java @@ -50,7 +50,7 @@ public void runOpMode() throws InterruptedException { ElapsedTime time = new ElapsedTime(); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { - double pos = time.seconds() * MotorScales.SPEED % 16.0; + double pos = (time.seconds() * MotorScales.SPEED) % 16.0; double power = 0; if (pos < 4.0) { power = pos / 4.0; diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/RoadRunnerTest.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/RoadRunnerTest.java index d196a41d..05b3b3bb 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/RoadRunnerTest.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/RoadRunnerTest.java @@ -30,21 +30,19 @@ public void uponInit() { hardware = new Hardware(hardwareMap); robot = new Robot(hardware, Alliance.NONE, StartingPosition.RIGHT); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.Right.START.toPose()); - CommandScheduler - .getInstance() - .scheduleForState( - new SequentialCommandGroup( - new TrajectorySequenceCommand( - robot.drivebaseSubsystem, - AutoConstants.Right.START_TO_LEFTSIDE - ), - new TrajectorySequenceCommand( - robot.drivebaseSubsystem, - AutoConstants.Right.LEFTSIDE_TO_START - ) + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + new TrajectorySequenceCommand( + robot.drivebaseSubsystem, + AutoConstants.Right.START_TO_LEFTSIDE ), - CommandOpMode.OpModeState.RUN - ); + new TrajectorySequenceCommand( + robot.drivebaseSubsystem, + AutoConstants.Right.LEFTSIDE_TO_START + ) + ), + CommandOpMode.OpModeState.RUN + ); } @Override diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/TeleVisionTest.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/TeleVisionTest.java index c8324ce4..efe9322a 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/TeleVisionTest.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/TeleVisionTest.java @@ -5,12 +5,10 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.technototes.library.command.CommandScheduler; -import com.technototes.library.logger.Loggable; import com.technototes.library.structure.CommandOpMode; import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionCommand; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; import org.firstinspires.ftc.twenty403.controls.ControlDriver; import org.firstinspires.ftc.twenty403.controls.ControlOperator; @@ -38,7 +36,7 @@ public void uponInit() { // I don't think we want to do this, as it stops the pipeline 'after init' and that // may be triggered *after* we start the pipeline when we 'upon start'. if (false && Robot.RobotConstant.CAMERA_CONNECTED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSystem)); + CommandScheduler.scheduleInit(robot.visionSystem.runVision); } } @@ -57,8 +55,5 @@ public void runLoop() { String.format("%f, %f", gamepad1.right_stick_x, gamepad1.right_stick_y) ); telemetry.addData("PoseEstimate", robot.drivebaseSubsystem.getPoseEstimate()); - telemetry.addData("Vis State: ", robot.visionSystem.visionPipeline.activeMode.toString()); - telemetry.addData("Junction X: ", robot.visionSystem.visionPipeline.junctionX); - telemetry.addData("Junction Y: ", robot.visionSystem.visionPipeline.junctionY); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/VisionAutoTest.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/VisionAutoTest.java index be6f75c0..200b711a 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/VisionAutoTest.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmode/testing/VisionAutoTest.java @@ -6,7 +6,6 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.twenty403.Hardware; import org.firstinspires.ftc.twenty403.Robot; -import org.firstinspires.ftc.twenty403.command.VisionCommand; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; @Autonomous(name = "Vision Auto Test") @@ -20,7 +19,7 @@ public void uponInit() { hardware = new Hardware(hardwareMap); robot = new Robot(hardware, Alliance.NONE, StartingPosition.LEFT); if (Robot.RobotConstant.CAMERA_CONNECTED) { - CommandScheduler.getInstance().scheduleInit(new VisionCommand(robot.visionSystem)); + CommandScheduler.scheduleInit(robot.visionSystem.runVision); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/ClawSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/ClawSubsystem.java index f8d70e35..44817944 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/ClawSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/ClawSubsystem.java @@ -1,6 +1,8 @@ package org.firstinspires.ftc.twenty403.subsystem; import com.acmerobotics.dashboard.config.Config; +import com.technototes.library.command.Command; +import com.technototes.library.command.SimpleRequiredCommand; import com.technototes.library.hardware.sensor.ColorDistanceSensor; import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Log; @@ -100,10 +102,9 @@ public boolean isClawClose() { return ( servoSet && //Math.abs(curPos - CLOSE_SERVO_POSITION) < Math.abs(curPos - OPEN_SERVO_POSITION) - ( - curPos <= CLOSE_SERVO_POSITION + autoCloseDeadzone - /*&& curPos >= CLOSE_SERVO_POSITION - autoCloseDeadzone*/ - ) + (curPos <= + CLOSE_SERVO_POSITION + + autoCloseDeadzone/*&& curPos >= CLOSE_SERVO_POSITION - autoCloseDeadzone*/) ); } @@ -119,8 +120,8 @@ public void periodic() { ) { close(); ///this.wait(.2); - //CommandScheduler.getInstance().schedule(new ClawAutoCloseWithLift(this,liftSubsystem)); - //CommandScheduler.getInstance().scheduleOnce(new ClawAutoCloseWithLift(this, liftSubsystem)); + //CommandScheduler.schedule(new ClawAutoCloseWithLift(this,liftSubsystem)); + //CommandScheduler.scheduleOnce(new ClawAutoCloseWithLift(this, liftSubsystem)); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/DrivebaseSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/DrivebaseSubsystem.java index 2641a47d..10fa8b62 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/DrivebaseSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/DrivebaseSubsystem.java @@ -49,7 +49,7 @@ public abstract static class DriveConstants implements MecanumConstants { 20, 0, 3, - MecanumConstants.getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV) + MecanumConstants.getMotorVelocityF((MAX_RPM / 60) * TICKS_PER_REV) ); @WheelRadius diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/LiftSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/LiftSubsystem.java index b9ad6e83..637cbc9d 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/LiftSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/LiftSubsystem.java @@ -5,6 +5,7 @@ import com.acmerobotics.roadrunner.control.PIDFController; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.util.Range; +import com.technototes.library.command.SimpleRequiredCommand; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/OdoSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/OdoSubsystem.java index 6217e873..89c1eaa1 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/OdoSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/OdoSubsystem.java @@ -58,7 +58,7 @@ public double WallDistance(double angle) { // cone stack is on the left if (leftDistance > 36 || rightDistance > 36) { return -123.4; - } else return (Math.cos(angle) * (leftDistance + rightDistance) / 2); + } else return ((Math.cos(angle) * (leftDistance + rightDistance)) / 2); } //Gray = Math.abs(Red - Blue) < 50; diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/VisionPipeline.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/VisionPipeline.java deleted file mode 100644 index f22562a0..00000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/VisionPipeline.java +++ /dev/null @@ -1,412 +0,0 @@ -package org.firstinspires.ftc.twenty403.subsystem; - -import android.graphics.Bitmap; -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.Range; -import com.technototes.library.logger.Log; -import com.technototes.library.logger.LogConfig; -import com.technototes.library.logger.Loggable; -import com.technototes.library.util.Alliance; -import java.util.function.Supplier; -import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; -import org.opencv.android.Utils; -import org.opencv.core.Core; -import org.opencv.core.Mat; -import org.opencv.core.Rect; -import org.opencv.core.Scalar; -import org.opencv.imgproc.Imgproc; -import org.openftc.easyopencv.OpenCvPipeline; - -@Config -public class VisionPipeline extends OpenCvPipeline implements Supplier, Loggable { - - public enum Mode { - Signal, - Junction, - Inactive, - } - - public Mode activeMode; - public Alliance alliance; - public StartingPosition side; - - public VisionPipeline(Alliance teamAlliance, StartingPosition startSide) { - super(); - alliance = teamAlliance; - side = startSide; - activeMode = Mode.Signal; - } - - @Config - public static class VisionConstants { - - @Config - public static class SignalDetection { - - public enum ParkingPosition { - LEFT, - CENTER, - RIGHT, - } - - // Yellow is around 25 (50 degrees) - public static double YELLOW = 30; - // Aqua is at 100 (200 degrees) - public static double AQUA = 100; - // Purple is at 170 (340 degrees) - public static double PINK = 170; - - // The low saturation point for color identification - public static double lowS = 70; - // The high saturation point for color identification - public static double highS = 255; - // The low value for color ID - public static double lowV = 50; - // The high value for color ID - public static double highV = 255; - // The 'range' around the hue that we're looking for - public static double RANGE = 10; - - // In the 160x120 bitmap, where are we looking? - public static int X = 165; - public static int Y = 136; - public static int WIDTH = 60; - public static int HEIGHT = 60; - - public static Scalar RGB_HIGHLIGHT = new Scalar(255, 128, 255); - } - - @Config - public static class JunctionDetection { - - public static double JYELLOW = 24; - // Other yellow value? - public static double JYELLOW2 = 28; - // the width, in pixels, of a junction - public static int JUNCTION_WIDTH = 10; - - public static double RANGE = 10; - - // The low saturation point for color identification - public static double lowS = 60; - // The high saturation point for color identification - public static double highS = 255; - // The low value for color ID - public static double lowV = 120; - // The high value for color ID - public static double highV = 255; - - // What color should we draw the outlining rectangle? - public static Scalar RGB_HIGHLIGHT = new Scalar(255, 128, 255); - public static int RED_RANGE = 10; - public static Scalar RGB_RED = new Scalar(255, 0, 0); - public static int GREEN_RANGE = 30; - public static Scalar RGB_GREEN = new Scalar(0, 255, 0); - public static Scalar RGB_BLACK = new Scalar(0, 0, 0); - public static int BLUE_RANGE = 50; - public static Scalar RGB_BLUE = new Scalar(0, 0, 255); - public static int YELLOW_RANGE = 70; - public static Scalar RGB_YELLOW = new Scalar(0, 255, 255); - public static int AQUA_RANGE = 90; - public static Scalar RGB_AQUA = new Scalar(255, 255, 0); - public static int PINK_RANGE = 110; - public static Scalar RGB_PINK = new Scalar(255, 0, 255); - public static int PURPLE_RANGE = 130; - public static Scalar RGB_PURPLE = new Scalar(128, 0, 128); - public static int WHITE_RANGE = 150; - public static Scalar RGB_WHITE = new Scalar(255, 255, 255); - - @Config - public static class OnlyXRight { - - public static double RIGHT_CENTER = 220; - public static double RIGHT_RANGE = 60; - } - - @Config - public static class OnlyXLeft { - - public static double LEFT_CENTER = 200; - public static double LEFT_RANGE = 80; - } - - @Config - public static class OnlyYForward { - - public static double FORWARD_CENTER = 220; - public static double FORWARD_RANGE = 70; - } - } - } - - @LogConfig.Run(duringRun = false, duringInit = true) - @Log.Boolean(name = "left") - public volatile boolean leftDetected = false; - - @LogConfig.Run(duringRun = false, duringInit = true) - @Log.Boolean(name = "middle") - public volatile boolean middleDetected = true; - - @LogConfig.Run(duringRun = false, duringInit = true) - @Log.Boolean(name = "right") - public volatile boolean rightDetected = false; - - @LogConfig.Run(duringRun = true, duringInit = true) - @Log.Number(name = "FPS") - public volatile double fps = 0.0; - - @LogConfig.Run(duringRun = true, duringInit = true) - @Log.Number(name = "Junction X") - public volatile double junctionX = -1; - - @LogConfig.Run(duringRun = true, duringInit = true) - @Log.Number(name = "Junction Y") - public volatile double junctionY = -1; - - private ElapsedTime time = new ElapsedTime(); - - public Mat customColorSpace = new Mat(); - public Mat Cr = new Mat(); - public Mat img = null; - - private int countColor(double hue) { - Scalar edge1 = new Scalar( - hue - VisionConstants.SignalDetection.RANGE, - VisionConstants.SignalDetection.lowS, - VisionConstants.SignalDetection.lowV - ); - Scalar edge2 = new Scalar( - hue + VisionConstants.SignalDetection.RANGE, - VisionConstants.SignalDetection.highS, - VisionConstants.SignalDetection.highV - ); - // Check to see which pixels are between edge1 & edge2, output into a boolean matrix Cr - Core.inRange(customColorSpace, edge1, edge2, Cr); - int count = 0; - for (int i = 0; i < Cr.width(); i++) { - for (int j = 0; j < Cr.height(); j++) { - if (Cr.get(j, i)[0] > 0) { - count++; - // Draw a dot on the image at this point - input was put into img - // The color choice makes things stripey, which makes it easier to identify - if (VisionSubsystem.VisionSubsystemConstants.DEBUG_VIEW) { - double[] colorToDraw = ((j + i) & 3) != 0 ? edge1.val : edge2.val; - img.put( - j + VisionConstants.SignalDetection.Y, - i + VisionConstants.SignalDetection.X, - colorToDraw - ); - } - } - } - } - return count; - } - - public void detectSignal(Mat input) { - // Put the input matrix in a member variable, so that other functions can draw on it - img = input; - - // First, slice the smaller rectangle out of the overall bitmap: - Mat rectToLookAt = input.submat( - // Row start to Row end - VisionConstants.SignalDetection.Y, - VisionConstants.SignalDetection.Y + VisionConstants.SignalDetection.HEIGHT, - // Col start to Col end - VisionConstants.SignalDetection.X, - VisionConstants.SignalDetection.X + VisionConstants.SignalDetection.WIDTH - ); - - // Next, convert the RGB image to HSV, because HUE is much easier to identify colors in - // The output is in 'customColorSpace' - Imgproc.cvtColor(rectToLookAt, customColorSpace, Imgproc.COLOR_RGB2HSV); - - // Check to see which colors occur: - int countY = countColor(VisionConstants.SignalDetection.YELLOW); - int countA = countColor(VisionConstants.SignalDetection.AQUA); - int countP = countColor(VisionConstants.SignalDetection.PINK); - - // Check which spot we should park in - middleDetected = countA >= countY && countA >= countP; - rightDetected = countP >= countA && countP >= countY; - leftDetected = !rightDetected && !middleDetected; - - // Draw a rectangle around the area we're looking at, for debugging - int x = Range.clip(VisionConstants.SignalDetection.X - 1, 0, input.width() - 1); - int y = Range.clip(VisionConstants.SignalDetection.Y - 1, 0, input.height() - 1); - int w = Range.clip(VisionConstants.SignalDetection.WIDTH + 2, 1, input.width() - x); - int h = Range.clip(VisionConstants.SignalDetection.HEIGHT + 2, 1, input.height() - y); - Imgproc.rectangle( - input, - new Rect(x, y, w, h), - VisionConstants.SignalDetection.RGB_HIGHLIGHT - ); - } - - public void init(Mat firstFrame) { - detectSignal(firstFrame); - } - - private static boolean brightEnough(double[] color) { - return ( - color[1] > VisionConstants.JunctionDetection.lowS && - color[1] < VisionConstants.JunctionDetection.highS && - color[2] > VisionConstants.JunctionDetection.lowV && - color[2] < VisionConstants.JunctionDetection.highV - ); - } - - private static boolean inRange(double[] color, double target) { - return ( - color[0] <= target + VisionConstants.JunctionDetection.RANGE && - color[0] >= target - VisionConstants.JunctionDetection.RANGE - ); - } - - public void detectJunction(Mat frame) { - Imgproc.rectangle(img, new Rect(2, 2, 7, 9), VisionConstants.JunctionDetection.RGB_GREEN); - Imgproc.cvtColor(frame, customColorSpace, Imgproc.COLOR_RGB2HSV); - int startX = -1; - int endX = -1; - for (int j = customColorSpace.height() - 1; j >= 0; j--) { - for (int i = 0; i < customColorSpace.width(); i++) { - double[] color = customColorSpace.get(j, i); - if ( - brightEnough(color) && - ( - inRange(color, VisionConstants.JunctionDetection.JYELLOW) || - inRange(color, VisionConstants.JunctionDetection.JYELLOW2) - ) - ) { - if (startX == -1) { - startX = i; - } else { - endX = i; - } - img.put(j, i, VisionConstants.JunctionDetection.RGB_BLACK.val); - // Draw a dot on the image at this point - input was put into img - // The color choice makes things stripey, which makes it easier to identif - // if less than 20 for range after not seeing yellow than set both to -1 as not junction ypou are - // looking for - } else { - if ( - startX != -1 && - ( - Math.abs(startX - endX) < - VisionConstants.JunctionDetection.JUNCTION_WIDTH - ) || - endX == -1 - ) { - startX = -1; - endX = -1; - } - // Debug some stuff: - if (brightEnough(color)) { - // Let's draw some colors to help identify the right range - if (inRange(color, VisionConstants.JunctionDetection.RED_RANGE)) { - img.put(j, i, VisionConstants.JunctionDetection.RGB_RED.val); - } - if (inRange(color, VisionConstants.JunctionDetection.GREEN_RANGE)) { - img.put(j, i, VisionConstants.JunctionDetection.RGB_GREEN.val); - } - if (inRange(color, VisionConstants.JunctionDetection.BLUE_RANGE)) { - img.put(j, i, VisionConstants.JunctionDetection.RGB_BLUE.val); - } - if (inRange(color, VisionConstants.JunctionDetection.YELLOW_RANGE)) { - img.put(j, i, VisionConstants.JunctionDetection.RGB_YELLOW.val); - } - if (inRange(color, VisionConstants.JunctionDetection.AQUA_RANGE)) { - img.put(j, i, VisionConstants.JunctionDetection.RGB_AQUA.val); - } - if (inRange(color, VisionConstants.JunctionDetection.PINK_RANGE)) { - img.put(j, i, VisionConstants.JunctionDetection.RGB_PINK.val); - } - if (inRange(color, VisionConstants.JunctionDetection.PURPLE_RANGE)) { - img.put(j, i, VisionConstants.JunctionDetection.RGB_PURPLE.val); - } - if (inRange(color, VisionConstants.JunctionDetection.WHITE_RANGE)) { - img.put(j, i, VisionConstants.JunctionDetection.RGB_WHITE.val); - } - } - } - } - - if (startX != -1 && endX != -1) { - // Check to see if the width of the range in the color is 'wide enough' to be a junction - if (Math.abs(startX - endX) > VisionConstants.JunctionDetection.JUNCTION_WIDTH) { - junctionY = j; - junctionX = (startX + endX) / 2; - Imgproc.rectangle( - img, - new Rect((int) junctionX, (int) junctionY, 5, 5), - VisionConstants.JunctionDetection.RGB_HIGHLIGHT - ); - return; - } - } - } - junctionX = -1; - junctionY = -1; - } - - @Override - public Mat processFrame(Mat input) { - // Update the FPS counter to see how slow the vision code is - // As of October 2022, it runs between 10 and 14 FPS. - fps = 1000 / time.milliseconds(); - time.reset(); - - switch (activeMode) { - case Signal: - detectSignal(input); - break; - case Junction: - detectJunction(input); - break; - case Inactive: - default: - return input; - } - if (VisionSubsystem.VisionSubsystemConstants.DEBUG_VIEW) { - sendBitmap(); - } - return input; - } - - @Override - public Integer get() { - return null; - } - - public boolean left() { - return leftDetected; - } - - public boolean middle() { - return middleDetected; - } - - public boolean right() { - return rightDetected; - } - - public double getJunctionX() { - return junctionX; - } - - public double getJunctionY() { - return junctionY; - } - - // Helper to send the bitmap to the FTC Dashboard - private void sendBitmap() { - FtcDashboard db = FtcDashboard.getInstance(); - if (db != null) { - Bitmap bitmap = Bitmap.createBitmap(img.cols(), img.rows(), Bitmap.Config.RGB_565); - Utils.matToBitmap(img, bitmap); - db.sendImage(bitmap); - } - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/VisionSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/VisionSubsystem.java index 3666caef..08e38ff8 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/VisionSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystem/VisionSubsystem.java @@ -1,63 +1,216 @@ package org.firstinspires.ftc.twenty403.subsystem; import com.acmerobotics.dashboard.config.Config; +import com.technototes.library.logger.Log; +import com.technototes.library.logger.LogConfig; import com.technototes.library.logger.Loggable; -import com.technototes.library.subsystem.Subsystem; import com.technototes.library.util.Alliance; -import com.technototes.vision.hardware.Webcam; +import com.technototes.vision.HSVRange; +import com.technototes.vision.hardware.Camera; +import com.technototes.vision.subsystem.BasicVisionSubsystem; import org.firstinspires.ftc.twenty403.command.autonomous.StartingPosition; +import org.opencv.core.Mat; +import org.opencv.core.Rect; import org.openftc.easyopencv.OpenCvCameraRotation; -public class VisionSubsystem implements Subsystem, Loggable { +public class VisionSubsystem extends BasicVisionSubsystem implements Loggable { @Config - public static class VisionSubsystemConstants { - - // This is a super-low res image. I don't think we need higher resolution... - // Note: This may be too small for the older camera. - // I think it only goes down to 320 x 240 - public static int WIDTH = 320; - public static int HEIGHT = 240; - // Change this if the camera is oriented differently + public static class VisionConstants { + + public static int CAM_WIDTH = 320; + public static int CAM_HEIGHT = 240; public static OpenCvCameraRotation ROTATION = OpenCvCameraRotation.UPRIGHT; - // Turn this on if we want to see the debug image - public static boolean DEBUG_VIEW = true; + + public static int CYAN = 100; + public static int YELLOW = 30; + public static int MAGENTA = 170; + + public static int HUE_RANGE = 10; + public static int SAT_LO = 70; + public static int SAT_HI = 255; + public static int VAL_LO = 50; + public static int VAL_HI = 255; + } + + public static class Rects { + + @Config + public static class Red { + + public static int RX = 10; + public static int RY = 20; + public static int RW = 25; + public static int RH = 25; + + public static int LX = 120; + public static int LY = 110; + public static int LW = 25; + public static int LH = 25; + } + + @Config + public static class Blue { + + public static int RX = 15; + public static int RY = 25; + public static int RW = 25; + public static int RH = 25; + + public static int LX = 100; + public static int LY = 90; + public static int LW = 25; + public static int LH = 25; + } + } + + @LogConfig.Run(duringRun = false, duringInit = true) + @Log.Boolean(name = "left") + public volatile boolean leftDetected = false; + + @LogConfig.Run(duringRun = false, duringInit = true) + @Log.Boolean(name = "middle") + public volatile boolean middleDetected = true; + + @LogConfig.Run(duringRun = false, duringInit = true) + @Log.Boolean(name = "right") + public volatile boolean rightDetected = false; + + Alliance alliance; + StartingPosition side; + Rect[] rects; + + // Here's the commend to use... + public final VisCommand runVision; + + public VisionSubsystem(Camera c, Alliance a, StartingPosition s) { + // Initialize the underlying subsystem and vision pipeline + super(c, VisionConstants.CAM_WIDTH, VisionConstants.CAM_HEIGHT, VisionConstants.ROTATION); + alliance = a; + side = s; + rects = new Rect[4]; + rects[0] = new Rect(Rects.Red.RX, Rects.Red.RY, Rects.Red.RW, Rects.Red.RH); + rects[1] = new Rect(Rects.Red.LX, Rects.Red.LY, Rects.Red.LW, Rects.Red.LH); + rects[2] = new Rect(Rects.Blue.RX, Rects.Blue.RY, Rects.Blue.RW, Rects.Blue.RH); + rects[3] = new Rect(Rects.Blue.LX, Rects.Blue.LY, Rects.Blue.LW, Rects.Blue.LH); + runVision = new VisCommand(); + } + + @Override + protected void detectionStart() { + // For PowerPlay, we're only looking at 1 spot on the camera, + // so we don't really need to do anything here + // But I'm updating the rectangles in real-time here, so they get updated by the + // FTC Dashboard in real time... + rects[0].x = Rects.Red.RX; + rects[0].y = Rects.Red.RY; + rects[0].width = Rects.Red.RW; + rects[0].height = Rects.Red.RH; + rects[1].x = Rects.Red.LX; + rects[1].y = Rects.Red.LY; + rects[1].width = Rects.Red.LW; + rects[1].height = Rects.Red.LH; + rects[2].x = Rects.Blue.RX; + rects[2].y = Rects.Blue.RY; + rects[2].width = Rects.Blue.RW; + rects[2].height = Rects.Blue.RH; + rects[3].x = Rects.Blue.LX; + rects[3].y = Rects.Blue.LY; + rects[3].width = Rects.Blue.LW; + rects[3].height = Rects.Blue.LH; + } + + @Override + public int numRectangles() { + return 4; + } + + private int getRectNum() { + if (alliance == Alliance.RED) { + return (side == StartingPosition.RIGHT) ? 0 : 1; + } else { + return (side == StartingPosition.RIGHT) ? 2 : 3; + } } - public Webcam camera; - public VisionPipeline visionPipeline; + @Override + public Rect getRect(int rectNumber) { + // In PowerPlay, we only have 1 rectangle, but we need to know if we're running red or blue + return rects[rectNumber]; + } - public VisionSubsystem(Webcam c, Alliance alliance, StartingPosition side) { - camera = c; - visionPipeline = new VisionPipeline(alliance, side); + @Override + protected void detectionEnd() { + // Again, only looking at one rectangle, so we don't have any post-processing to do... } - public void startStreaming() { - camera.startStreaming( - VisionSubsystemConstants.WIDTH, - VisionSubsystemConstants.HEIGHT, - VisionSubsystemConstants.ROTATION + @Override + public void runDetection(Mat inputHSV, int rectNumber) { + // We only run detection for the current alliance/side + if (rectNumber != getRectNum()) { + return; + } + // These are the color ranges we're looking for + HSVRange cyan = new HSVRange( + VisionConstants.CYAN, + VisionConstants.HUE_RANGE, + VisionConstants.SAT_LO, + VisionConstants.SAT_HI, + VisionConstants.VAL_LO, + VisionConstants.VAL_HI ); + HSVRange yellow = cyan.newHue(VisionConstants.YELLOW, VisionConstants.HUE_RANGE); + HSVRange magenta = cyan.newHue(VisionConstants.MAGENTA, VisionConstants.HUE_RANGE); + + // Now, count the # of pixels of each color in the rectangle + int xo = rects[rectNumber].x; + int yo = rects[rectNumber].y; + int cyanCount = countPixelsOfColor(cyan, inputHSV, this.curFrameRGB, xo, yo); + int yellowCount = countPixelsOfColor(yellow, inputHSV, this.curFrameRGB, xo, yo); + int magentaCount = countPixelsOfColor(magenta, inputHSV, this.curFrameRGB, xo, yo); + + // We could save these values in member variables, then check them in 'detectionEnd' + // but since we're only processing a single rectangle, why bother? + leftDetected = cyanCount > yellowCount && cyanCount > magentaCount; + middleDetected = yellowCount > cyanCount && yellowCount > magentaCount; + rightDetected = !leftDetected && !middleDetected; } - public void startVisionPipeline() { - camera.setPipeline(visionPipeline); - // The i -> lambda appears to be for *error reporting* so this line is silly: - camera.openCameraDeviceAsync(this::startStreaming, i -> startVisionPipeline()); + // We need to expose these values as functions for use by a ChoiceCommand + public boolean left() { + return leftDetected; } - public void pauseScanning() { - visionPipeline.activeMode = VisionPipeline.Mode.Inactive; + public boolean middle() { + return middleDetected; } - public void startJunctionScanning() { - visionPipeline.activeMode = VisionPipeline.Mode.Junction; + public boolean right() { + return rightDetected; } - public void stopVisionPipeline() { - camera.setPipeline(null); - camera.closeCameraDeviceAsync(() -> { - /* Do we need to do anything to stop the vision pipeline? */ - }); + public class VisCommand implements com.technototes.library.command.Command { + + public VisCommand() { + addRequirements(VisionSubsystem.this); + } + + @Override + public void initialize() { + startVisionPipeline(); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean cancel) { + stopVisionPipeline(); + } } } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 9c358d32..155ffb77 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -40,7 +40,7 @@ dependencies { implementation 'androidx.appcompat:appcompat:1.3.1' // TechnoLib: Comment this *out* for TechnoLib local - implementation 'com.github.technototes:TechnoLib:1.3.0' // FLIP: TechnoLibLocal + // implementation 'com.github.technototes:TechnoLib:1.3.0' // FLIP: TechnoLibLocal // RoadRunner: implementation 'com.acmerobotics.roadrunner:core:0.5.6' diff --git a/settings.gradle b/settings.gradle index 4441b444..0f3bfc27 100644 --- a/settings.gradle +++ b/settings.gradle @@ -2,12 +2,12 @@ include ':FtcRobotController' // Uncomment this to use TechnoLib from a subdirectory. You also need to uncomment // stuff in Sixteen750/ // Or just try 'yarn libflip' to let the script do it for you... -// include 'RobotLibrary' // FLIP: TechnoLibLocal -// project(':RobotLibrary').projectDir = new File('TechnoLib/RobotLibrary') // FLIP: TechnoLibLocal -// include 'Path' // FLIP: TechnoLibLocal -// project(':Path').projectDir = new File('TechnoLib/Path') // FLIP: TechnoLibLocal -// include 'Vision' // FLIP: TechnoLibLocal -// project(':Vision').projectDir = new File('TechnoLib/Vision') // FLIP: TechnoLibLocal +include 'RobotLibrary' // FLIP: TechnoLibLocal +project(':RobotLibrary').projectDir = new File('TechnoLib/RobotLibrary') // FLIP: TechnoLibLocal +include 'Path' // FLIP: TechnoLibLocal +project(':Path').projectDir = new File('TechnoLib/Path') // FLIP: TechnoLibLocal +include 'Vision' // FLIP: TechnoLibLocal +project(':Vision').projectDir = new File('TechnoLib/Vision') // FLIP: TechnoLibLocal include ':Sixteen750' // FLIP: BUILD ALL BOTS include ':Swerveteen750' include ':Twenty403' diff --git a/yarn.lock b/yarn.lock index 9a47d5e6..9a101281 100644 --- a/yarn.lock +++ b/yarn.lock @@ -28,10 +28,10 @@ debug@^4.1.1, debug@^4.3.4: dependencies: ms "2.1.2" -java-parser@2.0.3: - version "2.0.3" - resolved "https://registry.yarnpkg.com/java-parser/-/java-parser-2.0.3.tgz#02b4d52d530c8ce569576e755e1c0a6f6bfbafb0" - integrity sha512-z+ZsqcY3LCN0NPK1KnC7AXFHQxtzlsADJ2OVQmenKS/4gHBQ6SkEQYIMVV/2WrxRBMWXMXW7KeOVmXPLHCMRAQ== +java-parser@2.0.5: + version "2.0.5" + resolved "https://registry.yarnpkg.com/java-parser/-/java-parser-2.0.5.tgz#6bd3e2f75670cca387b78c6a1b1d10dd29b09032" + integrity sha512-AwieTO24Itcu0GgP9pBXs8gkqBtkmReclpBgXF4NkbIjdS7cn7hqpebjTmb5ouYYLFR+m3yh5fR3nW1NRrthdg== dependencies: chevrotain "6.5.0" lodash "4.17.21" @@ -47,23 +47,23 @@ ms@2.1.2: integrity sha512-sGkPx+VjMtmA6MX27oA4FBFELFCZZ4S4XqeGOXCv68tT+jb3vk/RyaKWP0PTKyWtmLSM0b+adUTEvbs1PEaH2w== prettier-plugin-java@^2.0.0: - version "2.0.0" - resolved "https://registry.yarnpkg.com/prettier-plugin-java/-/prettier-plugin-java-2.0.0.tgz#a5c983d8edfd55a4e5d6b7e6132e8bb2eb3e8272" - integrity sha512-ct7Zgog2XUpZ+olF3Ed4gPBNU7aWBu0cnpgDX7eqs0IZVH+9ZAduRkaJ0HlreoOm7SdUOc7UoKK1vk3T/coEoQ== + version "2.3.1" + resolved "https://registry.yarnpkg.com/prettier-plugin-java/-/prettier-plugin-java-2.3.1.tgz#eafd0cbb9b23eab917d8a3a40e289c731647e670" + integrity sha512-OYn8skqKnE5YUVL8f2ocayA6XCJK8PqsEz3pfATbDqzgdaSYDLhE/s8KrXrX9gj8KXIG6Wx0CMoXTNH8+ED22w== dependencies: - java-parser "2.0.3" + java-parser "2.0.5" lodash "4.17.21" - prettier "2.8.0" + prettier "3.0.3" -prettier@2.8.0: - version "2.8.0" - resolved "https://registry.yarnpkg.com/prettier/-/prettier-2.8.0.tgz#c7df58393c9ba77d6fba3921ae01faf994fb9dc9" - integrity sha512-9Lmg8hTFZKG0Asr/kW9Bp8tJjRVluO8EJQVfY2T7FMw9T5jy4I/Uvx0Rca/XWf50QQ1/SS48+6IJWnrb+2yemA== +prettier@3.0.3: + version "3.0.3" + resolved "https://registry.yarnpkg.com/prettier/-/prettier-3.0.3.tgz#432a51f7ba422d1469096c0fdc28e235db8f9643" + integrity sha512-L/4pUDMxcNa8R/EthV08Zt42WBO4h1rarVtK0K+QJG0X187OLo7l699jWw0GKuwzkPQ//jMFA/8Xm6Fh3J/DAg== prettier@^2.8.3: - version "2.8.3" - resolved "https://registry.yarnpkg.com/prettier/-/prettier-2.8.3.tgz#ab697b1d3dd46fb4626fbe2f543afe0cc98d8632" - integrity sha512-tJ/oJ4amDihPoufT5sM0Z1SKEuKay8LfVAMlbbhnnkvt6BUserZylqo2PN+p9KeljLr0OHa2rXHU1T8reeoTrw== + version "2.8.8" + resolved "https://registry.yarnpkg.com/prettier/-/prettier-2.8.8.tgz#e8c5d7e98a4305ffe3de2e1fc4aca1a71c28b1da" + integrity sha512-tdN8qQGvNjw4CHbY+XXk0JgCXn9QiF21a55rBe5LJAU+kDyC4WQn4+awm2Xfk2lQMk5fKup9XgzTZtGkjBdP9Q== regexp-to-ast@0.4.0: version "0.4.0" @@ -76,9 +76,9 @@ run-script-os@^1.1.6: integrity sha512-ql6P2LzhBTTDfzKts+Qo4H94VUKpxKDFz6QxxwaUZN0mwvi7L3lpOI7BqPCq7lgDh3XLl0dpeXwfcVIitlrYrw== simple-git@^3.16.0: - version "3.16.0" - resolved "https://registry.yarnpkg.com/simple-git/-/simple-git-3.16.0.tgz#421773e24680f5716999cc4a1d60127b4b6a9dec" - integrity sha512-zuWYsOLEhbJRWVxpjdiXl6eyAyGo/KzVW+KFhhw9MqEEJttcq+32jTWSGyxTdf9e/YCohxRE+9xpWFj9FdiJNw== + version "3.20.0" + resolved "https://registry.yarnpkg.com/simple-git/-/simple-git-3.20.0.tgz#ff9c3f736d6b2bf0e3510209569d206aac84833d" + integrity sha512-ozK8tl2hvLts8ijTs18iFruE+RoqmC/mqZhjs/+V7gS5W68JpJ3+FCTmLVqmR59MaUQ52MfGQuWsIqfsTbbJ0Q== dependencies: "@kwsites/file-exists" "^1.1.1" "@kwsites/promise-deferred" "^1.1.1"