diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BangBangController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BangBangController.java new file mode 100644 index 00000000000..a0886d90920 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BangBangController.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode; + +public class BangBangController { + private double lowValue = 0.0; + private double highValue = 1.0; + private double tolerance = 0.0; + + public BangBangController(double tolerance) { + this.tolerance = tolerance; + } + + public void setTolerance(double tolerance) { + this.tolerance = tolerance; + } + + public void setHighValue(double highValue) { + this.highValue = highValue; + } + + public void setLowValue(double lowValue) { + this.lowValue = lowValue; + } + + public double calculate(double measurement, double setpoint) { + if (Math.abs(measurement - setpoint) < tolerance) return lowValue; + else if (measurement < setpoint) { + return highValue; + } else return lowValue; + } + + public boolean isAtSetpoint(double measurement, double setpoint) { + return Math.abs(measurement - setpoint) < tolerance; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/BlueFarCycles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/BlueFarCycles.java index 740bc77b9fe..9e71074a0fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/BlueFarCycles.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/BlueFarCycles.java @@ -29,8 +29,8 @@ @Autonomous(group = "red far") public class BlueFarCycles extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double turretStartAngle = 90; @@ -42,11 +42,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, Math.PI / 2)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -73,63 +73,63 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ActionCommand(park.build(), requirements) @@ -146,12 +146,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -168,6 +168,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/BlueFarCyclesFull.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/BlueFarCyclesFull.java index 0eac54232d3..6ec85351bfe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/BlueFarCyclesFull.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/BlueFarCyclesFull.java @@ -29,8 +29,8 @@ @Autonomous(group = "red far") public class BlueFarCyclesFull extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double turretStartAngle = 90; @@ -42,11 +42,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, Math.PI / 2)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -73,62 +73,62 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(stack.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ActionCommand(park.build(), requirements) @@ -145,12 +145,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -167,6 +167,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/RedFarCycles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/RedFarCycles.java index 3c8d477e9ac..9845a9aec05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/RedFarCycles.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/RedFarCycles.java @@ -29,8 +29,8 @@ @Autonomous(group = "red far") public class RedFarCycles extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double turretStartAngle = 270; @@ -42,11 +42,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, Math.PI / 2)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -73,63 +73,63 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ActionCommand(park.build(), requirements) @@ -146,12 +146,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -168,6 +168,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/RedFarCyclesFull.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/RedFarCyclesFull.java index fd138424695..aab852e3107 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/RedFarCyclesFull.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/Far/RedFarCyclesFull.java @@ -29,8 +29,8 @@ @Autonomous(group = "red far") public class RedFarCyclesFull extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double turretStartAngle = 270; @@ -42,11 +42,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, Math.PI / 2)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -73,62 +73,62 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(stack.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2000), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2000), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ActionCommand(park.build(), requirements) @@ -145,12 +145,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -167,6 +167,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/BlueClose12.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/BlueClose12.java index d37de5d2c3d..db3efa8dbd3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/BlueClose12.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/BlueClose12.java @@ -18,7 +18,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -32,8 +31,8 @@ @Autonomous(name = "12 blue close", group = "blue close") public class BlueClose12 extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -46,11 +45,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -85,41 +84,41 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( new WaitCommand(1500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(3500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -136,12 +135,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -157,6 +156,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/BlueClose12Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/BlueClose12Sorted.java index d1572720cd7..142e666b787 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/BlueClose12Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/BlueClose12Sorted.java @@ -18,7 +18,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -32,8 +31,8 @@ @Autonomous(name = "12 blue close sort", group = "blue close") public class BlueClose12Sorted extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -46,11 +45,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -84,41 +83,41 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( new WaitCommand(1500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.sortedShooting(intake, carousel) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(3500), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -135,12 +134,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -155,6 +154,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/RedClose12.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/RedClose12.java index 29d2ae7b897..1510629aec5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/RedClose12.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/RedClose12.java @@ -18,7 +18,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -32,8 +31,8 @@ @Autonomous(name = "12 red close", group = "red close") public class RedClose12 extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -46,11 +45,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -84,41 +83,41 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( new WaitCommand(1500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(3500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -134,12 +133,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/RedClose12Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/RedClose12Sorted.java index 880bdfad6f9..44bf6cfe9b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/RedClose12Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close12/RedClose12Sorted.java @@ -18,7 +18,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -32,8 +31,8 @@ @Autonomous(name = "12 red close sort", group = "red close") public class RedClose12Sorted extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -46,11 +45,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -84,41 +83,41 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( new WaitCommand(1500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.sortedShooting(intake, carousel) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(3500), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -134,12 +133,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -155,6 +154,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/BlueClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/BlueClose.java index 3ac9883843a..3e53d08156c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/BlueClose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/BlueClose.java @@ -19,7 +19,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -33,8 +32,8 @@ @Autonomous(name = "18 blue close", group = "blue close") public class BlueClose extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -47,11 +46,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -101,7 +100,7 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), @@ -109,25 +108,25 @@ public void initialize() { // new InstantCommand(() -> DischargeSubsystem.shooting = true), // new WaitCommand(600), // new InstantCommand(() -> DischargeSubsystem.shooting = false), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), // new ParallelCommandGroup( // new ActionCommand(wheatleyAutoTwoP2.build(), requirements), -// new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) +// new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) // ), // new WaitCommand(500), new ParallelRaceGroup( new ActionCommand(midOneP1.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(pressGate.build(), requirements).withTimeout(450), @@ -139,14 +138,14 @@ public void initialize() { new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelRaceGroup( new ActionCommand(midOneP1.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(pressGate.build(), requirements).withTimeout(650), @@ -157,12 +156,12 @@ public void initialize() { new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelRaceGroup( new ActionCommand(midOneP1.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(pressGate.build(), requirements).withTimeout(650), @@ -173,19 +172,19 @@ public void initialize() { new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements)), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -201,12 +200,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/BlueCloseFull.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/BlueCloseFull.java index c99a0856238..14054938ab8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/BlueCloseFull.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/BlueCloseFull.java @@ -20,7 +20,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -34,8 +33,8 @@ @Autonomous(name = "18 blue close Full", group = "blue close") public class BlueCloseFull extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -48,11 +47,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -110,7 +109,7 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), @@ -118,14 +117,14 @@ public void initialize() { // new InstantCommand(() -> DischargeSubsystem.shooting = true), // new WaitCommand(600), // new InstantCommand(() -> DischargeSubsystem.shooting = false), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), @@ -139,19 +138,19 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), + CommandGroups.startIntake(intake, carousel).withTimeout(400), new ParallelCommandGroup( - new IntakeCommands.OutTakeState(intakeSubsystem), + intake.outtake(), new WaitCommand(500) ), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), @@ -163,41 +162,41 @@ public void initialize() { new ActionCommand(pressGate.build(), requirements) ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new WaitCommand(900) ), new ParallelCommandGroup( - new IntakeCommands.OutTakeState(intakeSubsystem), + intake.outtake(), new WaitCommand(500) ), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements)), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2700), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(600), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2700), + CommandGroups.startOuttake(intake, carousel).withTimeout(600), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -214,12 +213,12 @@ public void initialize_loop() { if (time.seconds() > 3) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.addData("heading", mecanumDrive.localizer.getPose().heading.toDouble() * 180 / Math.PI); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/RedClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/RedClose.java index 757e21c3825..9c576930bc5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/RedClose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/RedClose.java @@ -20,7 +20,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -34,8 +33,8 @@ @Autonomous(name = "18 red close", group = "red close") public class RedClose extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -48,11 +47,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -102,7 +101,7 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), @@ -110,14 +109,14 @@ public void initialize() { // new InstantCommand(() -> DischargeSubsystem.shooting = true), // new WaitCommand(600), // new InstantCommand(() -> DischargeSubsystem.shooting = false), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), @@ -131,7 +130,7 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), @@ -139,8 +138,8 @@ public void initialize() { new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(400), - new IntakeCommands.OutTakeState(intakeSubsystem).withTimeout(350), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + intake.outtake().withTimeout(350), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), @@ -153,7 +152,7 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), @@ -161,8 +160,8 @@ public void initialize() { new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(400), - new IntakeCommands.OutTakeState(intakeSubsystem).withTimeout(350), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + intake.outtake().withTimeout(350), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelRaceGroup( new SequentialCommandGroup( @@ -172,26 +171,26 @@ public void initialize() { new ActionCommand(pressGate.build(), requirements) ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(400), - new IntakeCommands.OutTakeState(intakeSubsystem).withTimeout(350), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + intake.outtake().withTimeout(350), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements)), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -207,12 +206,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/RedCloseFull.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/RedCloseFull.java index 6c479c864f9..8be78376b16 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/RedCloseFull.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close18/RedCloseFull.java @@ -20,7 +20,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -34,8 +33,8 @@ @Autonomous(name = "18 red close Full", group = "red close") public class RedCloseFull extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -48,11 +47,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -108,7 +107,7 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), @@ -116,14 +115,14 @@ public void initialize() { // new InstantCommand(() -> DischargeSubsystem.shooting = true), // new WaitCommand(600), // new InstantCommand(() -> DischargeSubsystem.shooting = false), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), @@ -138,19 +137,19 @@ public void initialize() { new WaitCommand(300) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), + CommandGroups.startIntake(intake, carousel).withTimeout(400), new ParallelCommandGroup( - new IntakeCommands.OutTakeState(intakeSubsystem), + intake.outtake(), new WaitCommand(200) ), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), @@ -163,38 +162,38 @@ public void initialize() { ), new WaitCommand(500) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new WaitCommand(900) ), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(200), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(200), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements)), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2700), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(600), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2700), + CommandGroups.startOuttake(intake, carousel).withTimeout(600), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -210,12 +209,12 @@ public void initialize_loop() { if (time.seconds() > 3) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.addData("heading", mecanumDrive.localizer.getPose().heading.toDouble() * 180 / Math.PI); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/BlueClose21.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/BlueClose21.java index 018fafc9eaa..0b995dff002 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/BlueClose21.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/BlueClose21.java @@ -33,8 +33,8 @@ @Autonomous(name = "21 blue close", group = "blue close") public class BlueClose21 extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; @@ -47,11 +47,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -107,7 +107,7 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), @@ -115,14 +115,14 @@ public void initialize() { // new InstantCommand(() -> DischargeSubsystem.shooting = true), // new WaitCommand(600), // new InstantCommand(() -> DischargeSubsystem.shooting = false), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), @@ -136,16 +136,16 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(600), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startIntake(intake, carousel).withTimeout(400), + CommandGroups.startOuttake(intake, carousel).withTimeout(600), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelRaceGroup( @@ -158,23 +158,23 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startIntake(intake, carousel).withTimeout(400), + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements)), @@ -189,18 +189,18 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new WaitCommand(900) ), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), @@ -214,23 +214,23 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new WaitCommand(900) ), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ) @@ -247,12 +247,12 @@ public void initialize_loop() { if (time.seconds() > 3) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.addData("heading", mecanumDrive.localizer.getPose().heading.toDouble() * 180 / Math.PI); @@ -272,6 +272,6 @@ public void run() { @Override public void end() { - intakeSubsystem.stopSensorThread(); + intake.stopSensorThread(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/BlueClose21Full.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/BlueClose21Full.java index a256a64eff2..1ead1f1890e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/BlueClose21Full.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/BlueClose21Full.java @@ -33,8 +33,8 @@ @Autonomous(name = "21 blue close full", group = "blue close") public class BlueClose21Full extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; @@ -47,11 +47,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -66,7 +66,7 @@ public void initialize() { .splineToLinearHeading(new Pose2d(16, 36, Math.PI / 2), Math.PI / 2) .splineToSplineHeading(new Pose2d(16, 36.1, Math.PI / 2), Math.PI / 2) // .splineToConstantHeading(new Vector2d(14, 48), Math.PI *5/ 8) - .splineToConstantHeading(new Vector2d(-16, 7), -Math.PI * 3 / 4,null, new ProfileAccelConstraint(-60,60)); + .splineToConstantHeading(new Vector2d(-16, 7), -Math.PI * 3 / 4, null, new ProfileAccelConstraint(-60, 60)); TrajectoryActionBuilder wheatleyAutoTwoP2 = mecanumDrive.actionBuilder((new Pose2d(14, 44, Math.PI / 2)), reversed) .setTangent(-Math.PI / 2) .splineToConstantHeading(new Vector2d(-8, 12), -Math.PI * 3 / 4); @@ -88,7 +88,7 @@ public void initialize() { .splineToLinearHeading(new Pose2d(-8, 10, Math.PI / 2), -Math.PI * 5 / 8, null, new ProfileAccelConstraint(-100, 100)); TrajectoryActionBuilder midTwoP2 = mecanumDrive.actionBuilder(new Pose2d(12, 57, Math.PI * 12.1 / 16), reversed) .setTangent(-Math.PI / 2) - .splineToLinearHeading(new Pose2d(-13, 15, Math.PI / 2), -Math.PI * 6 / 8,null,new ProfileAccelConstraint(-100,100)); + .splineToLinearHeading(new Pose2d(-13, 15, Math.PI / 2), -Math.PI * 6 / 8, null, new ProfileAccelConstraint(-100, 100)); TrajectoryActionBuilder wheatleyAutoThree = mecanumDrive.actionBuilder(new Pose2d(-13, 20, Math.PI / 2), reversed) .setTangent(Math.PI / 2) @@ -99,7 +99,7 @@ public void initialize() { .splineToConstantHeading(new Vector2d(36, 28), Math.PI / 4) .splineToConstantHeading(new Vector2d(42, 52), Math.PI / 2) .splineToConstantHeading(new Vector2d(42, 52.1), -Math.PI / 2) - .splineToConstantHeading(new Vector2d(-18, 10), -Math.PI * 3 / 4,null,new ProfileAccelConstraint(-60,60)); + .splineToConstantHeading(new Vector2d(-18, 10), -Math.PI * 3 / 4, null, new ProfileAccelConstraint(-60, 60)); TrajectoryActionBuilder prepareGate = mecanumDrive.actionBuilder(new Pose2d(-6, 16, Math.PI / 2), reversed) .splineToConstantHeading(new Vector2d(0, 25), Math.PI / 2); @@ -107,7 +107,7 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), @@ -115,14 +115,14 @@ public void initialize() { // new InstantCommand(() -> DischargeSubsystem.shooting = true), // new WaitCommand(600), // new InstantCommand(() -> DischargeSubsystem.shooting = false), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), @@ -136,16 +136,16 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(600), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startIntake(intake, carousel).withTimeout(400), + CommandGroups.startOuttake(intake, carousel).withTimeout(600), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelRaceGroup( @@ -158,23 +158,23 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startIntake(intake, carousel).withTimeout(400), + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements)), @@ -189,33 +189,33 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new WaitCommand(900) ), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2500), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2500), + CommandGroups.startOuttake(intake, carousel).withTimeout(400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ) @@ -232,12 +232,12 @@ public void initialize_loop() { if (time.seconds() > 3) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.addData("heading", mecanumDrive.localizer.getPose().heading.toDouble() * 180 / Math.PI); @@ -257,6 +257,6 @@ public void run() { @Override public void end() { - intakeSubsystem.stopSensorThread(); + intake.stopSensorThread(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/RedClose21.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/RedClose21.java index 1e9c5a8bc2a..eb757ac240a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/RedClose21.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/RedClose21.java @@ -33,8 +33,8 @@ @Autonomous(name = "21 red close", group = "red close") public class RedClose21 extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; @@ -47,11 +47,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -107,7 +107,7 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), @@ -115,14 +115,14 @@ public void initialize() { // new InstantCommand(() -> DischargeSubsystem.shooting = true), // new WaitCommand(600), // new InstantCommand(() -> DischargeSubsystem.shooting = false), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), @@ -136,16 +136,16 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(600), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startIntake(intake, carousel).withTimeout(400), + CommandGroups.startOuttake(intake, carousel).withTimeout(600), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelRaceGroup( @@ -158,23 +158,23 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startIntake(intake, carousel).withTimeout(400), + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), @@ -190,18 +190,18 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new WaitCommand(900) ), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), @@ -215,23 +215,23 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new WaitCommand(900) ), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ) @@ -248,12 +248,12 @@ public void initialize_loop() { if (time.seconds() > 3) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.addData("heading", mecanumDrive.localizer.getPose().heading.toDouble() * 180 / Math.PI); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/RedClose21Full.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/RedClose21Full.java index 5f6a908077f..644147a0bbc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/RedClose21Full.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/close21/RedClose21Full.java @@ -33,8 +33,8 @@ @Autonomous(name = "21 red close full", group = "red close") public class RedClose21Full extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; @@ -47,11 +47,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -107,7 +107,7 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), @@ -115,14 +115,14 @@ public void initialize() { // new InstantCommand(() -> DischargeSubsystem.shooting = true), // new WaitCommand(600), // new InstantCommand(() -> DischargeSubsystem.shooting = false), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), @@ -136,16 +136,16 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(600), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startIntake(intake, carousel).withTimeout(400), + CommandGroups.startOuttake(intake, carousel).withTimeout(600), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelRaceGroup( @@ -158,23 +158,23 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startIntake(intake, carousel).withTimeout(400), + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + CommandGroups.startOuttake(intake, carousel).withTimeout(300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), @@ -190,33 +190,33 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new WaitCommand(900) ), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2500), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2500), + CommandGroups.startOuttake(intake, carousel).withTimeout(400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ) @@ -233,12 +233,12 @@ public void initialize_loop() { if (time.seconds() > 3) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.addData("heading", mecanumDrive.localizer.getPose().heading.toDouble() * 180 / Math.PI); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RedClose18Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RedClose18Sorted.java index 4c500c9c9b1..a8da13d2068 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RedClose18Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RedClose18Sorted.java @@ -20,7 +20,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -34,8 +33,8 @@ @Autonomous(name = "18 red close sorted", group = "red close") public class RedClose18Sorted extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -48,11 +47,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -108,7 +107,7 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), @@ -116,14 +115,14 @@ public void initialize() { // new InstantCommand(() -> DischargeSubsystem.shooting = true), // new WaitCommand(600), // new InstantCommand(() -> DischargeSubsystem.shooting = false), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), @@ -137,19 +136,19 @@ public void initialize() { ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midOneP2.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(400), + CommandGroups.startIntake(intake, carousel).withTimeout(400), new ParallelCommandGroup( - new IntakeCommands.OutTakeState(intakeSubsystem), + intake.outtake(), new WaitCommand(500) ), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), @@ -161,38 +160,38 @@ public void initialize() { new ActionCommand(pressGate.build(), requirements) ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(midTwoP2.build(), requirements), new SequentialCommandGroup( new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new WaitCommand(900) ), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(500), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem)) + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + CommandGroups.sortedShooting(intake, carousel)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoThree.build(), requirements)), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2700), - new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem).withTimeout(600), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(2700), + CommandGroups.startOuttake(intake, carousel).withTimeout(600), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -209,12 +208,12 @@ public void initialize_loop() { if (time.seconds() > 3) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.addData("heading", mecanumDrive.localizer.getPose().heading.toDouble() * 180 / Math.PI); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RedClose18V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RedClose18V2.java index c4099389263..e5d3db7d788 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RedClose18V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RedClose18V2.java @@ -15,14 +15,12 @@ import com.seattlesolvers.solverslib.command.SequentialCommandGroup; import com.seattlesolvers.solverslib.command.Subsystem; import com.seattlesolvers.solverslib.command.WaitCommand; -import com.seattlesolvers.solverslib.command.WaitUntilCommand; import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.actions.ActionCommand; import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -36,8 +34,8 @@ @Autonomous(name = "18 red close V2", group = "red close") public class RedClose18V2 extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -50,89 +48,89 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); boolean reversed = SavedValues.teamColor == AutoShooter.TeamColor.BLUE; requirements.add(mecanumDrive); - TrajectoryActionBuilder shootPreload = mecanumDrive.actionBuilder(new Pose2d(-41.2, 54.3, 0),reversed) + TrajectoryActionBuilder shootPreload = mecanumDrive.actionBuilder(new Pose2d(-41.2, 54.3, 0), reversed) .setTangent(-Math.PI / 4) .splineToConstantHeading(new Vector2d(-30, 42), -Math.PI / 4); - TrajectoryActionBuilder spikeOne = mecanumDrive.actionBuilder(new Pose2d(-30,42,0),reversed) + TrajectoryActionBuilder spikeOne = mecanumDrive.actionBuilder(new Pose2d(-30, 42, 0), reversed) .setTangent(Math.PI / 6) - .splineToConstantHeading(new Vector2d(-18,47),0)//might need to wait - .splineToConstantHeading(new Vector2d(-30,42),-Math.PI * 7 / 8); - TrajectoryActionBuilder spikeTwo = mecanumDrive.actionBuilder(new Pose2d(-30,42,0),reversed) + .splineToConstantHeading(new Vector2d(-18, 47), 0)//might need to wait + .splineToConstantHeading(new Vector2d(-30, 42), -Math.PI * 7 / 8); + TrajectoryActionBuilder spikeTwo = mecanumDrive.actionBuilder(new Pose2d(-30, 42, 0), reversed) .setTangent(Math.PI / 9) - .splineToConstantHeading(new Vector2d(6,47),0) + .splineToConstantHeading(new Vector2d(6, 47), 0) .setTangent(-Math.PI) - .splineTo(new Vector2d(-14,23),-Math.PI * 26 / 36); - TrajectoryActionBuilder gateCycle = mecanumDrive.actionBuilder(new Pose2d(-14,23,0),reversed) + .splineTo(new Vector2d(-14, 23), -Math.PI * 26 / 36); + TrajectoryActionBuilder gateCycle = mecanumDrive.actionBuilder(new Pose2d(-14, 23, 0), reversed) .setTangent(Math.PI * 10 / 36) // .splineTo(new Vector2d(5,40),Math.PI * 18 / 36) // .splineToSplineHeading(new Pose2d(5,40.1,Math.PI /2),Math.PI /2) - .splineToLinearHeading(new Pose2d(15,62,Math.PI * 10.5 / 16),Math.PI / 2); - TrajectoryActionBuilder pressGate = mecanumDrive.actionBuilder(new Pose2d(16,62.5,Math.PI * 10.5 / 16), reversed) - .setTangent(Math.PI * 3.5/4) + .splineToLinearHeading(new Pose2d(15, 62, Math.PI * 10.5 / 16), Math.PI / 2); + TrajectoryActionBuilder pressGate = mecanumDrive.actionBuilder(new Pose2d(16, 62.5, Math.PI * 10.5 / 16), reversed) + .setTangent(Math.PI * 3.5 / 4) .lineToX(12); - TrajectoryActionBuilder gateCycleP2 = mecanumDrive.actionBuilder(new Pose2d(14,60,Math.PI * 11 / 16)) - .setTangent(-Math.PI*5 / 16) - .splineTo(new Vector2d(-13,20),-Math.PI * 4/5); - TrajectoryActionBuilder spikeThree = mecanumDrive.actionBuilder(new Pose2d(-13,20,Math.PI / 5)) + TrajectoryActionBuilder gateCycleP2 = mecanumDrive.actionBuilder(new Pose2d(14, 60, Math.PI * 11 / 16)) + .setTangent(-Math.PI * 5 / 16) + .splineTo(new Vector2d(-13, 20), -Math.PI * 4 / 5); + TrajectoryActionBuilder spikeThree = mecanumDrive.actionBuilder(new Pose2d(-13, 20, Math.PI / 5)) .setTangent(Math.PI / 5) - .splineTo(new Vector2d(30,47),0) + .splineTo(new Vector2d(30, 47), 0) .setTangent(-Math.PI) - .splineTo(new Vector2d(-13,20),-Math.PI * 3 / 4); - TrajectoryActionBuilder loadingZone = mecanumDrive.actionBuilder(new Pose2d(-13,20,Math.PI / 4)) + .splineTo(new Vector2d(-13, 20), -Math.PI * 3 / 4); + TrajectoryActionBuilder loadingZone = mecanumDrive.actionBuilder(new Pose2d(-13, 20, Math.PI / 4)) .setTangent(Math.PI / 4) - .splineTo(new Vector2d(46,58),0) - .splineToSplineHeading(new Pose2d(46.1,58,0),-Math.PI)//outtake a bit - .splineToConstantHeading(new Vector2d(-8,16),-Math.PI * 3 / 4); - TrajectoryActionBuilder park = mecanumDrive.actionBuilder(new Pose2d(-8,16,0)) - .setTangent(Math.PI/4) - .splineToConstantHeading(new Vector2d(0,30),Math.PI/2); + .splineTo(new Vector2d(46, 58), 0) + .splineToSplineHeading(new Pose2d(46.1, 58, 0), -Math.PI)//outtake a bit + .splineToConstantHeading(new Vector2d(-8, 16), -Math.PI * 3 / 4); + TrajectoryActionBuilder park = mecanumDrive.actionBuilder(new Pose2d(-8, 16, 0)) + .setTangent(Math.PI / 4) + .splineToConstantHeading(new Vector2d(0, 30), Math.PI / 2); CommandScheduler.getInstance().schedule( new ParallelCommandGroup( - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new InstantCommand(() -> intakeSubsystem.setPosition(1)), + intake.open(), new ParallelCommandGroup( new ActionCommand(shootPreload.build(), requirements), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), - new InstantCommand(() -> intakeSubsystem.setPosition(1)), + intake.open(), new ParallelCommandGroup( new ActionCommand(spikeOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), - new InstantCommand(() -> intakeSubsystem.setPosition(1)), + intake.open(), new ParallelCommandGroup( new ActionCommand(spikeTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), - new InstantCommand(() -> intakeSubsystem.setPosition(1)), + intake.open(), new ParallelRaceGroup( new SequentialCommandGroup( new ActionCommand(gateCycle.build(), requirements), new ParallelDeadlineGroup( new WaitCommand(800), - new ActionCommand(pressGate.build(),requirements) + new ActionCommand(pressGate.build(), requirements) ) ), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), @@ -140,35 +138,34 @@ public void initialize() { new ActionCommand(gateCycleP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(200), - new IntakeCommands.OutTakeState(intakeSubsystem).withTimeout(550), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + intake.outtake().withTimeout(550), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), - new InstantCommand(() -> intakeSubsystem.setPosition(1)), + intake.open(), new ParallelCommandGroup( new ActionCommand(spikeThree.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2300), - new CommandGroups.StartOuttake(intakeSubsystem,carouselSubsystem).withTimeout(400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2300), + CommandGroups.startOuttake(intake, carousel).withTimeout(400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), - new InstantCommand(() -> intakeSubsystem.setPosition(1)), + intake.open(), new ParallelCommandGroup( new ActionCommand(loadingZone.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(2700), - new CommandGroups.StartOuttake(intakeSubsystem,carouselSubsystem).withTimeout(500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(2700), + CommandGroups.startOuttake(intake, carousel).withTimeout(500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), - new ParallelCommandGroup( new ActionCommand(park.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -184,12 +181,12 @@ public void initialize_loop() { if (time.seconds() > 3) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.addData("heading", mecanumDrive.localizer.getPose().heading.toDouble() * 180 / Math.PI); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RobotRevealAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RobotRevealAuto.java index 959cbab479f..aa574371301 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RobotRevealAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/expiremental/RobotRevealAuto.java @@ -3,7 +3,6 @@ import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.ProfileAccelConstraint; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; @@ -11,7 +10,6 @@ import com.qualcomm.robotcore.util.Range; import com.seattlesolvers.solverslib.command.CommandScheduler; import com.seattlesolvers.solverslib.command.ParallelCommandGroup; -import com.seattlesolvers.solverslib.command.ParallelRaceGroup; import com.seattlesolvers.solverslib.command.SequentialCommandGroup; import com.seattlesolvers.solverslib.command.Subsystem; import com.seattlesolvers.solverslib.command.WaitCommand; @@ -22,7 +20,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -36,8 +33,8 @@ @Autonomous() public class RobotRevealAuto extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double turretStartAngle = 90; @@ -49,11 +46,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, -Math.PI / 2)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -70,23 +67,23 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), - new WaitUntilCommand(() -> dischargeSubsystem.getRPM() > 3000), + new CommandGroups.Shoot(intake, carousel), + new WaitUntilCommand(() -> dischargeSubsystem.flywheel.getRPM() > 3000), new WaitCommand(1), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3000), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(3000), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1900), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1900), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ) @@ -103,12 +100,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -123,6 +120,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/BlueFar15.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/BlueFar15.java index 36202854673..93eb11a1173 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/BlueFar15.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/BlueFar15.java @@ -19,7 +19,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -33,8 +32,8 @@ @Autonomous(name = "15 blue far") public class BlueFar15 extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -47,11 +46,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, Math.PI / 2)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -91,48 +90,48 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1850), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1850), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new WaitCommand(100), - new IntakeCommands.ClosedState(intakeSubsystem), + intake.closeState(), new WaitCommand(800), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(3500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -148,12 +147,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -168,6 +167,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/BlueFar15Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/BlueFar15Sorted.java index 3868ec92055..c913169269e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/BlueFar15Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/BlueFar15Sorted.java @@ -19,7 +19,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -33,8 +32,8 @@ @Autonomous(name = "15 blue far sort") public class BlueFar15Sorted extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -47,11 +46,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, Math.PI / 2)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -91,48 +90,48 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1850), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1850), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new WaitCommand(100), - new IntakeCommands.ClosedState(intakeSubsystem), + intake.closeState(), new WaitCommand(800), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(300), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem)) + CommandGroups.sortedShooting(intake, carousel)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1300), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(1300), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(3500), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -148,12 +147,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -168,6 +167,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/RedFar15.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/RedFar15.java index b21a6378bb6..e8fe6e55d12 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/RedFar15.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/RedFar15.java @@ -19,7 +19,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -33,8 +32,8 @@ @Autonomous(name = "15 red far") public class RedFar15 extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -47,11 +46,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, Math.PI / 2)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -91,48 +90,48 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1850), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1850), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new WaitCommand(100), - new IntakeCommands.ClosedState(intakeSubsystem), + intake.closeState(), new WaitCommand(800), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(3500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -149,12 +148,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -169,7 +168,7 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/RedFar15Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/RedFar15Sorted.java index 90360d067b8..43eccf73c58 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/RedFar15Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/far15/RedFar15Sorted.java @@ -19,7 +19,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -33,8 +32,8 @@ @Autonomous(name = "15 red far sorted") public class RedFar15Sorted extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -47,11 +46,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, Math.PI / 2)); mecanumDrive.driveMode(); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); @@ -91,48 +90,48 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1850), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1850), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new WaitCommand(100), - new IntakeCommands.ClosedState(intakeSubsystem), + intake.closeState(), new WaitCommand(800), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(300), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem)) + CommandGroups.sortedShooting(intake, carousel)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1300), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(1300), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), - new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel).withTimeout(3500), + CommandGroups.sortedShooting(intake, carousel) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -149,12 +148,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -170,7 +169,7 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/NineAutoOld.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/NineAutoOld.java index 3880534c8a2..c859c076f75 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/NineAutoOld.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/NineAutoOld.java @@ -16,7 +16,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -30,8 +29,8 @@ @Autonomous public class NineAutoOld extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -39,11 +38,11 @@ public class NineAutoOld extends ActionOpMode { @Override public void initialize() { Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-47.2, 47.7, Math.PI)); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); boolean reversed = SavedValues.teamColor == AutoShooter.TeamColor.BLUE; @@ -77,28 +76,28 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem,mecanumDrive,dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), // new WaitCommand(4000), - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), // new WaitCommand(10000), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelRaceGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem), + CommandGroups.startIntake(intake, carousel), new ActionCommand(wheatleyAutoThree.build(), requirements) ), - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -115,6 +114,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.carouselTicks = carouselSubsystem.getPosition(); + SavedValues.carouselTicks = carousel.getPosition(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/NineAutoReal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/NineAutoReal.java index 80ebbe61088..3f9e92bba9c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/NineAutoReal.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/NineAutoReal.java @@ -17,7 +17,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -31,8 +30,8 @@ @Autonomous(name = "9 red") public class NineAutoReal extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -40,11 +39,11 @@ public class NineAutoReal extends ActionOpMode { @Override public void initialize() { Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-41.2, 54.3, 0)); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); boolean reversed = SavedValues.teamColor == AutoShooter.TeamColor.BLUE; @@ -76,41 +75,41 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( new WaitCommand(1300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1400), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1400), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), // new ParallelCommandGroup( // new SequentialCommandGroup( -// new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), -// new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive, SavedValues.teamColor) +// CommandGroups.startIntake(intake, carousel).withTimeout(3500), +// new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive, SavedValues.teamColor) // ), // new ActionCommand(wheatleyAutoFour.build(), requirements) // ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -128,6 +127,6 @@ public void run() { multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/RedFar18.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/RedFar18.java index 955eb7fc41d..818f23acc2b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/RedFar18.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeAutos/old/RedFar18.java @@ -19,7 +19,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -33,8 +32,8 @@ @Autonomous(name = "18 red far") public class RedFar18 extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; double iteration = 1; @@ -47,11 +46,11 @@ public void initialize() { SavedValues.currentCount = 0; SavedValues.teamColor = AutoShooter.TeamColor.RED; Set requirements = new HashSet<>(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); dischargeSubsystem.resetTurret(); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(61.5, 22, Math.PI / 2)); limelightSubsystem = new LimelightSubsystem(hardwareMap, SavedValues.teamColor, mecanumDrive); boolean reversed = SavedValues.teamColor == AutoShooter.TeamColor.BLUE; @@ -99,35 +98,35 @@ public void initialize() { CommandScheduler.getInstance().schedule( new ParallelCommandGroup( // new LimelightCommands.KalmanFilter(limelightSubsystem, mecanumDrive, dischargeSubsystem::getTurretAngle), - new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, SavedValues.teamColor), + new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, SavedValues.teamColor), new SequentialCommandGroup( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem), + new CommandGroups.Shoot(intake, carousel), new ParallelCommandGroup( new ActionCommand(wheatleyAutoOne.build(), requirements), new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1850), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1850), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ) ), new ParallelRaceGroup( new ActionCommand(wheatleyAutoTwo.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new WaitCommand(100), - new IntakeCommands.ClosedState(intakeSubsystem), + intake.closeState(), new WaitCommand(800), new ParallelCommandGroup( new ActionCommand(wheatleyAutoTwoP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelRaceGroup( new ActionCommand(midAutoP1.build(), requirements), - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem) + CommandGroups.startIntake(intake, carousel) ), new WaitCommand(700), @@ -135,27 +134,27 @@ public void initialize() { new ActionCommand(midAutoP2.build(), requirements), new SequentialCommandGroup( new WaitCommand(300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive)) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive)) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(1300), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(1300), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoThree.build(), requirements) ), new ParallelCommandGroup( new SequentialCommandGroup( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem).withTimeout(3500), - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + CommandGroups.startIntake(intake, carousel).withTimeout(3500), + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) ), new ActionCommand(wheatleyAutoFour.build(), requirements) ), new ParallelCommandGroup( new ActionCommand(prepareGate.build(), requirements), - new IntakeCommands.ClosedState(intakeSubsystem) + intake.closeState() ) @@ -172,12 +171,12 @@ public void initialize_loop() { if (time.seconds() > 1) { int current = limelightSubsystem.getMotif(); SavedValues.startMotif = (current != -1) ? current : SavedValues.startMotif; - double power = -(turretStartAngle - dischargeSubsystem.getTurretAngle()) * 0.018; - if (Math.abs(turretStartAngle - dischargeSubsystem.getTurretAngle()) < 5) { + double power = -(turretStartAngle - dischargeSubsystem.turret.getAngle()) * 0.018; + if (Math.abs(turretStartAngle - dischargeSubsystem.turret.getAngle()) < 5) { power = 0; } power = Range.clip(power, -0.3, 0.3); - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); multipleTelemetry.addData("used", SavedValues.startMotif); multipleTelemetry.addData("current", current); multipleTelemetry.update(); @@ -190,10 +189,10 @@ public void run() { super.run(); multipleTelemetry.addData("x", mecanumDrive.localizer.getPose().position.x); multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); - multipleTelemetry.addData("power",dischargeSubsystem.getFlyWheelPower()); + multipleTelemetry.addData("power",dischargeSubsystem.flywheel.getPower()); multipleTelemetry.update(); SavedValues.position = mecanumDrive.localizer.getPose(); - SavedValues.turretAngle = dischargeSubsystem.getTurretAngle(); + SavedValues.turretAngle = dischargeSubsystem.turret.getAngle(); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/CarouselCommands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/CarouselCommands.java index 3b2516163c8..1528cef10bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/CarouselCommands.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/CarouselCommands.java @@ -8,8 +8,8 @@ import com.seattlesolvers.solverslib.command.SequentialCommandGroup; import com.seattlesolvers.solverslib.command.WaitCommand; import com.seattlesolvers.solverslib.command.WaitUntilCommand; +import com.seattlesolvers.solverslib.controller.PIDController; -import org.firstinspires.ftc.teamcode.decodeOpModes.testers.AutomaticShootingTuner; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -19,64 +19,27 @@ import java.util.HashMap; public class CarouselCommands { + public static final double CAROUSEL_STEP = 0.3; + public static final double CAROUSEL_FULL = 1.0; + public static final double CAROUSEL_HALF = 0.5; + public static final long DEFAULT_TIMEOUT = 500; // millis - public static class MoveToAngle extends CommandBase { - private final CarouselSubsystem carouselSubsystem; - double targetAngle; - double currentAngle; - double kp = 0.018; - int tolerance = 3; - private final boolean end; - public MoveToAngle(CarouselSubsystem carouselSubsystem, double angle) { - this.carouselSubsystem = carouselSubsystem; - this.targetAngle = angle; - end = false; - addRequirements(carouselSubsystem); - } - - public MoveToAngle(CarouselSubsystem carouselSubsystem, double angle, boolean end) { - this.carouselSubsystem = carouselSubsystem; - this.targetAngle = angle; - this.end = end; - addRequirements(carouselSubsystem); - } - - - @Override - public void execute() { - currentAngle = carouselSubsystem.getAngle(); - double error = targetAngle - currentAngle; - double power = kp * error; - carouselSubsystem.setSpinPower(power); - } - - @Override - public boolean isFinished() { - return end && Math.abs(currentAngle - targetAngle) < tolerance; - } + public static class RotateDistance extends CommandBase { + private final double SWITCH_DIRECTION_CURRENT = 7.0; - - @Override - public void end(boolean interrupted) { - carouselSubsystem.setSpinPower(0); - } - } - - - public static class SlideDistance extends CommandBase { - private final CarouselSubsystem carouselSubsystem; + private final CarouselSubsystem carousel; private double targetPos; int moveDirection; double power; double distance; - public SlideDistance(CarouselSubsystem carouselSubsystem, double distance, double power) {//distance in thirds - this.carouselSubsystem = carouselSubsystem; + public RotateDistance(CarouselSubsystem carousel, double distance, double power) { //distance in thirds + this.carousel = carousel; this.power = power; this.distance = distance; - addRequirements(carouselSubsystem); + addRequirements(carousel); } @@ -87,30 +50,29 @@ public void initialize() { power *= -1; } - targetPos = (carouselSubsystem.getPosition() + distance * carouselSubsystem.spinConversion); + targetPos = (carousel.getPosition() + distance * carousel.spinConversion); } @Override public void execute() { - if (carouselSubsystem.getCurrent() > 7) { - carouselSubsystem.setSpinPower(-power); + if (carousel.getCurrent() > SWITCH_DIRECTION_CURRENT) { + carousel.setSpinPower(-power); } else { - carouselSubsystem.setSpinPower(power); + carousel.setSpinPower(power); } } @Override public boolean isFinished() { - return carouselSubsystem.getPosition() * moveDirection > targetPos * moveDirection; + return carousel.getPosition() * moveDirection > targetPos * moveDirection; } @Override public void end(boolean interrupted) { - carouselSubsystem.setSpinPower(0); + carousel.setSpinPower(0); } } - public static class SmartDischarge extends SelectCommand { private static final double transferSpeed = 0.7, travelSpeed = 1; @@ -122,73 +84,68 @@ enum Sequence { NONE } - public SmartDischarge(CarouselSubsystem carouselSubsystem, IntakeSubsystem intakeSubsystem) { + public SmartDischarge(CarouselSubsystem carousel, IntakeSubsystem intake) { super(new HashMap() {{ put(Sequence.ONE, new SequentialCommandGroup( - new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(500), - new IntakeCommands.TransferState(intakeSubsystem), - new SlideDistance(carouselSubsystem, -0.3, transferSpeed), + new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(DEFAULT_TIMEOUT), + intake.transfer(), + new RotateDistance(carousel, -CAROUSEL_STEP, transferSpeed), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed), - new SlideDistance(carouselSubsystem, -0.6, transferSpeed), + new RotateDistance(carousel, -(CAROUSEL_STEP * 2), transferSpeed), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atSpeed), new WaitCommand(200), - new SlideDistance(carouselSubsystem, -1, transferSpeed) + new RotateDistance(carousel, -CAROUSEL_FULL, transferSpeed) )); put(Sequence.TWO, new SequentialCommandGroup( new ParallelCommandGroup( - new SlideDistance(carouselSubsystem, 1.6, travelSpeed), - new IntakeCommands.TransferState(intakeSubsystem) + new RotateDistance(carousel, CAROUSEL_STEP * 2 + CAROUSEL_FULL, travelSpeed), + intake.transfer() ), - new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(500), - new SlideDistance(carouselSubsystem, 0.3, transferSpeed), + new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(DEFAULT_TIMEOUT), + new RotateDistance(carousel, CAROUSEL_STEP, transferSpeed), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed), - new SlideDistance(carouselSubsystem, 0.6, transferSpeed), + new RotateDistance(carousel, CAROUSEL_STEP * 2, transferSpeed), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atSpeed), - new SlideDistance(carouselSubsystem, 1, transferSpeed) + new RotateDistance(carousel, CAROUSEL_FULL, transferSpeed) )); put(Sequence.THREE, new SequentialCommandGroup( - new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(500), - new IntakeCommands.TransferState(intakeSubsystem), - new SlideDistance(carouselSubsystem, -0.3, transferSpeed), - new SlideDistance(carouselSubsystem, 2, travelSpeed), + new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(DEFAULT_TIMEOUT), + intake.transfer(), + new RotateDistance(carousel, -CAROUSEL_STEP, transferSpeed), + new RotateDistance(carousel, CAROUSEL_FULL * 2, travelSpeed), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed), - new SlideDistance(carouselSubsystem, 0.5, transferSpeed), -// new SlideDistance(carouselSubsystem,-2,travelSpeed), + new RotateDistance(carousel, CAROUSEL_HALF, transferSpeed), +// new SlideDistance(carousel,-2,travelSpeed), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atSpeed), new WaitCommand(200), - new SlideDistance(carouselSubsystem, 1, transferSpeed) + new RotateDistance(carousel, CAROUSEL_FULL, transferSpeed) )); put(Sequence.FOUR, new SequentialCommandGroup( new ParallelCommandGroup( - new SlideDistance(carouselSubsystem, 1.2, travelSpeed), - new IntakeCommands.TransferState(intakeSubsystem) + new RotateDistance(carousel, 1.2, travelSpeed), + intake.transfer() ), - new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(500), -// new SlideDistance(carouselSubsystem, 0.3, transferSpeed), - new SlideDistance(carouselSubsystem, -1.8, travelSpeed), + new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(DEFAULT_TIMEOUT), + new RotateDistance(carousel, -1.8, travelSpeed), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed), - new SlideDistance(carouselSubsystem, -0.6, transferSpeed) -// new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atSpeed), -// new WaitCommand(200), -// new SlideDistance(carouselSubsystem, -1, transferSpeed) + new RotateDistance(carousel, -CAROUSEL_STEP * 2, transferSpeed) )); put(Sequence.NONE, new SequentialCommandGroup( - - new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(500), - new IntakeCommands.TransferState(intakeSubsystem), - new SlideDistance(carouselSubsystem, -0.3, transferSpeed), + new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed).withTimeout(DEFAULT_TIMEOUT), + intake.transfer(), + new RotateDistance(carousel, -CAROUSEL_STEP, transferSpeed), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed), - new SlideDistance(carouselSubsystem, -0.6, transferSpeed), + new RotateDistance(carousel, -CAROUSEL_STEP * 2, transferSpeed), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed), - new SlideDistance(carouselSubsystem, -1, transferSpeed) + new RotateDistance(carousel, -CAROUSEL_FULL, transferSpeed) )); - }}, () -> getSequence(carouselSubsystem)); + }}, () -> getSequence(carousel)); } - private static Sequence getSequence(CarouselSubsystem carouselSubsystem) { + private static Sequence getSequence(CarouselSubsystem carousel) { int wanted = (3 - SavedValues.currentCount % 3 + SavedValues.startMotif) % 3; - int current = carouselSubsystem.getGreenPlacement(); + int current = carousel.getGreenPlacement(); // return Sequence.FOUR; if ((current == 1 && wanted == 0) || current == -1) { return Sequence.NONE; @@ -214,23 +171,23 @@ enum Sequence { } - public Discharge(CarouselSubsystem carouselSubsystem) { + public Discharge(CarouselSubsystem carousel) { super(new HashMap() {{ put(Sequence.CLOSE, new SequentialCommandGroup( new InstantCommand(() -> DischargeSubsystem.shooting = false), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atSpeed), - new SlideDistance(carouselSubsystem, 2.2, 1) + new RotateDistance(carousel, 2.2, 1) )); put(Sequence.MIDDLE, new SequentialCommandGroup( - new SlideDistance(carouselSubsystem, 2, 0.95), + new RotateDistance(carousel, 2, 0.95), new WaitCommand(80), - new SlideDistance(carouselSubsystem, 1.2, 0.65))); + new RotateDistance(carousel, 1.2, 0.65))); put(Sequence.FAR, new SequentialCommandGroup( new InstantCommand(() -> DischargeSubsystem.shooting = false), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.atCloseSpeed), new InstantCommand(() -> DischargeSubsystem.shooting = true), - new SlideDistance(carouselSubsystem, 2.2, 1), + new RotateDistance(carousel, 2.2, 1), new InstantCommand(() -> DischargeSubsystem.shooting = false))); @@ -243,6 +200,4 @@ public Discharge(CarouselSubsystem carouselSubsystem) { }); } } - - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/CommandGroups.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/CommandGroups.java index 19c5375c6ec..0fedafe039a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/CommandGroups.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/CommandGroups.java @@ -3,6 +3,7 @@ import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; import com.qualcomm.robotcore.util.Range; +import com.seattlesolvers.solverslib.command.Command; import com.seattlesolvers.solverslib.command.CommandBase; import com.seattlesolvers.solverslib.command.InstantCommand; import com.seattlesolvers.solverslib.command.ParallelCommandGroup; @@ -19,17 +20,16 @@ import java.util.function.Supplier; public class CommandGroups { - public static class Shoot extends ParallelRaceGroup { - - public Shoot(IntakeSubsystem intakeSubsystem, CarouselSubsystem carouselSubsystem) { - addCommands(new SequentialCommandGroup( + public static class Shoot extends SequentialCommandGroup { + public Shoot(IntakeSubsystem intake, CarouselSubsystem carousel) { + addCommands( new InstantCommand(() -> DischargeCommands.AutomaticAiming.shooting = true), - new IntakeCommands.TransferState(intakeSubsystem), + intake.transfer(), // new WaitCommand(100), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.inRange), - new CarouselCommands.Discharge(carouselSubsystem), + new CarouselCommands.Discharge(carousel), new InstantCommand(() -> DischargeCommands.AutomaticAiming.shooting = false) - )); + ); } @Override @@ -42,11 +42,11 @@ public void end(boolean interrupted) { } public static class PrepareShooting extends SequentialCommandGroup { - public PrepareShooting(IntakeSubsystem intakeSubsystem, CarouselSubsystem carouselSubsystem, MecanumDrive mecanumDrive) { + public PrepareShooting(IntakeSubsystem intake, CarouselSubsystem carousel, MecanumDrive mecanumDrive) { addCommands( new InstantCommand(() -> DischargeCommands.AutomaticAiming.shooting = true), - new IntakeCommands.TransferState(intakeSubsystem), + intake.transfer(), new WaitUntilCommand(() -> { PoseVelocity2d movement = mecanumDrive.localizer.update(); Pose2d pos = mecanumDrive.localizer.getPose(); @@ -55,7 +55,7 @@ public PrepareShooting(IntakeSubsystem intakeSubsystem, CarouselSubsystem carous return AutoShooter.canLaunch(new Pose2d(pos.position.x + movementEffect.getX(), pos.position.y + movementEffect.getY(), pos.heading.toDouble())); }), new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.inRange), - new CarouselCommands.Discharge(carouselSubsystem), + new CarouselCommands.Discharge(carousel), new InstantCommand(() -> DischargeCommands.AutomaticAiming.shooting = false) ); @@ -70,22 +70,18 @@ public void end(boolean interrupted) { } } - public static class StartIntake extends ParallelCommandGroup { - public StartIntake(IntakeSubsystem intakeSubsystem, CarouselSubsystem carouselSubsystem) { - addCommands( - new IntakeCommands.IntakeState(intakeSubsystem), - new CarouselCommands.MoveToAngle(carouselSubsystem, 180) - ); - } + public static Command startIntake(IntakeSubsystem intake, CarouselSubsystem carousel) { + return new ParallelCommandGroup( + intake.intake(), + carousel.rotateToAngle(180) + ); } - public static class StartOuttake extends ParallelCommandGroup { - public StartOuttake(IntakeSubsystem intakeSubsystem, CarouselSubsystem carouselSubsystem) { - addCommands( - new IntakeCommands.OutTakeState(intakeSubsystem), - new CarouselCommands.MoveToAngle(carouselSubsystem, 195) - ); - } + public static Command startOuttake(IntakeSubsystem intake, CarouselSubsystem carousel) { + return new ParallelCommandGroup( + intake.outtake(), + carousel.rotateToAngle(195) + ); } @@ -121,12 +117,12 @@ public void execute() { double rPower = Range.clip(power - delta, 0.1, power); double lPower = Range.clip(power + delta, 0.1, power); - if(left > 5000 && right > 5000){ + if (left > 5000 && right > 5000) { mecanumDrive.rightFront.setPower(0.1); mecanumDrive.rightBack.setPower(-0.1); mecanumDrive.leftFront.setPower(0.1); mecanumDrive.leftBack.setPower(-0.1); - }else{ + } else { mecanumDrive.rightFront.setPower(rPower); mecanumDrive.rightBack.setPower(-rPower); @@ -137,20 +133,15 @@ public void execute() { } } - public static class SortedShooting extends SequentialCommandGroup { - public SortedShooting(IntakeSubsystem intakeSubsystem, CarouselSubsystem carouselSubsystem) { - addCommands( - new InstantCommand(() -> DischargeCommands.AutomaticAiming.shooting = true), - new IntakeCommands.SortingState(intakeSubsystem), - new CarouselCommands.SlideDistance(carouselSubsystem, -0.56, 0.6).withTimeout(1000).whenFinished(() -> carouselSubsystem.setSpinPower(0)), + public static Command sortedShooting(IntakeSubsystem intake, CarouselSubsystem carousel) { + return new SequentialCommandGroup( + new InstantCommand(() -> DischargeCommands.AutomaticAiming.shooting = true), + intake.sortState(), + new CarouselCommands.RotateDistance(carousel, -0.56, 0.6).withTimeout(1000).whenFinished(() -> carousel.setSpinPower(0)), // new WaitCommand(200), - new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.inRange), - new CarouselCommands.SmartDischarge(carouselSubsystem, intakeSubsystem), - new InstantCommand(() -> DischargeCommands.AutomaticAiming.shooting = false) - - ); - } + new WaitUntilCommand(() -> DischargeCommands.AutomaticAiming.inRange), + new CarouselCommands.SmartDischarge(carousel, intake), + new InstantCommand(() -> DischargeCommands.AutomaticAiming.shooting = false) + ); } - - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/DischargeCommands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/DischargeCommands.java index b073970ba69..5b2fed62522 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/DischargeCommands.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/DischargeCommands.java @@ -7,6 +7,8 @@ import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; import com.seattlesolvers.solverslib.command.CommandBase; +import com.seattlesolvers.solverslib.controller.PIDController; +import com.seattlesolvers.solverslib.controller.wpilibcontroller.SimpleMotorFeedforward; import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; @@ -17,7 +19,6 @@ @Config public class DischargeCommands { - public static class setState extends CommandBase { DischargeSubsystem dischargeSubsystem; double flyWheelRPM; @@ -37,14 +38,14 @@ public setState(DischargeSubsystem dischargeSubsystem, double flyWheelRPM, doubl @Override public void initialize() { canShoot = false; - dischargeSubsystem.setRampDegree(rampDegree); + dischargeSubsystem.ramp.setRampDegree(rampDegree); } @Override public void execute() { - dischargeSubsystem.setFlyWheelRPM(flyWheelRPM); - dischargeSubsystem.setTurretPower((turretAngle - dischargeSubsystem.getTurretAngle()) * kp); - canShoot = Math.abs(dischargeSubsystem.getRPM() - flyWheelRPM) < 300; + dischargeSubsystem.flywheel.runRPM(flyWheelRPM); + dischargeSubsystem.turret.runToAngleDirect(turretAngle); + canShoot = Math.abs(dischargeSubsystem.flywheel.getRPM() - flyWheelRPM) < 300; } } @@ -53,7 +54,7 @@ public void execute() { public static class AutomaticAiming extends CommandBase { DischargeSubsystem dischargeSubsystem; LimelightSubsystem limelightSubsystem; - CarouselSubsystem carouselSubsystem; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; AutoShooter.TeamColor teamColor; public static double kp = 0.022;//0.018 @@ -63,7 +64,8 @@ public static class AutomaticAiming extends CommandBase { public static boolean shooting = false; public static boolean atSpeed = false;//+- 200rpm public static boolean atCloseSpeed = false;//+- 70rpm - public static double kv = 0.14, ks = 0.06, kd = -0.45, ka = 0.08; + public static SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.06, 0.14, 0.08); + public static double kd = -0.45; private com.seattlesolvers.solverslib.geometry.Vector2d mecanumToTurret; public static boolean aim = true; public static boolean inRange = true; @@ -78,10 +80,10 @@ public static class AutomaticAiming extends CommandBase { double currentTime, lastTime, deltaTime; double integral = 0; - public AutomaticAiming(DischargeSubsystem dischargeSubsystem, LimelightSubsystem limelightSubsystem, MecanumDrive mecanumDrive, CarouselSubsystem carouselSubsystem, AutoShooter.TeamColor teamColor) { + public AutomaticAiming(DischargeSubsystem dischargeSubsystem, LimelightSubsystem limelightSubsystem, MecanumDrive mecanumDrive, CarouselSubsystem carousel, AutoShooter.TeamColor teamColor) { this.dischargeSubsystem = dischargeSubsystem; this.limelightSubsystem = limelightSubsystem; - this.carouselSubsystem = carouselSubsystem; + this.carousel = carousel; this.mecanumDrive = mecanumDrive; this.teamColor = teamColor; addRequirements(dischargeSubsystem); @@ -107,25 +109,24 @@ public void execute() { movement = mecanumDrive.localizer.update(); - mecanumToTurret = new com.seattlesolvers.solverslib.geometry.Vector2d(1.5748, 0).rotateBy(mecanumDrive.localizer.getPose().heading.toDouble() / Math.PI * 180); currentPos = mecanumDrive.localizer.getPose(); double time = AutoShooter.getTime(mecanumDrive.localizer.getPose()) * kMovement; movementEffect = new com.seattlesolvers.solverslib.geometry.Vector2d(movement.linearVel.x * time, movement.linearVel.y * time) .rotateBy(mecanumDrive.localizer.getPose().heading.toDouble() / Math.PI * 180); - if(movementEffect.magnitude() > effectSpeed){ + if (movementEffect.magnitude() > effectSpeed) { movementEffect = movementEffect.times(effectSpeed / movementEffect.magnitude()); } - acceleration = movementEffect.div(time).minus((lastMovement != null) ? lastMovement : movementEffect).times(time).div(deltaTime).times(ka); + acceleration = movementEffect.div(time).minus((lastMovement != null) ? lastMovement : movementEffect).times(time).div(deltaTime).times(ff.ka); aimTurret(); if (AutoShooter.canLaunch(new Pose2d(new Vector2d(currentPos.position.x + mecanumToTurret.getX(), currentPos.position.y + mecanumToTurret.getY()), currentPos.heading.toDouble()))) { double[] launchVector = AutoShooter.getLaunchVector(new Pose2d(new Vector2d(currentPos.position.x + mecanumToTurret.getX() + movementEffect.getX(), currentPos.position.y + mecanumToTurret.getY() + movementEffect.getY()), currentPos.heading.toDouble()), teamColor); - dischargeSubsystem.setRampDegree(launchVector[0]); - dischargeSubsystem.setFlyWheelRPM(launchVector[1] + rpmCorrection); - double delta = Math.abs(dischargeSubsystem.getRPM() - (launchVector[1] + rpmCorrection)); + dischargeSubsystem.ramp.setRampDegree(launchVector[0]); + dischargeSubsystem.flywheel.runRPM(launchVector[1] + rpmCorrection); + double delta = Math.abs(dischargeSubsystem.flywheel.getRPM() - (launchVector[1] + rpmCorrection)); atSpeed = delta < 120; atCloseSpeed = delta < 70; } else if (shooting) { @@ -134,9 +135,9 @@ public void execute() { currentPos.position.y + mecanumToTurret.getY() + movementEffect.getY() / time); double[] launchVector = AutoShooter.getLaunchVector(new Pose2d(new Vector2d(closest.getX(), closest.getY()), currentPos.heading.toDouble()), teamColor); - dischargeSubsystem.setRampDegree(launchVector[0]); - dischargeSubsystem.setFlyWheelRPM(launchVector[1] + rpmCorrection); - double delta = Math.abs(dischargeSubsystem.getRPM() - (launchVector[1] + rpmCorrection)); + dischargeSubsystem.ramp.setRampDegree(launchVector[0]); + dischargeSubsystem.flywheel.runRPM(launchVector[1] + rpmCorrection); + double delta = Math.abs(dischargeSubsystem.flywheel.getRPM() - (launchVector[1] + rpmCorrection)); atSpeed = delta < 120; atCloseSpeed = delta < 70; } else { @@ -145,17 +146,17 @@ public void execute() { currentPos.position.y + mecanumToTurret.getY() + movementEffect.getY()); double[] launchVector = AutoShooter.getLaunchVector(new Pose2d(new Vector2d(closest.getX(), closest.getY()), currentPos.heading.toDouble()), teamColor); - dischargeSubsystem.setRampDegree(launchVector[0]); - dischargeSubsystem.stayRPM(launchVector[1] + rpmCorrection); + dischargeSubsystem.ramp.setRampDegree(launchVector[0]); + dischargeSubsystem.flywheel.keepRPM(launchVector[1] + rpmCorrection); atSpeed = false; } lastPos = currentPos; lastMovement = movementEffect.div(time); } else { inRange = true; - dischargeSubsystem.setRampDegree(41); - dischargeSubsystem.setFlyWheelRPM(3000 + rpmCorrection); - dischargeSubsystem.setTurretPower(0); + dischargeSubsystem.ramp.setRampDegree(41); + dischargeSubsystem.flywheel.runRPM(3000 + rpmCorrection); + dischargeSubsystem.turret.setPower(0); atSpeed = true; atCloseSpeed = true; } @@ -164,14 +165,13 @@ public void execute() { private void aimTurret() { double angleError = limelightSubsystem.currentTx; - double turretAngle = dischargeSubsystem.getTurretAngle(); + double turretAngle = dischargeSubsystem.turret.getAngle(); com.seattlesolvers.solverslib.geometry.Vector2d closest = AutoShooter.closestPoint( currentPos.position.x + mecanumToTurret.getX() + movementEffect.getX(), currentPos.position.y + mecanumToTurret.getY() + movementEffect.getY()); double power; if (angleError != 0 && inRange && !(Math.abs(currentPos.position.y) > 40) && limelight) { - - double rps = dischargeSubsystem.getRPS(); + double turretAngularVelocity = dischargeSubsystem.turret.getAngularVelocity(); double aprilTagAngle = AutoShooter.getAprilTagAngle(new Pose2d(currentPos.position.x + mecanumToTurret.getX(), currentPos.position.y + mecanumToTurret.getY(), currentPos.heading.toDouble() - Math.PI), teamColor); @@ -181,13 +181,13 @@ private void aimTurret() { currentPos.heading.toDouble() - Math.PI), teamColor) - aprilTagAngle; integral += (wantedError - angleError) * (currentTime - lastTime); power = -(wantedError - angleError) * kp; - if(Math.signum(power) == Math.signum(integral)){ + if (Math.signum(power) == Math.signum(integral)) { integral = 0; } - power += rps * kd; + power += turretAngularVelocity * kd; power += integral * ki; - power = (ks > Math.abs(power)) ? Math.signum(power) * ks : power; + power = (ff.ks > Math.abs(power)) ? Math.signum(power) * ff.ks : power; } else if (lastAngleError != 0 && angleError == 0) { integral = 0; @@ -195,7 +195,7 @@ private void aimTurret() { llTime = time.seconds(); power = 0; } else if (time.seconds() - llTime > 0.1) { - double rps = dischargeSubsystem.getRPS(); + double turretAngularVelocity = dischargeSubsystem.turret.getAngularVelocity(); launchAngle = (AutoShooter.canLaunch(currentPos)) ? (AutoShooter.getLaunchAngle(new Pose2d(currentPos.position.x + mecanumToTurret.getX() + movementEffect.getX(), currentPos.position.y + mecanumToTurret.getY() + movementEffect.getY(), @@ -207,8 +207,8 @@ private void aimTurret() { launchAngle = Range.clip(launchAngle, 20, 340); integral = 0; power = -(launchAngle - turretAngle) * kp; - power += rps * kd; - power += Math.signum(power) * ks; + power += turretAngularVelocity * kd; + power += Math.signum(power) * ff.ks; } else { power = 0; integral = 0; @@ -216,17 +216,17 @@ private void aimTurret() { } lastAngleError = angleError; lastTurretAngle = turretAngle; - power += movement.angVel * kv; + power += movement.angVel * ff.kv; - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); } @Override public void end(boolean interrupted) { - dischargeSubsystem.setFlyWheelPower(0); - dischargeSubsystem.setTurretPower(0); + dischargeSubsystem.flywheel.setPower(0); + dischargeSubsystem.turret.stopPower(); } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/IntakeCommands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/IntakeCommands.java deleted file mode 100644 index 61cda159fdb..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeCommands/IntakeCommands.java +++ /dev/null @@ -1,137 +0,0 @@ -package org.firstinspires.ftc.teamcode.decodeCommands; - -import com.qualcomm.robotcore.util.ElapsedTime; -import com.seattlesolvers.solverslib.command.CommandBase; - -import org.firstinspires.ftc.teamcode.decodeSubsystems.IntakeSubsystem; - - -public class IntakeCommands { - - - public static class IntakeState extends CommandBase { - IntakeSubsystem intakeSubsystem; - ElapsedTime time; - double threeTime; - - - public static boolean resetCount; - - public IntakeState(IntakeSubsystem intakeSubsystem) { - this.intakeSubsystem = intakeSubsystem; - time = new ElapsedTime(); - addRequirements(intakeSubsystem); - } - - @Override - public void initialize() { - time.reset(); - IntakeSubsystem.count = 0; -// intakeSubsystem.intaking = true; - resetCount = false; - threeTime = 0; - intakeSubsystem.setPower(1); - intakeSubsystem.setPosition(1); - } - - - @Override - public boolean isFinished() { - return IntakeSubsystem.count >= 3; - } - - @Override - public void end(boolean interrupted) { -// intakeSubsystem.intaking = false; - intakeSubsystem.setPower(0); - } - } - - public static class TransferState extends CommandBase { - IntakeSubsystem intakeSubsystem; - - public TransferState(IntakeSubsystem intakeSubsystem) { - this.intakeSubsystem = intakeSubsystem; - addRequirements(intakeSubsystem); - } - - @Override - public void initialize() { - intakeSubsystem.setPower(1); - intakeSubsystem.setPosition(0); - } - - @Override - public boolean isFinished() { - return true; - } - } - - - public static class ClosedState extends CommandBase { - IntakeSubsystem intakeSubsystem; - - - public ClosedState(IntakeSubsystem intakeSubsystem) { - this.intakeSubsystem = intakeSubsystem; - addRequirements(intakeSubsystem); - } - - @Override - public void initialize() { - intakeSubsystem.setPower(0); - intakeSubsystem.setPosition(1); - } - - @Override - public boolean isFinished() { - return true; - } - - } - - public static class OutTakeState extends CommandBase { - IntakeSubsystem intakeSubsystem; - - - public OutTakeState(IntakeSubsystem intakeSubsystem) { - this.intakeSubsystem = intakeSubsystem; - addRequirements(intakeSubsystem); - } - - @Override - public void initialize() { - intakeSubsystem.setPower(-0.7); - intakeSubsystem.setPosition(1); - } - - @Override - public boolean isFinished() { - return true; - } - - } - - - public static class SortingState extends CommandBase { - IntakeSubsystem intakeSubsystem; - - public SortingState(IntakeSubsystem intakeSubsystem) { - this.intakeSubsystem = intakeSubsystem; - addRequirements(intakeSubsystem); - } - - @Override - public void initialize() { - intakeSubsystem.setPower(0); - intakeSubsystem.setPosition(0); - } - - @Override - public boolean isFinished() { - return true; - } - - } - -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/WheatleyOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/WheatleyOpMode.java index f01ded2be94..403a6cdcebc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/WheatleyOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/WheatleyOpMode.java @@ -14,7 +14,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; import org.firstinspires.ftc.teamcode.decodeCommands.DischargeCommands; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -26,8 +25,8 @@ @TeleOp public class WheatleyOpMode extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; GamepadEx driver, system; @@ -57,9 +56,9 @@ public void initialize() { time = new ElapsedTime(); teamColor = SavedValues.teamColor; - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); - carouselSubsystem = new CarouselSubsystem(hardwareMap); + carousel = new CarouselSubsystem(hardwareMap); mecanumDrive = new MecanumDrive(hardwareMap, SavedValues.position); mecanumDrive.driveMode(); @@ -71,27 +70,27 @@ public void initialize() { initButtons(); mecanumDrive.setDefaultCommand(new MecanumCommands.Drive(mecanumDrive, () -> driver.getLeftY(), () -> driver.getLeftX(), () -> driver.getRightX() * 1.25, () -> 1 - 0.5 * gamepad1.right_trigger, teamColor)); - dischargeSubsystem.setDefaultCommand(new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carouselSubsystem, teamColor)); - driverA.whenPressed(new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem)); + dischargeSubsystem.setDefaultCommand(new DischargeCommands.AutomaticAiming(dischargeSubsystem, limelightSubsystem, mecanumDrive, carousel, teamColor)); + driverA.whenPressed(CommandGroups.startIntake(intake, carousel)); driverB.whenPressed( - new CommandGroups.PrepareShooting(intakeSubsystem, carouselSubsystem, mecanumDrive) + new CommandGroups.PrepareShooting(intake, carousel, mecanumDrive) .whenFinished(() -> CommandScheduler.getInstance().schedule( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem))) + CommandGroups.startIntake(intake, carousel))) ); driverX.whenPressed( - new CommandGroups.Shoot(intakeSubsystem, carouselSubsystem) + new CommandGroups.Shoot(intake, carousel) .whenFinished(() -> CommandScheduler.getInstance().schedule( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem))) + CommandGroups.startIntake(intake, carousel))) ); driverDPadLeft.whenPressed(() -> DischargeCommands.AutomaticAiming.aim = !DischargeCommands.AutomaticAiming.aim); // driverLeftBumper.whenPressed(() -> IntakeCommands.IntakeState.resetCount = !IntakeCommands.IntakeState.resetCount); - driverY.whenPressed(new CommandGroups.StartOuttake(intakeSubsystem, carouselSubsystem)); - driverLeftBumper.whenPressed(new CommandGroups.SortedShooting(intakeSubsystem, carouselSubsystem) + driverY.whenPressed(CommandGroups.startOuttake(intake, carousel)); + driverLeftBumper.whenPressed(CommandGroups.sortedShooting(intake, carousel) .whenFinished(() -> CommandScheduler.getInstance().schedule( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem))) + CommandGroups.startIntake(intake, carousel))) ); - driverRightBumper.whenPressed(new IntakeCommands.ClosedState(intakeSubsystem)); + driverRightBumper.whenPressed(intake.closeState()); mecanumDrive.lazyImu.get().resetYaw(); systemDPadLeft.whenPressed(() -> DischargeCommands.AutomaticAiming.turretCorrection += 2); systemDPadRight.whenPressed(() -> DischargeCommands.AutomaticAiming.turretCorrection -= 2); @@ -112,7 +111,7 @@ public void initialize() { schedule(new DischargeCommands.setState(dischargeSubsystem,0,60, 180));})); driverLeftTrigger.whileActiveOnce(new MecanumCommands.Aim(mecanumDrive, () -> driver.getLeftY(), () -> driver.getLeftX(),() -> 1 - 0.5 * gamepad1.right_trigger,-120 * Math.PI/180,2, teamColor)); driverRightStick.whileActiveOnce(new MecanumCommands.Aim(mecanumDrive, () -> driver.getLeftY(), () -> driver.getLeftX(),() -> 1 - 0.5 * gamepad1.right_trigger,Math.PI/2,1, teamColor) - .beforeStarting(new IntakeCommands.ClosedState(intakeSubsystem))); + .beforeStarting(intake.closeState())); systemX.whenPressed(() -> mecanumDrive.localizer.setPose(new Pose2d(63, 0, Math.PI))); } @@ -148,17 +147,17 @@ public void run() { multipleTelemetry.addData("------------Carousel------------",0); multipleTelemetry.addData("carouselCount", IntakeSubsystem.count); -// multipleTelemetry.addData("rightSwitchState", intakeSubsystem.rightSwitchState()); -// multipleTelemetry.addData("leftSwitchState", intakeSubsystem.leftSwitchState()); +// multipleTelemetry.addData("rightSwitchState", intake.rightSwitchState()); +// multipleTelemetry.addData("leftSwitchState", intake.leftSwitchState()); // multipleTelemetry.addData("SwitchLoopCount", IntakeSubsystem.switchLoopCount); - multipleTelemetry.addData("carouselAngle", carouselSubsystem.getAngle()); - multipleTelemetry.addData("carouselPosition", carouselSubsystem.getPosition()); + multipleTelemetry.addData("carouselAngle", carousel.getAngle()); + multipleTelemetry.addData("carouselPosition", carousel.getPosition()); multipleTelemetry.addData("----------------Discharge-------------",0); - multipleTelemetry.addData("turretAngle", dischargeSubsystem.getTurretAngle()); - multipleTelemetry.addData("turret Ticks", dischargeSubsystem.getTurretPosition()); - multipleTelemetry.addData("rpm", dischargeSubsystem.getRPM()); - multipleTelemetry.addData("flyWheelPower", dischargeSubsystem.flyWheelMotor.motorEx.getPower()); + multipleTelemetry.addData("turretAngle", dischargeSubsystem.turret.getAngle()); + multipleTelemetry.addData("turret Ticks", dischargeSubsystem.turret.getPosition()); + multipleTelemetry.addData("rpm", dischargeSubsystem.flywheel.getRPM()); + multipleTelemetry.addData("flyWheelPower", dischargeSubsystem.flywheel.getPower()); multipleTelemetry.addData("correction", DischargeCommands.AutomaticAiming.turretCorrection); multipleTelemetry.update(); @@ -168,7 +167,7 @@ public void run() { @Override public void end() { limelightSubsystem.stopLimelight(); - intakeSubsystem.stopSensorThread(); + intake.stopSensorThread(); } public void initButtons() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/reset.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/reset.java index 2701ea55aea..3ebca243d3e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/reset.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/reset.java @@ -12,15 +12,15 @@ @Autonomous public class reset extends LinearOpMode { DischargeSubsystem dischargeSubsystem; - CarouselSubsystem carouselSubsystem; + CarouselSubsystem carousel; @Override public void runOpMode() throws InterruptedException { dischargeSubsystem = new DischargeSubsystem(hardwareMap); - carouselSubsystem = new CarouselSubsystem(hardwareMap); + carousel = new CarouselSubsystem(hardwareMap); waitForStart(); dischargeSubsystem.resetTurret(); - carouselSubsystem.resetEncoders(); + carousel.resetEncoders(); SavedValues.position = new Pose2d(63, 0, Math.PI); SavedValues.turretAngle = 180; SavedValues.teamColor = AutoShooter.TeamColor.BLUE; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/AccelerationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/AccelerationTest.java index 7902eb506a5..5a6e34ebdd3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/AccelerationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/AccelerationTest.java @@ -43,12 +43,12 @@ public void initialize_loop() { @Override public void run() { - if (!reached && Math.abs(dischargeSubsystem.getRPM() - wantedRpm) < 100) { + if (!reached && Math.abs(dischargeSubsystem.flywheel.getRPM() - wantedRpm) < 100) { timeTaken = time.seconds(); reached = true; } multipleTelemetry.addData("time taken", timeTaken); - dischargeSubsystem.setFlyWheelRPM(wantedRpm); + dischargeSubsystem.flywheel.runRPM(wantedRpm); multipleTelemetry.update(); super.run(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/AutomaticShootingTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/AutomaticShootingTuner.java index 513e9760a47..86c13feb582 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/AutomaticShootingTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/AutomaticShootingTuner.java @@ -17,7 +17,6 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.AutoShooter; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -29,8 +28,8 @@ @TeleOp(group = "tests") public class AutomaticShootingTuner extends ActionOpMode { DischargeSubsystem dischargeSubsystem; - IntakeSubsystem intakeSubsystem; - CarouselSubsystem carouselSubsystem; + IntakeSubsystem intake; + CarouselSubsystem carousel; MecanumDrive mecanumDrive; LimelightSubsystem limelightSubsystem; GamepadEx driver, system; @@ -56,10 +55,10 @@ public class AutomaticShootingTuner extends ActionOpMode { public void initialize() { SavedValues.teamColor = AutoShooter.TeamColor.BLUE; - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); dischargeSubsystem = new DischargeSubsystem(hardwareMap); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(new Vector2d(63, 0), Math.PI)); @@ -69,11 +68,11 @@ public void initialize() { system = new GamepadEx(gamepad2); initButtons(); mecanumDrive.setDefaultCommand(new MecanumCommands.Drive(mecanumDrive, () -> driver.getLeftY(), () -> driver.getLeftX(), () -> driver.getRightX())); - driverA.whenPressed(new CommandGroups.StartIntake(intakeSubsystem,carouselSubsystem)); - driverB.whenPressed(new IntakeCommands.ClosedState(intakeSubsystem)); - driverY.whenPressed(new IntakeCommands.OutTakeState(intakeSubsystem)); - driverX.whenPressed(new CommandGroups.Shoot(intakeSubsystem,carouselSubsystem) .whenFinished(() -> CommandScheduler.getInstance().schedule( - new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem)))); + driverA.whenPressed(CommandGroups.startIntake(intake,carousel)); + driverB.whenPressed(intake.closeState()); + driverY.whenPressed(intake.outtake()); + driverX.whenPressed(new CommandGroups.Shoot(intake,carousel) .whenFinished(() -> CommandScheduler.getInstance().schedule( + CommandGroups.startIntake(intake, carousel)))); systemDPadUp.whenPressed(() -> wantedRPM += 50); systemDPadDown.whenPressed(() -> wantedRPM -= 50); systemA.whenPressed(() -> wantedDegree += 2); @@ -81,13 +80,13 @@ public void initialize() { systemX.whenPressed(() -> wantedDegree += 0.2); systemB.whenPressed(() -> wantedDegree -= 0.2); - driverY.whenPressed(new IntakeCommands.OutTakeState(intakeSubsystem)); + driverY.whenPressed(intake.outtake()); driverDPadDown.whenPressed(new SequentialCommandGroup( new InstantCommand(() -> work = false), new WaitCommand(30000), new InstantCommand(() -> work = true) )); - driverDPadUp.whenPressed(new IntakeCommands.ClosedState(intakeSubsystem)); + driverDPadUp.whenPressed(intake.closeState()); systemDPadLeft.whenPressed(() -> correction +=2); systemDPadRight.whenPressed(() -> correction -=2); // limelightSubsystem.startLimelight(); @@ -96,29 +95,29 @@ public void initialize() { @Override public void run() { aimTurret(); - atSpeed = Math.abs(dischargeSubsystem.getRPM() - wantedRPM) < 70; + atSpeed = Math.abs(dischargeSubsystem.flywheel.getRPM() - wantedRPM) < 70; mecanumDrive.localizer.update(); double launchAngle = (AutoShooter.getLaunchAngle(new Pose2d(mecanumDrive.localizer.getPose().position,mecanumDrive.localizer.getPose().heading.toDouble() - Math.PI), AutoShooter.TeamColor.BLUE) + 360) % 360; -// double power = -((launchAngle + 360) % 360 - dischargeSubsystem.getTurretAngle()); +// double power = -((launchAngle + 360) % 360 - dischargeSubsystem.turret.getAngle()); // power += Math.signum(power) * 0.03; // power = Range.clip(power, -0.4, 0.4); -// dischargeSubsystem.setTurretPower(power); +// dischargeSubsystem.turret.setPower(power); if(work){ - dischargeSubsystem.setFlyWheelRPM(wantedRPM); - dischargeSubsystem.setRampDegree(wantedDegree); + dischargeSubsystem.flywheel.runRPM(wantedRPM); + dischargeSubsystem.ramp.setRampDegree(wantedDegree); }else{ - dischargeSubsystem.setFlyWheelRPM(0); + dischargeSubsystem.flywheel.runRPM(0); } super.run(); - multipleTelemetry.addData("turretAngle", dischargeSubsystem.getTurretAngle()); + multipleTelemetry.addData("turretAngle", dischargeSubsystem.turret.getAngle()); multipleTelemetry.addData("rampDegree", wantedDegree); multipleTelemetry.addData("wantedRPM", wantedRPM ); - multipleTelemetry.addData("rpm", dischargeSubsystem.getRPM()); - multipleTelemetry.addData("power", dischargeSubsystem.getFlyWheelPower()); + multipleTelemetry.addData("rpm", dischargeSubsystem.flywheel.getRPM()); + multipleTelemetry.addData("power", dischargeSubsystem.flywheel.getPower()); multipleTelemetry.addData("x", mecanumDrive.localizer.getPose().position.x); multipleTelemetry.addData("y", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.addData("distance", AutoShooter.getGoalDistance(mecanumDrive.localizer.getPose(), AutoShooter.TeamColor.BLUE)); @@ -138,14 +137,14 @@ private void aimTurret(){ launchAngle = Range.clip(launchAngle,24,332); double power; -// if (limelightSubsystem.getTx() != 0 && dischargeSubsystem.getTurretAngle() < 355 && dischargeSubsystem.getTurretAngle() > 5){ +// if (limelightSubsystem.getTx() != 0 && dischargeSubsystem.turret.getAngle() < 355 && dischargeSubsystem.turret.getAngle() > 5){ // power = limelightSubsystem.getTx() * kp; // } else{ - power = -(launchAngle - dischargeSubsystem.getTurretAngle()) * 0.018; + power = -(launchAngle - dischargeSubsystem.turret.getAngle()) * 0.018; // } // power += mecanumSpeed * 0.12; power += Math.signum(power) * 0.04; - dischargeSubsystem.setTurretPower(power); + dischargeSubsystem.turret.setPower(power); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/CarouselTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/CarouselTest.java index 2bca9eec2d7..d19e91d74f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/CarouselTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/CarouselTest.java @@ -1,8 +1,6 @@ package org.firstinspires.ftc.teamcode.decodeOpModes.testers; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.seattlesolvers.solverslib.command.SequentialCommandGroup; -import com.seattlesolvers.solverslib.command.WaitCommand; import com.seattlesolvers.solverslib.command.button.Button; import com.seattlesolvers.solverslib.gamepad.GamepadEx; import com.seattlesolvers.solverslib.gamepad.GamepadKeys; @@ -10,11 +8,10 @@ import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CarouselCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; -import org.firstinspires.ftc.teamcode.decodeSubsystems.Motif; @TeleOp(name = "CarouselTest", group = "tests") public class CarouselTest extends ActionOpMode { - CarouselSubsystem carouselSubsystem; + CarouselSubsystem carousel; GamepadEx gamepadEx; Button A, B; @@ -22,16 +19,16 @@ public class CarouselTest extends ActionOpMode { public void initialize() { gamepadEx = new GamepadEx(gamepad1); - carouselSubsystem = new CarouselSubsystem(hardwareMap); - carouselSubsystem.resetEncoders(); - gamepadEx.getGamepadButton(GamepadKeys.Button.A).whenPressed(new CarouselCommands.SlideDistance(carouselSubsystem, 3, 1)); + carousel = new CarouselSubsystem(hardwareMap); + carousel.resetEncoders(); + gamepadEx.getGamepadButton(GamepadKeys.Button.A).whenPressed(new CarouselCommands.RotateDistance(carousel, 3, 1)); } @Override public void run() { - carouselSubsystem.setSpinPower(gamepad1.left_stick_x); - multipleTelemetry.addData("pos", carouselSubsystem.getPosition()); - multipleTelemetry.addData("angle", carouselSubsystem.getAngle()); + carousel.setSpinPower(gamepad1.left_stick_x); + multipleTelemetry.addData("pos", carousel.getPosition()); + multipleTelemetry.addData("angle", carousel.getAngle()); multipleTelemetry.update(); super.run(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/DistanceSensorTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/DistanceSensorTester.java index 595d0d0d420..b6580e8212c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/DistanceSensorTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/DistanceSensorTester.java @@ -2,20 +2,13 @@ import com.acmerobotics.roadrunner.Pose2d; -import com.qualcomm.hardware.lynx.LynxController; -import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.Rev2mDistanceSensor; -import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.ColorSensor; -import com.qualcomm.robotcore.hardware.DistanceSensor; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.actions.ActionOpMode; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; -import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.IntakeSubsystem; import org.firstinspires.ftc.teamcode.needle.commands.MecanumCommands; @@ -29,13 +22,13 @@ public class DistanceSensorTester extends ActionOpMode { int count = 0; double leftDistance = 0, rightDistance = 0; double start = 0, fStart = 0; - IntakeSubsystem intakeSubsystem; + IntakeSubsystem intake; MecanumDrive mecanumDrive; @Override public void initialize() { mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(-63, 0, -Math.PI)); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); left = hardwareMap.get(Rev2mDistanceSensor.class, "leftDistance"); right = hardwareMap.get(Rev2mDistanceSensor.class, "rightDistance"); time = new ElapsedTime(); @@ -48,15 +41,15 @@ public void initialize() { public void run() { i++; if (count < 3) { - intakeSubsystem.setPower(gamepad1.left_stick_x); + intake.setPower(gamepad1.left_stick_x); start = time.seconds(); fStart = start; } else if (count >= 3) { // start = time.seconds(); // if(start - fStart > 0.3){ - intakeSubsystem.setPower(0); + intake.stopIntake(); } else { - intakeSubsystem.setPower(-0.5); + intake.runReverse(); if (time.seconds() - start > 0.7) { count = 3; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/FlyWheelTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/FlyWheelTester.java index c3ec87e8b91..84e2cc1d781 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/FlyWheelTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/FlyWheelTester.java @@ -3,12 +3,9 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Servo; import com.seattlesolvers.solverslib.command.button.GamepadButton; import com.seattlesolvers.solverslib.gamepad.GamepadEx; import com.seattlesolvers.solverslib.gamepad.GamepadKeys; -import com.seattlesolvers.solverslib.hardware.motors.Motor; -import com.seattlesolvers.solverslib.hardware.motors.MotorEx; import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeSubsystems.DischargeSubsystem; @@ -17,64 +14,50 @@ @TeleOp(group = "tests") public class FlyWheelTester extends ActionOpMode { GamepadEx gamepad; - GamepadButton A, B, X, Y, up, down; - // MotorEx flyWheelMotor; -// Servo servo; - double power = 0; - double kS = 0.09, kV = 0.0002, kP = 0.000833333, kI = 0.001; + GamepadButton increaseRPMButton, decreaseRPMButton, increasePowerButton, decreasePowerButton, increaseWantedAngleButton, decreaseWantedAngleButton; + + private double power = 0; public static double wantedRpm = 6000; - double wantedAngle = 28; - DischargeSubsystem dischargeSubsystem; - DcMotor m2; + private double wantedAngle = 28; + + private DcMotor m2; + private DischargeSubsystem dischargeSubsystem; + + + private void setupBindings() { + increaseRPMButton = new GamepadButton(gamepad, GamepadKeys.Button.A); + decreaseRPMButton = new GamepadButton(gamepad, GamepadKeys.Button.B); + increasePowerButton = new GamepadButton(gamepad, GamepadKeys.Button.X); + decreasePowerButton = new GamepadButton(gamepad, GamepadKeys.Button.Y); + increaseWantedAngleButton = new GamepadButton(gamepad, GamepadKeys.Button.DPAD_UP); + decreaseWantedAngleButton = new GamepadButton(gamepad, GamepadKeys.Button.DPAD_DOWN); + + increaseWantedAngleButton.whenPressed(() -> wantedAngle += 1); + decreaseWantedAngleButton.whenPressed(() -> wantedAngle -= 1); + increaseRPMButton.whenPressed(() -> wantedRpm += 100); + decreaseRPMButton.whenPressed(() -> wantedRpm -= 100); + increasePowerButton.whenPressed(() -> power += 0.1); + decreasePowerButton.whenPressed(() -> power -= 0.1); + } @Override public void initialize() { -// m2 = hardwareMap.dcMotor.get("turretMotor"); dischargeSubsystem = new DischargeSubsystem(hardwareMap); gamepad = new GamepadEx(gamepad1); - A = new GamepadButton(gamepad, GamepadKeys.Button.A); - B = new GamepadButton(gamepad, GamepadKeys.Button.B); - X = new GamepadButton(gamepad, GamepadKeys.Button.X); - Y = new GamepadButton(gamepad, GamepadKeys.Button.Y); - up = new GamepadButton(gamepad, GamepadKeys.Button.DPAD_UP); - down = new GamepadButton(gamepad, GamepadKeys.Button.DPAD_DOWN); - up.whenPressed(() -> wantedAngle += 1); - down.whenPressed(() -> wantedAngle -= 1); - A.whenPressed(() -> wantedRpm += 100); - B.whenPressed(() -> wantedRpm -= 100); -// X.whenPressed(() -> wantedAngle = 75); -// Y.whenPressed(() -> wantedAngle = 30); -// A.whenPressed(() -> power += 0.01); -// B.whenPressed(() -> power -= 0.01); - X.whenPressed(() -> power += 0.1); - Y.whenPressed(() -> power -= 0.1); -// X.whenPressed(new Runnable() { -// @Override -// public void run() { -// power += 0.01; -// flyWheelMotor.set(power); -// } -// }); - - -// flyWheelMotor.setRunMode(Motor.RunMode.VelocityControl); + setupBindings(); } @Override public void run() { -// m2.setPower(power); - dischargeSubsystem.setRampDegree(wantedAngle); -// dischargeSubsystem.setFlyWheelRPM(wantedRpm); -// dischargeSubsystem.setFlyWheelPower(power); - dischargeSubsystem.setFlyWheelRPM(wantedRpm); + super.run(); - multipleTelemetry.addData("rpm", dischargeSubsystem.getRPM()); - multipleTelemetry.addData("power", dischargeSubsystem.getFlyWheelPower()); + dischargeSubsystem.ramp.setRampDegree(wantedAngle); + dischargeSubsystem.flywheel.runRPM(wantedRpm); + + multipleTelemetry.addData("rpm", dischargeSubsystem.flywheel.getRPM()); + multipleTelemetry.addData("power", dischargeSubsystem.flywheel.getPower()); multipleTelemetry.addData("wanted", wantedRpm); multipleTelemetry.addData("angle", wantedAngle); multipleTelemetry.update(); - super.run(); } - - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/KitBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/KitBot.java index 541d864e483..c98b010250c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/KitBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/KitBot.java @@ -10,10 +10,6 @@ import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.actions.ActionOpMode; -import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; -import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; -import org.firstinspires.ftc.teamcode.decodeSubsystems.IntakeSubsystem; import org.firstinspires.ftc.teamcode.needle.commands.MecanumCommands; @TeleOp(group = "tests") diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/LimelightSubsystemTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/LimelightSubsystemTester.java index 5b26c71f580..c6adefb861c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/LimelightSubsystemTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/LimelightSubsystemTester.java @@ -41,10 +41,10 @@ public void run() { multipleTelemetry.addData("pinpointY", mecanumDrive.localizer.getPose().position.y); multipleTelemetry.addData("heading", mecanumDrive.localizer.getPose().heading.toDouble() * 180 / Math.PI); multipleTelemetry.addData("IMUheading", mecanumDrive.lazyImu.get().getRobotYawPitchRollAngles().getYaw()); - multipleTelemetry.addData("turretAngle", dischargeSubsystem.getTurretAngle()); + multipleTelemetry.addData("turretAngle", dischargeSubsystem.turret.getAngle()); multipleTelemetry.update(); super.run(); -// Position llCalcPos = limelightSubsystem.getRobotPosMT2(dischargeSubsystem.getTurretAngle()); +// Position llCalcPos = limelightSubsystem.getRobotPosMT2(dischargeSubsystem.turret.getAngle()); // if(llCalcPos != null){ // multipleTelemetry.addData("llCalcX",llCalcPos.x); // multipleTelemetry.addData("llCalcY",llCalcPos.y); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/SortingTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/SortingTest.java index 8adc39a6766..c49da4a58b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/SortingTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/SortingTest.java @@ -2,7 +2,6 @@ import com.acmerobotics.roadrunner.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.seattlesolvers.solverslib.command.ScheduleCommand; import com.seattlesolvers.solverslib.command.button.Button; import com.seattlesolvers.solverslib.command.button.GamepadButton; import com.seattlesolvers.solverslib.gamepad.GamepadEx; @@ -11,16 +10,14 @@ import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.actions.ActionOpMode; import org.firstinspires.ftc.teamcode.decodeCommands.CommandGroups; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.CarouselSubsystem; import org.firstinspires.ftc.teamcode.decodeSubsystems.IntakeSubsystem; -import org.firstinspires.ftc.teamcode.decodeSubsystems.Motif; import org.firstinspires.ftc.teamcode.needle.commands.MecanumCommands; @TeleOp(group = "tests") public class SortingTest extends ActionOpMode { - CarouselSubsystem carouselSubsystem; - IntakeSubsystem intakeSubsystem; + CarouselSubsystem carousel; + IntakeSubsystem intake; MecanumDrive mecanumDrive; GamepadEx gamepad; Button A, B, X, Y, UP; @@ -28,10 +25,10 @@ public class SortingTest extends ActionOpMode { @Override public void initialize() { - carouselSubsystem = new CarouselSubsystem(hardwareMap); + carousel = new CarouselSubsystem(hardwareMap); mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(63, 0, -Math.PI)); - carouselSubsystem.resetEncoders(); - intakeSubsystem = new IntakeSubsystem(hardwareMap); + carousel.resetEncoders(); + intake = new IntakeSubsystem(hardwareMap); gamepad = new GamepadEx(gamepad1); A = new GamepadButton(gamepad, GamepadKeys.Button.A); B = new GamepadButton(gamepad, GamepadKeys.Button.B); @@ -39,15 +36,15 @@ public void initialize() { Y = new GamepadButton(gamepad, GamepadKeys.Button.Y); UP = new GamepadButton(gamepad, GamepadKeys.Button.DPAD_UP); - A.whenPressed(new CommandGroups.StartIntake(intakeSubsystem, carouselSubsystem)); - UP.whenPressed(new IntakeCommands.OutTakeState(intakeSubsystem)); + A.whenPressed(CommandGroups.startIntake(intake, carousel)); + UP.whenPressed(intake.outtake()); schedule(new MecanumCommands.Drive(mecanumDrive, () -> gamepad.getLeftY(), () -> gamepad.getLeftX(), () -> gamepad.getRightX())); } @Override public void run() { - multipleTelemetry.addData("greenPos", carouselSubsystem.getGreenPlacement()); - multipleTelemetry.addData("intakeCurrent", intakeSubsystem.getCurrent()); + multipleTelemetry.addData("greenPos", carousel.getGreenPlacement()); + multipleTelemetry.addData("intakeCurrent", intake.getCurrent()); multipleTelemetry.update(); super.run(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/TouchSensorTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/TouchSensorTester.java index d44b25fcde5..0f826975372 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/TouchSensorTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/TouchSensorTester.java @@ -9,7 +9,6 @@ import com.seattlesolvers.solverslib.gamepad.GamepadKeys; import org.firstinspires.ftc.teamcode.actions.ActionOpMode; -import org.firstinspires.ftc.teamcode.decodeCommands.IntakeCommands; import org.firstinspires.ftc.teamcode.decodeSubsystems.IntakeSubsystem; @TeleOp(group = "tests") @@ -19,7 +18,7 @@ public class TouchSensorTester extends ActionOpMode { double lastTime, currentTime; boolean last = false; DigitalChannel left; - IntakeSubsystem intakeSubsystem; + IntakeSubsystem intake; int i = 0; GamepadEx gamepad; Button A, B, X, Y; @@ -27,16 +26,17 @@ public class TouchSensorTester extends ActionOpMode { @Override public void initialize() { - intakeSubsystem = new IntakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); gamepad = new GamepadEx(gamepad1); A = new GamepadButton(gamepad, GamepadKeys.Button.A); Y = new GamepadButton(gamepad, GamepadKeys.Button.Y); X = new GamepadButton(gamepad, GamepadKeys.Button.X); B = new GamepadButton(gamepad, GamepadKeys.Button.B); - ; - X.whenPressed(new IntakeCommands.IntakeState(intakeSubsystem)); - Y.whenPressed(new IntakeCommands.OutTakeState(intakeSubsystem)); - B.whenPressed(new IntakeCommands.ClosedState(intakeSubsystem)); + + + X.whenPressed(intake.intake()); + Y.whenPressed(intake.outtake()); + B.whenPressed(intake.closeState()); } @Override @@ -44,15 +44,15 @@ public void run() { super.run(); multipleTelemetry.addData("count", IntakeSubsystem.count); - multipleTelemetry.addData("rightSwitchState", intakeSubsystem.rightSwitchState()); - multipleTelemetry.addData("leftSwitchState", intakeSubsystem.leftSwitchState()); -// multipleTelemetry.addData("left",intakeSubsystem.leftSwitch.getState() ? 0:1); -// multipleTelemetry.addData("right",intakeSubsystem.rightSwitch.getState() ? 0:1); + multipleTelemetry.addData("rightSwitchState", intake.rightSwitchState()); + multipleTelemetry.addData("leftSwitchState", intake.leftSwitchState()); +// multipleTelemetry.addData("left",intake.leftSwitch.getState() ? 0:1); +// multipleTelemetry.addData("right",intake.rightSwitch.getState() ? 0:1); multipleTelemetry.update(); } @Override public void end() { - intakeSubsystem.stopSensorThread(); + intake.stopSensorThread(); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/TurretTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/TurretTester.java index 257a1ddfe5d..a4e78448794 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/TurretTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeOpModes/testers/TurretTester.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.decodeOpModes.testers; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import com.seattlesolvers.solverslib.command.InstantCommand; import com.seattlesolvers.solverslib.command.SequentialCommandGroup; @@ -25,7 +24,7 @@ public class TurretTester extends ActionOpMode { @Override public void initialize() { dischargeSubsystem = new DischargeSubsystem(hardwareMap); - dischargeSubsystem.resetTurret(); + dischargeSubsystem.turret.reset(); gamepad = new GamepadEx(gamepad1); A = new GamepadButton(gamepad, GamepadKeys.Button.A); B = new GamepadButton(gamepad, GamepadKeys.Button.B); @@ -33,15 +32,15 @@ public void initialize() { A.whenPressed(() -> manual = !manual); B.whenPressed(() -> power += 0.01); X.whenPressed(new SequentialCommandGroup( new InstantCommand((() ->{ - lastVelocity = dischargeSubsystem.getRPS(); + lastVelocity = dischargeSubsystem.turret.getAngularVelocity(); lastTime = time.seconds(); - dischargeSubsystem.setTurretPower(0.5); + dischargeSubsystem.turret.setPower(0.5); } )), new WaitCommand(500), new InstantCommand(() -> { - velocity = dischargeSubsystem.getRPS(); + velocity = dischargeSubsystem.turret.getAngularVelocity(); currentTime = time.seconds(); - dischargeSubsystem.setTurretPower(0); + dischargeSubsystem.turret.stopPower(); }))); time = new ElapsedTime(); lastTime = 0; @@ -53,18 +52,18 @@ public void initialize() { @Override public void run() { // if (manual) { -// dischargeSubsystem.setTurretPower(gamepad.getRightX() * (0.5 + 0.5 * gamepad.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER))); +// dischargeSubsystem.turret.setPower(gamepad.getRightX() * (0.5 + 0.5 * gamepad.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER))); // } else { -// dischargeSubsystem.setTurretPower(power); +// dischargeSubsystem.turret.setPower(power); // } // velocity = dischargeSubsystem.getRPS(); // currentTime = time.seconds(); double acceleration = (velocity - lastVelocity) / (currentTime - lastTime); multipleTelemetry.addData("power", power); - multipleTelemetry.addData("speed", dischargeSubsystem.getRPS()); + multipleTelemetry.addData("speed", dischargeSubsystem.turret.getAngularVelocity()); multipleTelemetry.addData("acceleration",acceleration); - multipleTelemetry.addData("ticks", dischargeSubsystem.getTurretPosition()); - multipleTelemetry.addData("angle", dischargeSubsystem.getTurretAngle()); + multipleTelemetry.addData("ticks", dischargeSubsystem.turret.getPosition()); + multipleTelemetry.addData("angle", dischargeSubsystem.turret.getAngle()); multipleTelemetry.update(); super.run(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/CarouselSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/CarouselSubsystem.java index 71206d8e09b..20a53f850a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/CarouselSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/CarouselSubsystem.java @@ -5,7 +5,10 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DistanceSensor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.seattlesolvers.solverslib.command.Command; +import com.seattlesolvers.solverslib.command.FunctionalCommand; import com.seattlesolvers.solverslib.command.SubsystemBase; +import com.seattlesolvers.solverslib.controller.PIDController; import com.seattlesolvers.solverslib.hardware.motors.Motor; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; @@ -14,7 +17,7 @@ public class CarouselSubsystem extends SubsystemBase { private final DcMotorEx carouselMotor; double tickPerRev = 384.5;//537.6 - + private final PIDController pid = new PIDController(0.018, 0, 0); public final double spinConversion = tickPerRev * 132.0 / 39.0 / 3.0; // for moving the motor a third of a spin public ColorSensor leftColorSensor, rightColorSensor, middleColorSensor; @@ -38,6 +41,10 @@ public void setSpinPower(double power) { carouselMotor.setPower(power); } + public void stop() { + setSpinPower(0); + } + public int getPosition() { return carouselMotor.getCurrentPosition() + startingTicks; } @@ -94,4 +101,17 @@ public static SensorColors colorIdentifier(ColorSensor colorSensor) { } return SensorColors.Unknown; } + + + public Command rotateToAngle(double targetAngle, boolean finishWhenAtSetpoint) { + double tolerance = 3; + return new FunctionalCommand(() -> { + }, + () -> setSpinPower(pid.calculate(getAngle(), targetAngle)), + (i) -> stop(), + () -> finishWhenAtSetpoint && Math.abs(getAngle() - targetAngle) < tolerance); + } + public Command rotateToAngle(double targetAngle) { + return rotateToAngle(targetAngle, false); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/DischargeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/DischargeSubsystem.java index 2abfeba15de..720f1027bc1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/DischargeSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/DischargeSubsystem.java @@ -1,113 +1,23 @@ package org.firstinspires.ftc.teamcode.decodeSubsystems; -import static java.lang.Math.abs; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareDevice; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.util.Range; import com.seattlesolvers.solverslib.command.SubsystemBase; -import com.seattlesolvers.solverslib.hardware.motors.Motor; -import com.seattlesolvers.solverslib.hardware.motors.MotorEx; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; public class DischargeSubsystem extends SubsystemBase { - - private final DcMotorEx turretMotor; - public final MotorEx flyWheelMotor; - private final Servo rampServo; - private final double kS = 0.07, kV = 0.00016305959, kP = 0.00133333, kI = 0.000005;//0.17,0.00025772193 - private final double ticksPerDegree = 383.6 * (198.0 / 49.0) / 360.0; - private double startAngle; - private final double gearRatio = 37.0 / 37; // 34:30 - double integral; + public final FlywheelSubsystem flywheel; + public final TurretSubsystem turret; + public final RampSubsystem ramp; public static boolean shooting = false; public DischargeSubsystem(HardwareMap hm) { - flyWheelMotor = new MotorEx(hm, "flyWheelMotor", Motor.GoBILDA.BARE); - flyWheelMotor.encoder.setDirection(Motor.Direction.REVERSE); // for testing 2.7.26 - turretMotor = hm.get(DcMotorEx.class, "turretMotor"); - turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rampServo = hm.get(Servo.class, "rampServo"); - startAngle = 180; - integral = 0; + flywheel = new FlywheelSubsystem(hm); + turret = new TurretSubsystem(hm); + ramp = new RampSubsystem(hm); + shooting = false; } public void resetTurret() { - turretMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - turretMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - } - - public void setFlyWheelPower(double flyWheelPower) { - flyWheelMotor.set(flyWheelPower); -// turretMotor.setPower(-flyWheelPower); - } - - public void setFlyWheelRPM(double rpm) { - if (abs(rpm) < 200) { - flyWheelMotor.set(0); -// turretMotor.setPower(0); - return; - } - - -// rpm *= gearRatio; - double currentRPM = getRPM(); - if (currentRPM - rpm > 280) { - flyWheelMotor.set(0); - } else if (currentRPM < rpm || shooting) { - flyWheelMotor.set(1); -// turretMotor.setPower(-1); - } else { - flyWheelMotor.set(kS * Math.signum(rpm) + kV * rpm - 0.07 + kP * (rpm - getRPM())); -// turretMotor.setPower(-kS * Math.signum(rpm) - kV * rpm + 0.04); - } -// flyWheelMotor.set(1); - } - - public void stayRPM(double rpm) { - flyWheelMotor.set(kS * Math.signum(rpm) + kV * rpm - 0.02 + kP * (rpm - getRPM())); - } - - public double getRPM() { - return -flyWheelMotor.getCorrectedVelocity() / flyWheelMotor.getCPR() * 60 * gearRatio; - } - - public double getFlyWheelPower() { - return flyWheelMotor.get(); - } - - public void setTurretPower(double turretPower) { - if (getTurretAngle() > 340) { - turretPower = Math.max(turretPower, 0); - } else if (getTurretAngle() < 20) { - turretPower = Math.min(turretPower, 0); - } - turretMotor.setPower(turretPower); - } - - public double getTurretPosition() { - return turretMotor.getCurrentPosition(); - } - - public double getTurretAngle() { - return 360 - (turretMotor.getCurrentPosition() / ticksPerDegree + startAngle); - } - - public void setTurretAngle(double angle) { - startAngle += getTurretAngle() - angle; - } - - public double getRPS() { - return turretMotor.getVelocity(AngleUnit.RADIANS) / (198.0 / 49.0); - } - - public void setRampDegree(double rampDegree) { - double pos = (60 - rampDegree) / 30.0; - rampServo.setPosition(Math.min(pos * 0.6 - 0.2, 0.6)); + turret.reset(); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/FlywheelSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/FlywheelSubsystem.java new file mode 100644 index 00000000000..01845514622 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/FlywheelSubsystem.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode.decodeSubsystems; + +import static java.lang.Math.abs; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.seattlesolvers.solverslib.command.SubsystemBase; +import com.seattlesolvers.solverslib.controller.PIDController; +import com.seattlesolvers.solverslib.controller.wpilibcontroller.SimpleMotorFeedforward; +import com.seattlesolvers.solverslib.hardware.motors.Motor; +import com.seattlesolvers.solverslib.hardware.motors.MotorEx; + +import org.firstinspires.ftc.teamcode.BangBangController; + +public class FlywheelSubsystem extends SubsystemBase { + private final double gearRatio = 37.0 / 37; // 34:30 + private final SimpleMotorFeedforward flywheelFeedforward = new SimpleMotorFeedforward(0.07, 0.00016305959, 0); + private final PIDController flywheelPID = new PIDController(0.00133333, 0.000005, 0); + private final BangBangController bangbang = new BangBangController(200); + + public final MotorEx flyWheelMotor; + + public FlywheelSubsystem(HardwareMap hm) { + flyWheelMotor = new MotorEx(hm, "flyWheelMotor", Motor.GoBILDA.BARE); + flyWheelMotor.encoder.setDirection(Motor.Direction.REVERSE); // for testing 2.7.26 + } + + public void setPower(double flyWheelPower) { + flyWheelMotor.set(flyWheelPower); + } + + public void runRPM(double rpm) { + if (abs(rpm) < 200) { + flyWheelMotor.set(0); + return; + } + + double currentRPM = getRPM(); + + if (bangbang.isAtSetpoint(currentRPM, rpm)) { // switching to pid for close range-error + flyWheelMotor.set(calculateFlywheelCloseLoopOutput(currentRPM, rpm)); + } else { + flyWheelMotor.set(bangbang.calculate(currentRPM, rpm)); + } + } + + + private double calculateFlywheelCloseLoopOutput(double currentRPM, double setpoint) { + return flywheelPID.calculate(currentRPM, setpoint) + flywheelFeedforward.calculate(setpoint); + } + + public void keepRPM(double rpm) { + flyWheelMotor.set(calculateFlywheelCloseLoopOutput(getRPM(), rpm)); + } + + public double getRPM() { + return -flyWheelMotor.getCorrectedVelocity() / flyWheelMotor.getCPR() * 60 * gearRatio; + } + + public double getPower() { + return flyWheelMotor.get(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/IntakeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/IntakeSubsystem.java index 9652cc36f94..ac36600cdac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/IntakeSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/IntakeSubsystem.java @@ -5,11 +5,18 @@ import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; +import com.seattlesolvers.solverslib.command.Command; +import com.seattlesolvers.solverslib.command.Command; +import com.seattlesolvers.solverslib.command.FunctionalCommand; +import com.seattlesolvers.solverslib.command.InstantCommand; import com.seattlesolvers.solverslib.command.SubsystemBase; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; public class IntakeSubsystem extends SubsystemBase { + private static final double OPEN_POSITION = 1; + private static final double CLOSED_POSITION = 0; + private final DcMotorEx motor; private final Servo intakeServo1, intakeServo2; public final DigitalChannel leftSwitch, rightSwitch; @@ -38,13 +45,61 @@ public void setPower(double power) { motor.setPower(power); } + public void runIntakeFull() { + setPower(1); + } + + public void runReverse() { + setPower(-0.7); + } + + public void stopIntake() { + setPower(0); + } + + + + public Command transfer() { + return new InstantCommand(() -> { + runIntakeFull(); + close(); + }, this); + } + + public Command outtake() { + return new InstantCommand(this::runReverse, this).alongWith(open()); + } + + public Command sortState() { + return new InstantCommand(() -> { + stopIntake(); + close(); + }, this); + } + + public Command closeState() { + return new InstantCommand(this::stopIntake, this).alongWith(open()); + } + + public Command intake() { + // Functionally the same as IntakeState, + // unsure why there were a lot of timers and stuff there + return new FunctionalCommand(() -> { + count = 0; + runIntakeFull(); + }, () -> { + }, (interrupted) -> stopIntake(), () -> count >= 3) + .alongWith(open()); + } + + private void startSensorThread() { sensorThread = new Thread(() -> { boolean currentRight, currentLeft; boolean lastRight = false, lastLeft = false; while (running) { - if(intaking){ + if (intaking) { // check switch state currentRight = rightSwitchState(); @@ -82,11 +137,11 @@ private void startSensorThread() { sensorThread.start(); } - public boolean rightSwitchState(){ + public boolean rightSwitchState() { return rightSwitch.getState(); } - public boolean leftSwitchState(){ + public boolean leftSwitchState() { return !leftSwitch.getState(); } @@ -94,14 +149,21 @@ public void stopSensorThread() { running = false; } - public void setPosition(double position) { + private void setPosition(double position) { intakeServo1.setPosition(position); intakeServo2.setPosition(1 - position); } + public Command open() { + return new InstantCommand(() -> setPosition(OPEN_POSITION), this); + } + + public void close() { + setPosition(CLOSED_POSITION); + } + public double getCurrent() { return motor.getCurrent(CurrentUnit.AMPS); } - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/RampSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/RampSubsystem.java new file mode 100644 index 00000000000..c81dc524362 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/RampSubsystem.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.decodeSubsystems; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.seattlesolvers.solverslib.command.SubsystemBase; + + +public class RampSubsystem extends SubsystemBase { + + private final Servo rampServo; + + public RampSubsystem(HardwareMap hm) { + rampServo = hm.get(Servo.class, "rampServo"); + } + + + public void setRampDegree(double rampDegree) { + double pos = (60 - rampDegree) / 30.0; + rampServo.setPosition(Math.min(pos * 0.6 - 0.2, 0.6)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/TurretSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/TurretSubsystem.java new file mode 100644 index 00000000000..7e2087947aa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/decodeSubsystems/TurretSubsystem.java @@ -0,0 +1,65 @@ +package org.firstinspires.ftc.teamcode.decodeSubsystems; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.seattlesolvers.solverslib.command.Command; +import com.seattlesolvers.solverslib.command.FunctionalCommand; +import com.seattlesolvers.solverslib.command.SubsystemBase; +import com.seattlesolvers.solverslib.controller.PIDController; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +public class TurretSubsystem extends SubsystemBase { + private double startAngle = 180; + private final DcMotorEx turretMotor; + + private final double ticksPerDegree = 383.6 * (198.0 / 49.0) / 360.0; + private final PIDController pid = new PIDController(-0.02, 0, 0); + + public TurretSubsystem(HardwareMap hm) { + turretMotor = hm.get(DcMotorEx.class, "turretMotor"); + turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + + public double getAngle() { + return 360 - (turretMotor.getCurrentPosition() / ticksPerDegree + startAngle); + } + + public void setAngle(double angle) { + startAngle += getAngle() - angle; + } + + + public void reset() { + turretMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + turretMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + public double getAngularVelocity() { + return turretMotor.getVelocity(AngleUnit.RADIANS) / (198.0 / 49.0); + } + + public double getPosition() { + return turretMotor.getCurrentPosition(); + } + + public void stopPower() { + setPower(0); + } + + public void runToAngleDirect(double angle) { + setPower(pid.calculate(getAngle(), angle)); + } + + + public void setPower(double turretPower) { + if (getAngle() > 340) { + turretPower = Math.max(turretPower, 0); + } else if (getAngle() < 20) { + turretPower = Math.min(turretPower, 0); + } + turretMotor.setPower(turretPower); + } +}