diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/ClawAndWristBot.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/ClawAndWristBot.java index 1818e6e3..6434cd9e 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/ClawAndWristBot.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/ClawAndWristBot.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.hoops; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import org.firstinspires.ftc.hoops.subsystems.ClawAndWristSubsystem; //@Config @@ -13,10 +13,6 @@ public class ClawAndWristBot { public ClawAndWristSubsystem caw; public ClawAndWristBot() { - caw = new ClawAndWristSubsystem( - new ServoGroup(CLAW1), - new ServoGroup(CLAW2), - new ServoGroup(WRIST) - ); + caw = new ClawAndWristSubsystem(new Servo(CLAW1), new Servo(CLAW2), new Servo(WRIST)); } } diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/Hardware.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/Hardware.java index 6483ba2e..82b45b1d 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/Hardware.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/Hardware.java @@ -11,7 +11,7 @@ import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.hardware.sensor.IMU; import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Loggable; import com.technototes.vision.hardware.Webcam; import java.util.List; @@ -25,8 +25,8 @@ public class Hardware implements Loggable { public Motor placeholder1; public DcMotorEx liftMotor; - public ServoGroup placeholder2; - public ServoGroup servo; + public Servo placeholder2; + public Servo servo; public IGyro imu; public Webcam camera; @@ -67,7 +67,7 @@ public Hardware(HardwareMap hwmap) { } if (Setup.Connected.TESTSUBSYSTEM) { if (Setup.Connected.SERVO) { - this.servo = new ServoGroup(Setup.HardwareNames.SERVO); + this.servo = new Servo(Setup.HardwareNames.SERVO); } // if (Setup.Connected.COLOR_SENSOR) { // this.colorSensor = new ColorDistanceSensor(Setup.HardwareNames.COLOR); diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/ClawAndWristSubsystem.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/ClawAndWristSubsystem.java index bf36ee12..17fb2d71 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/ClawAndWristSubsystem.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/ClawAndWristSubsystem.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.hoops.subsystems; import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -31,11 +31,11 @@ public class ClawAndWristSubsystem implements Subsystem, Loggable { @Log(name = "Wrist") public double WristPos; - private ServoGroup claw1; - private ServoGroup claw2; - private ServoGroup wrist; + private Servo claw1; + private Servo claw2; + private Servo wrist; - public ClawAndWristSubsystem(ServoGroup c1, ServoGroup c2, ServoGroup w) { + public ClawAndWristSubsystem(Servo c1, Servo c2, Servo w) { claw1 = c1; claw2 = c2; wrist = w; diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/PlacementSubsystem.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/PlacementSubsystem.java index 6370224a..89598a11 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/PlacementSubsystem.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/PlacementSubsystem.java @@ -2,7 +2,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -17,7 +17,7 @@ public class PlacementSubsystem implements Subsystem, Loggable { public static double ScoreServo = 0.5; public static double ArmServo = 0.5; - public ServoGroup armServo; + public Servo armServo; public static double ScoreServoInput = 0.5; @@ -27,7 +27,7 @@ public class PlacementSubsystem implements Subsystem, Loggable { public static double ArmServoOutput = 0.5; - public ServoGroup scoreServo; + public Servo scoreServo; public DcMotorEx liftMotor; private boolean isHardware; diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/TestSubsystem.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/TestSubsystem.java index 4cc5857a..54dd2b82 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/TestSubsystem.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/TestSubsystem.java @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.util.Range; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -37,7 +37,7 @@ public class TestSubsystem implements Subsystem, Loggable { // but only while/if the the distance is greater than 10cm private EncodedMotor theMotor; - private ServoGroup servo; + private Servo servo; private Rev2MDistanceSensor theSensor; private boolean running; diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Controls.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Controls.java deleted file mode 100644 index 95e91267..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Controls.java +++ /dev/null @@ -1,291 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl; - -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.ARM_ENABLED; -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.BRAKE_ENABLED; -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.CAP_ENABLED; -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.CAROUSEL_ENABLED; -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.DRIVE_ENABLED; -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.EXTENSION_ENABLED; -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.INTAKE_ENABLED; -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.LIFT_ENABLED; - -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.library.control.CommandAxis; -import com.technototes.library.control.CommandButton; -import com.technototes.library.control.CommandGamepad; -import com.technototes.library.control.CommandInput; -import com.technototes.library.control.Stick; -import com.technototes.library.util.Alliance; -import com.technototes.path.command.ResetGyroCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands; -import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmAllianceCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmInCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmSharedCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpVariableCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapArmTranslateCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapCloseCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapDownCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapOpenCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapOutCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapStoreCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapTurretTranslateCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.carousel.CarouselLeftCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.carousel.CarouselRightCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionSideCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.TurretTranslateCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeSafeCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevel1Command; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevelCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftSharedCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftTranslateCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class Controls { - - public CommandGamepad driverGamepad, codriverGamepad; - - public Robot robot; - - public CommandAxis dumpAxis; - public CommandInput toIntakeButton; - public CommandButton sharedHubButton, allianceHubButton; - - public CommandButton liftAdjustUpButton, liftAdjustDownButton, turretAdjustRightButton, turretAdjustLeftButton; - - public CommandButton intakeInButton, intakeOutButton; - - public CommandButton carouselButton, carouselBackButton; - - public CommandButton capUpButton, capDownButton; - - public Stick driveLeftStick, driveRightStick; - public CommandButton resetGyroButton, brakeButton; - - public CommandButton strategy1Button, strategy2Button; - - public Controls(Robot r, CommandGamepad driver, CommandGamepad codriver) { - driverGamepad = driver; - codriverGamepad = codriver; - robot = r; - - dumpAxis = driverGamepad.leftTrigger.setTriggerThreshold(0.2); - sharedHubButton = driverGamepad.leftBumper; - allianceHubButton = driverGamepad.rightBumper; - toIntakeButton = driverGamepad.rightTrigger.setTriggerThreshold(0.3); - - liftAdjustUpButton = driverGamepad.dpadUp; - liftAdjustDownButton = driverGamepad.dpadDown; - turretAdjustRightButton = driverGamepad.dpadRight; - turretAdjustLeftButton = driverGamepad.dpadLeft; - - intakeInButton = driverGamepad.ps_cross; - intakeOutButton = driverGamepad.ps_circle; - - carouselButton = driverGamepad.ps_square; - carouselBackButton = driverGamepad.ps_triangle; - - resetGyroButton = driverGamepad.rightStickButton; - brakeButton = driverGamepad.leftStickButton; - - driveLeftStick = driverGamepad.leftStick; - driveRightStick = driverGamepad.rightStick; - - capUpButton = driverGamepad.ps_options; - - strategy1Button = driverGamepad.ps_options; - strategy2Button = driverGamepad.ps_share; - - RobotState.setStrategy( - RobotState.AllianceHubStrategy.HIGH, - RobotState.SharedHubStrategy.OWN - ); - - bindControls(); - } - - public void bindControls() { - if (LIFT_ENABLED) bindLiftControls(); - if (ARM_ENABLED) bindArmControls(); - if (DRIVE_ENABLED) bindDriveControls(); - if (INTAKE_ENABLED) bindIntakeControls(); - if (CAROUSEL_ENABLED) bindCarouselControls(); - if (CAP_ENABLED) bindCapControls(); - if (EXTENSION_ENABLED) bindExtensionControls(); - if (BRAKE_ENABLED) bindBrakeControls(); - - strategy1Button.whenPressed(() -> RobotState.strategy1()); - codriverGamepad.ps_options.whenPressed(() -> RobotState.strategy1()); - strategy2Button.whenPressed(() -> RobotState.strategy2()); - codriverGamepad.ps_share.whenPressed(() -> RobotState.strategy2()); - } - - public void bindBrakeControls() { - brakeButton.whenPressed(robot.brakeSubsystem::lower); - CommandScheduler.scheduleJoystick( - robot.brakeSubsystem::raise, - () -> driveLeftStick.getDistanceFromCenter() > 0.1 && robot.brakeSubsystem.get() - ); - } - - public void bindArmControls() { - dumpAxis.whilePressedOnce( - new BucketDumpVariableCommand(robot.armSubsystem, dumpAxis).asConditional( - EXTENSION_ENABLED ? robot.extensionSubsystem::isSlideOut : () -> true - ) - ); - toIntakeButton.whenPressed(new ArmInCommand(robot.armSubsystem)); - allianceHubButton.whileReleasedOnce(new ArmAllianceCommand(robot.armSubsystem)); - sharedHubButton.whileReleasedOnce(new ArmSharedCommand(robot.armSubsystem)); - intakeInButton.whenPressed(new ArmInCommand(robot.armSubsystem)); - } - - public void bindLiftControls() { - sharedHubButton.whileReleasedOnce( - new LiftSharedCommand(robot.liftSubsystem).withTimeout(0.5) - ); - allianceHubButton.whileReleasedOnce( - new LiftLevelCommand(robot.liftSubsystem).withTimeout(0.5) - ); - toIntakeButton.whenPressed( - new LiftLevel1Command(robot.liftSubsystem) - .withTimeout(0.5) - .alongWith(new WaitCommand(0.5)) - .andThen(new LiftCollectCommand(robot.liftSubsystem).withTimeout(0.4)) - ); - liftAdjustUpButton.whilePressed(new LiftTranslateCommand(robot.liftSubsystem, 50)); - liftAdjustDownButton.whilePressed(new LiftTranslateCommand(robot.liftSubsystem, -50)); - } - - public void bindDriveControls() { - if (EXTENSION_ENABLED && ARM_ENABLED && LIFT_ENABLED && INTAKE_ENABLED) { - // allianceHubButton.whilePressed( new TeleopDepositAllianceCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem).andThen(new TeleopIntakeAllianceWarehouseCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem))); - // sharedHubButton.whilePressed(new TeleopDepositSharedCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem).andThen(new TeleopIntakeSharedWarehouseCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem))); - } - robot.drivebaseSubsystem.setDefaultCommand( - new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) - ); - codriverGamepad.leftTrigger.whilePressedOnce( - new JoystickDriveCommand( - robot.drivebaseSubsystem, - codriverGamepad.leftStick, - codriverGamepad.rightStick - ) - ); // allianceHubButton.whenPressed(new DriveSpeedCommand(robot.drivebaseSubsystem).cancelUpon(toIntakeButton)); - resetGyroButton.whenPressed(new ResetGyroCommand(robot.drivebaseSubsystem)); - } - - public void bindIntakeControls() { - if (ARM_ENABLED) { - toIntakeButton.whenPressed( - new SequentialCommandGroup( - new WaitCommand(0.8), - new IntakeSafeCommand(robot.intakeSubsystem), - driverGamepad::rumbleBlip, - robot.armSubsystem::slightCarry, - new WaitCommand(0.2), - new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.5) - ).ignoreCancel() - ); - intakeInButton.whilePressedContinuous( - new SequentialCommandGroup( - new IntakeSafeCommand(robot.intakeSubsystem), - driverGamepad::rumbleBlip, - robot.armSubsystem::slightCarry, - new WaitCommand(0.2), - new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.5) - ).ignoreCancel() - ); - } else { - toIntakeButton.whenPressed( - new WaitCommand(0.8).andThen(new IntakeSafeCommand(robot.intakeSubsystem)) - ); - intakeInButton.whilePressedContinuous(new IntakeInCommand(robot.intakeSubsystem)); - } - intakeOutButton.whilePressedOnce(new IntakeOutCommand(robot.intakeSubsystem)); - allianceHubButton.whenReleased( - new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.2) - ); - sharedHubButton.whenReleased(new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.2)); - } - - public void bindCarouselControls() { - carouselButton.whilePressedOnce( - RobotConstants.getAlliance() - .selectOf( - new CarouselLeftCommand(robot.carouselSubsystem), - new CarouselRightCommand(robot.carouselSubsystem) - ) - ); - carouselBackButton.whilePressedOnce( - RobotConstants.getAlliance() - .selectOf( - new CarouselRightCommand(robot.carouselSubsystem), - new CarouselLeftCommand(robot.carouselSubsystem) - ) - ); - } - - public void bindCapControls() { - codriverGamepad.ps_circle.whenPressed(new CapOpenCommand(robot.capSubsystem)); - codriverGamepad.ps_cross.whenPressed(new CapCloseCommand(robot.capSubsystem)); - codriverGamepad.rightBumper.whenPressed(new CapOutCommand(robot.capSubsystem)); - codriverGamepad.leftBumper.whenPressed(new CapStoreCommand(robot.capSubsystem)); - codriverGamepad.rightTrigger.whenPressed(new CapDownCommand(robot.capSubsystem)); - codriverGamepad.dpadLeft.whilePressed( - new CapTurretTranslateCommand(robot.capSubsystem, -0.02) - ); - codriverGamepad.dpadRight.whilePressed( - new CapTurretTranslateCommand(robot.capSubsystem, 0.02) - ); - codriverGamepad.dpadUp.whilePressed(new CapArmTranslateCommand(robot.capSubsystem, 0.05)); - codriverGamepad.dpadDown.whilePressed( - new CapArmTranslateCommand(robot.capSubsystem, -0.05) - ); - } - - public void bindExtensionControls() { - allianceHubButton.whileReleasedOnce( - new ExtensionCommand( - robot.extensionSubsystem, - ExtensionSubsystem.ExtensionConstants.TELEOP_ALLIANCE, - ExtensionSubsystem.ExtensionConstants.CENTER - ) - ); - sharedHubButton.whileReleasedOnce(new ExtensionSideCommand(robot.extensionSubsystem)); - toIntakeButton.whenPressed(new ExtensionCollectCommand(robot.extensionSubsystem)); - turretAdjustLeftButton.whilePressed( - new TurretTranslateCommand( - robot.extensionSubsystem, - 0.05, - () -> - DRIVE_ENABLED && - ((RobotConstants.getAlliance() == Alliance.RED) ^ - (robot.drivebaseSubsystem.getExternalHeading() > - ExtensionSubsystem.ExtensionConstants.SNAP_1 && - robot.drivebaseSubsystem.getExternalHeading() < - ExtensionSubsystem.ExtensionConstants.SNAP_2)) - ) - ); - turretAdjustRightButton.whilePressed( - new TurretTranslateCommand( - robot.extensionSubsystem, - -0.05, - () -> - DRIVE_ENABLED && - ((RobotConstants.getAlliance() == Alliance.RED) ^ - (robot.drivebaseSubsystem.getExternalHeading() > - ExtensionSubsystem.ExtensionConstants.SNAP_1 && - robot.drivebaseSubsystem.getExternalHeading() < - ExtensionSubsystem.ExtensionConstants.SNAP_2)) - ) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java index d76edd63..930868d6 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java @@ -1,147 +1,91 @@ package org.firstinspires.ftc.ptechnodactyl; -import static org.firstinspires.ftc.ptechnodactyl.Hardware.HardwareConstants.*; -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.*; - +import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.hardware.motor.Motor; +import com.technototes.library.hardware.sensor.AdafruitIMU; import com.technototes.library.hardware.sensor.ColorDistanceSensor; +import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.hardware.sensor.IMU; -import com.technototes.library.hardware.sensor.IMU.AxesSigns; import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Loggable; import com.technototes.vision.hardware.Webcam; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.BrakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import java.util.List; +import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; public class Hardware implements Loggable { - @com.acmerobotics.dashboard.config.Config - public static class HardwareConstants { - - public static String LIFT = "lift"; - - public static String DUMP = "dump"; - public static String ARM = "arm"; - - public static String SLIDE = "slide"; - public static String TURRET = "turret"; - - public static String FL_MOTOR = "flmotor"; - public static String FR_MOTOR = "frmotor"; - public static String RL_MOTOR = "rlmotor"; - public static String RR_MOTOR = "rrmotor"; - public static String IMU = "imu"; - public static String L_RANGE = "ldistance"; - public static String R_RANGE = "rdistance"; - public static String F_RANGE = "fdistance"; - - public static String CAROUSEL = "carousel"; - - public static String CAMERA = "webcam"; - - public static String INTAKE = "intake"; - public static String INTAKE_COLOR = "irange"; - - public static String CAP = "cap"; - - public static String BRAKE = "brake"; - } - - public EncodedMotor liftMotor; - - public Servo dumpServo; - public Servo armServo; + public List hubs; - public Servo turretServo; - public Servo slideServo; + public EncodedMotor theMotor, flMotor, frMotor, rlMotor, rrMotor, arm; + public Motor placeholder1; + public DcMotorEx liftMotor; - public EncodedMotor flDriveMotor; - public EncodedMotor frDriveMotor; - public EncodedMotor rlDriveMotor; - public EncodedMotor rrDriveMotor; - public static IMU imu; - public Rev2MDistanceSensor leftRangeSensor; - public Rev2MDistanceSensor rightRangeSensor; - public Rev2MDistanceSensor frontRangeSensor; - - public Motor intakeMotor; - public ColorDistanceSensor intakeSensor; - - public Motor carouselMotor; - - public Servo capServo; + public Servo placeholder2; + public Servo servo, clawServo; + public IGyro imu; public Webcam camera; - - public Servo brake; - - public Servo capLeftArmServo; - public Servo capRightArmServo; - public Servo capArmServos; - - public Servo capClawServo; - public Servo capTurretServo; - - public Hardware() { - if (BRAKE_ENABLED) { - brake = new Servo(BRAKE).startAt(BrakeSubsystem.BrakeConstants.UP); - } - if (LIFT_ENABLED) { - liftMotor = new EncodedMotor(LIFT).brake().tare(); + public Rev2MDistanceSensor distanceSensor; + public ColorDistanceSensor colorSensor; + + public Hardware(HardwareMap hwmap) { + hubs = hwmap.getAll(LynxModule.class); + if (Setup.Connected.EXTERNAL_IMU) { + imu = new AdafruitIMU(Setup.HardwareNames.EXTERNAL_IMU, AdafruitIMU.Orientation.Pitch); + Setup.HardwareNames.MOTOR = "External IMU is in use"; + } else { + imu = new IMU( + Setup.HardwareNames.IMU, + RevHubOrientationOnRobot.LogoFacingDirection.UP, + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD + ); + Setup.HardwareNames.MOTOR = "Internal IMU is being used"; } - if (ARM_ENABLED) { - dumpServo = new Servo(DUMP).invert().startAt(ArmSubsystem.ArmConstants.CARRY); - armServo = new Servo(ARM).startAt(ArmSubsystem.ArmConstants.UP); + if (Setup.Connected.DRIVEBASE) { + this.frMotor = new EncodedMotor<>(Setup.HardwareNames.FRMOTOR); + this.flMotor = new EncodedMotor<>(Setup.HardwareNames.FLMOTOR); + this.rrMotor = new EncodedMotor<>(Setup.HardwareNames.RRMOTOR); + this.rlMotor = new EncodedMotor<>(Setup.HardwareNames.RLMOTOR); + if (Setup.Connected.DISTANCE_SENSOR) { + this.distanceSensor = new Rev2MDistanceSensor(Setup.HardwareNames.DISTANCE); + } } - if (EXTENSION_ENABLED) { - slideServo = new Servo(SLIDE).startAt(ExtensionSubsystem.ExtensionConstants.IN); - turretServo = new Servo(TURRET) - .startAt(ExtensionSubsystem.ExtensionConstants.CENTER) - .expandedRange(); + if (Setup.Connected.MOTOR) { + this.theMotor = new EncodedMotor<>(Setup.HardwareNames.MOTOR); } - if (DRIVE_ENABLED) { - flDriveMotor = new EncodedMotor<>(FL_MOTOR); - frDriveMotor = new EncodedMotor<>(FR_MOTOR); - rlDriveMotor = new EncodedMotor<>(RL_MOTOR); - rrDriveMotor = new EncodedMotor<>(RR_MOTOR); - imu = new IMU( - IMU, - RevHubOrientationOnRobot.LogoFacingDirection.UP, - RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD - ).remapAxesAndSigns(AxesOrder.YXZ, AxesSigns.NPP); - leftRangeSensor = new Rev2MDistanceSensor(L_RANGE).onUnit(DistanceUnit.INCH); - rightRangeSensor = new Rev2MDistanceSensor(R_RANGE).onUnit(DistanceUnit.INCH); - frontRangeSensor = new Rev2MDistanceSensor(F_RANGE).onUnit(DistanceUnit.INCH); + if (Setup.Connected.FLYWHEEL) { + this.placeholder1 = new Motor<>(Setup.HardwareNames.FLYWHEELMOTOR); } - if (CAROUSEL_ENABLED) { - carouselMotor = new Motor(CAROUSEL).brake(); + if (Setup.Connected.WEBCAM) { + camera = new Webcam(Setup.HardwareNames.CAMERA); } - if (VISION_ENABLED) { - camera = new Webcam(CAMERA); + if (Setup.Connected.TESTSUBSYSTEM) { + if (Setup.Connected.SERVO) { + this.servo = new Servo(Setup.HardwareNames.SERVO); + } + // if (Setup.Connected.COLOR_SENSOR) { + // this.colorSensor = new ColorDistanceSensor(Setup.HardwareNames.COLOR); + // } } - if (INTAKE_ENABLED) { - intakeMotor = new Motor<>(INTAKE); - intakeSensor = new ColorDistanceSensor(INTAKE_COLOR).onUnit(DistanceUnit.INCH); + if (Setup.Connected.CLAWSUBSYSTEM) { + this.clawServo = new Servo(Setup.HardwareNames.CLAWSERVO); + this.arm = new EncodedMotor<>(Setup.HardwareNames.ARM); } - if (CAP_ENABLED) { - capLeftArmServo = new Servo("caparml") - .onRange(0.25, 0.65) - .invert() - .startAt(CapSubsystem.CapConstants.ARM_INIT); - capRightArmServo = new Servo("caparmr") - .onRange(0.35, 0.75) - .startAt(CapSubsystem.CapConstants.ARM_INIT); + } - capClawServo = new Servo("capclaw").startAt(CapSubsystem.CapConstants.CLAW_CLOSE); - capTurretServo = new Servo("capturr").startAt(CapSubsystem.CapConstants.TURRET_INIT); + // We can read the voltage from the different hubs for fun... + public double voltage() { + double volt = 0; + double count = 0; + for (LynxModule lm : hubs) { + count += 1; + volt += lm.getInputVoltage(VoltageUnit.VOLTS); } + return volt / count; } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java index fb1fc8e0..0a34a722 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java @@ -1,103 +1,33 @@ package org.firstinspires.ftc.ptechnodactyl; -import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.*; - -import com.acmerobotics.dashboard.config.Config; -import com.technototes.library.logger.Log; -import com.technototes.library.logger.LogConfig; import com.technototes.library.logger.Loggable; -import com.technototes.library.util.Color; -import org.firstinspires.ftc.ptechnodactyl.opmodes.tele.TeleOpBase; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.BrakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; +import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; public class Robot implements Loggable { - @Config - public static class SubsystemConstants { - - public static boolean LIFT_ENABLED = true; - public static boolean ARM_ENABLED = true; - public static boolean EXTENSION_ENABLED = true; - public static boolean DRIVE_ENABLED = true; - public static boolean CAROUSEL_ENABLED = true; - public static boolean INTAKE_ENABLED = true; - public static boolean VISION_ENABLED = true; - public static boolean CAP_ENABLED = true; - public static boolean SPEAKER_ENABLED = true; - public static boolean BRAKE_ENABLED = true; - } - - @Log.Number(name = "Lift") - public LiftSubsystem liftSubsystem; - - @Log(name = "Deposit") - public ArmSubsystem armSubsystem; - - @Log(name = "Extension") - public ExtensionSubsystem extensionSubsystem; - - @LogConfig.DenyList(TeleOpBase.class) - @Log(name = "Drivebase") + public StartingPosition position; + public Alliance alliance; + public double initialVoltage; public DrivebaseSubsystem drivebaseSubsystem; - - @Log.Number(name = "Carousel") - public CarouselSubsystem carouselSubsystem; - - @Log(name = "Intake") - public IntakeSubsystem intakeSubsystem; - - @Log(name = "Cap") - public CapSubsystem capSubsystem; - - public VisionSubsystem visionSubsystem; - - @Log.Boolean(name = "Brake", trueValue = "ACTIVE", falseValue = "INACTIVE", index = 0) - public BrakeSubsystem brakeSubsystem; - - public Robot(Hardware hardware) { - if (BRAKE_ENABLED) brakeSubsystem = new BrakeSubsystem(hardware.brake); - - if (LIFT_ENABLED) liftSubsystem = new LiftSubsystem(hardware.liftMotor); - - if (ARM_ENABLED) armSubsystem = new ArmSubsystem(hardware.dumpServo, hardware.armServo); - - if (EXTENSION_ENABLED) extensionSubsystem = new ExtensionSubsystem( - hardware.slideServo, - hardware.turretServo - ); - - if (DRIVE_ENABLED) drivebaseSubsystem = new DrivebaseSubsystem( - hardware.flDriveMotor, - hardware.frDriveMotor, - hardware.rlDriveMotor, - hardware.rrDriveMotor, - hardware.imu, - hardware.leftRangeSensor, - hardware.rightRangeSensor, - hardware.frontRangeSensor - ); - - if (CAROUSEL_ENABLED) carouselSubsystem = new CarouselSubsystem(hardware.carouselMotor); - - if (INTAKE_ENABLED) intakeSubsystem = new IntakeSubsystem( - hardware.intakeMotor, - hardware.intakeSensor - ); - - if (VISION_ENABLED) visionSubsystem = new VisionSubsystem(hardware.camera); - - if (CAP_ENABLED) capSubsystem = new CapSubsystem( - hardware.capArmServos, - hardware.capClawServo, - hardware.capTurretServo - ); + public ClawSubsystem clawSubsystem; + + public Robot(Hardware hw) { + this.initialVoltage = hw.voltage(); + if (Setup.Connected.DRIVEBASE) { + this.drivebaseSubsystem = new DrivebaseSubsystem( + hw.flMotor, + hw.frMotor, + hw.rlMotor, + hw.rrMotor, + hw.imu + ); + } + if (Setup.Connected.CLAWSUBSYSTEM) { + this.clawSubsystem = new ClawSubsystem(hw); + } } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java deleted file mode 100644 index 7acfe50c..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java +++ /dev/null @@ -1,271 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl; - -import static java.lang.Math.toRadians; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.technototes.library.util.Alliance; -import com.technototes.path.geometry.ConfigurablePose; -import com.technototes.path.trajectorysequence.TrajectorySequence; -import com.technototes.path.trajectorysequence.TrajectorySequenceBuilder; -import java.util.function.BiFunction; -import java.util.function.Function; -import java.util.function.Supplier; - -public class RobotConstants { - - @Config - public static class AutoRedConstants { - - public static ConfigurablePose CYCLE_START = new ConfigurablePose(12, -63, toRadians(90)); - public static ConfigurablePose ALLIANCE_HUB = new ConfigurablePose(7, -52, toRadians(125)); - public static ConfigurablePose CYCLE_TRENCH = new ConfigurablePose(24, -64, toRadians(180)); - public static ConfigurablePose CYCLE_INTERMEDIATE = new ConfigurablePose( - 42, - -64, - toRadians(180) - ); - public static ConfigurablePose[] AUTO_WAREHOUSE = new ConfigurablePose[] { - new ConfigurablePose(44, -64, toRadians(190)), - new ConfigurablePose(46.5, -64, toRadians(190)), - new ConfigurablePose(49, -64, toRadians(190)), - new ConfigurablePose(51.5, -64, toRadians(190)), - new ConfigurablePose(54, -64, toRadians(190)), - }; - - public static ConfigurablePose DUCK_START = new ConfigurablePose(-36, -63, toRadians(90)); - public static ConfigurablePose DUCK_HUB = new ConfigurablePose(-32, -52, toRadians(55)); - public static ConfigurablePose CAROUSEL = new ConfigurablePose(-61, -59, toRadians(0)); - public static ConfigurablePose DUCK_INTAKE_START = new ConfigurablePose( - -30, - -58, - toRadians(145) - ); - public static ConfigurablePose DUCK_INTAKE_END = new ConfigurablePose( - -60, - -61, - toRadians(35) - ); - public static ConfigurablePose SQUARE = new ConfigurablePose(-67, -36, toRadians(0)); - public static ConfigurablePose BARRIER_PARK = new ConfigurablePose(60, -30, toRadians(0)); - - public static ConfigurablePose SHARED_TRENCH = new ConfigurablePose(64, -23, toRadians(90)); - public static ConfigurablePose SHARED_HUB = new ConfigurablePose(64, -17, toRadians(100)); - public static ConfigurablePose SHARED_INTAKE = new ConfigurablePose(64, -50, toRadians(85)); - } - - @Config - public static class AutoBlueConstants { - - public static ConfigurablePose CYCLE_START = new ConfigurablePose(12, 63, toRadians(-90)); - public static ConfigurablePose ALLIANCE_HUB = new ConfigurablePose(7, 52, toRadians(-125)); - public static ConfigurablePose CYCLE_TRENCH = new ConfigurablePose(24, 64, toRadians(-180)); - public static ConfigurablePose CYCLE_INTERMEDIATE = new ConfigurablePose( - 42, - 64, - toRadians(-180) - ); - public static ConfigurablePose[] AUTO_WAREHOUSE = new ConfigurablePose[] { - new ConfigurablePose(44, 64, toRadians(-190)), - new ConfigurablePose(46.5, 64, toRadians(-190)), - new ConfigurablePose(49, 64, toRadians(-190)), - new ConfigurablePose(51.5, 64, toRadians(-190)), - new ConfigurablePose(54, 64, toRadians(-190)), - }; - - public static ConfigurablePose DUCK_START = new ConfigurablePose(-36, 63, toRadians(-90)); - public static ConfigurablePose DUCK_HUB = new ConfigurablePose(-32, 52, toRadians(-55)); - public static ConfigurablePose CAROUSEL = new ConfigurablePose(-61, 59, toRadians(-90)); - public static ConfigurablePose DUCK_INTAKE_START = new ConfigurablePose( - -30, - 58, - toRadians(-145) - ); - public static ConfigurablePose DUCK_INTAKE_END = new ConfigurablePose( - -60, - 61, - toRadians(-35) - ); - public static ConfigurablePose SQUARE = new ConfigurablePose(-67, 36, toRadians(0)); - public static ConfigurablePose BARRIER_PARK = new ConfigurablePose(60, 30, toRadians(0)); - - public static ConfigurablePose SHARED_TRENCH = new ConfigurablePose(64, 23, toRadians(-90)); - public static ConfigurablePose SHARED_HUB = new ConfigurablePose(64, 17, toRadians(-90)); - public static ConfigurablePose SHARED_INTAKE = new ConfigurablePose(64, 50, toRadians(-90)); - } - - private static Alliance alliance = Alliance.BLUE; - - public static void updateAlliance(Alliance i) { - alliance = i; - } - - public static Alliance getAlliance() { - return alliance; - } - - public static final Supplier CYCLE_START_SELECT = () -> - alliance - .selectOf(AutoRedConstants.CYCLE_START, AutoBlueConstants.CYCLE_START) - .toPose(), ALLIANCE_HUB_SELECT = () -> - alliance - .selectOf(AutoRedConstants.ALLIANCE_HUB, AutoBlueConstants.ALLIANCE_HUB) - .toPose(), SHARED_HUB_SELECT = () -> - alliance - .selectOf(AutoRedConstants.SHARED_HUB, AutoBlueConstants.SHARED_HUB) - .toPose(), SHARED_INTAKE_SELECT = () -> - alliance - .selectOf(AutoRedConstants.SHARED_INTAKE, AutoBlueConstants.SHARED_INTAKE) - .toPose(), ALLIANCE_TRENCH_SELECT = () -> - alliance - .selectOf(AutoRedConstants.CYCLE_TRENCH, AutoBlueConstants.CYCLE_TRENCH) - .toPose(), CYCLE_INTERMEDIATE_SELECT = () -> - alliance - .selectOf(AutoRedConstants.CYCLE_INTERMEDIATE, AutoBlueConstants.CYCLE_INTERMEDIATE) - .toPose(), SHARED_TRENCH_SELECT = () -> - alliance - .selectOf(AutoRedConstants.SHARED_TRENCH, AutoBlueConstants.SHARED_TRENCH) - .toPose(), DUCK_START_SELECT = () -> - alliance - .selectOf(AutoRedConstants.DUCK_START, AutoBlueConstants.DUCK_START) - .toPose(), DUCK_ALLIANCE_HUB_SELECT = () -> - alliance - .selectOf(AutoRedConstants.DUCK_HUB, AutoBlueConstants.DUCK_HUB) - .toPose(), CAROUSEL_SELECT = () -> - alliance - .selectOf(AutoRedConstants.CAROUSEL, AutoBlueConstants.CAROUSEL) - .toPose(), DUCK_INTAKE_START_SELECT = () -> - alliance - .selectOf(AutoRedConstants.DUCK_INTAKE_START, AutoBlueConstants.DUCK_INTAKE_START) - .toPose(), DUCK_INTAKE_END_SELECT = () -> - alliance - .selectOf(AutoRedConstants.DUCK_INTAKE_END, AutoBlueConstants.DUCK_INTAKE_END) - .toPose(), SQUARE_SELECT = () -> - alliance - .selectOf(AutoRedConstants.SQUARE, AutoBlueConstants.SQUARE) - .toPose(), BARRIER_SELECT = () -> - alliance.selectOf(AutoRedConstants.BARRIER_PARK, AutoBlueConstants.BARRIER_PARK).toPose(); - - public static final Function CYCLE_INTAKE_SELECT = i -> - alliance - .selectOf(AutoRedConstants.AUTO_WAREHOUSE[i], AutoBlueConstants.AUTO_WAREHOUSE[i]) - .toPose(); - - public static final Function< - Function, - TrajectorySequence - > CYCLE_DEPOSIT_PRELOAD = b -> - b - .apply(CYCLE_START_SELECT.get()) - .setAccelConstraint((a, e, c, d) -> 30) - .lineToLinearHeading(ALLIANCE_HUB_SELECT.get()) - .build(), DUCK_DEPOSIT_PRELOAD = b -> - b - .apply(DUCK_START_SELECT.get()) - .setAccelConstraint((a, e, c, d) -> 30) - .lineToLinearHeading(DUCK_ALLIANCE_HUB_SELECT.get()) - .build(), HUB_TO_CAROUSEL = b -> - b - .apply(DUCK_ALLIANCE_HUB_SELECT.get()) - .setAccelConstraint((a, e, c, d) -> 30) - .lineToLinearHeading(CAROUSEL_SELECT.get()) - .build(), HUB_TO_SQUARE = b -> - b - .apply(DUCK_ALLIANCE_HUB_SELECT.get()) - .setAccelConstraint((a, e, c, d) -> 30) - .lineToLinearHeading(SQUARE_SELECT.get()) - .build(), HUB_BARRIER_PARK = b -> - b - .apply(DUCK_ALLIANCE_HUB_SELECT.get()) - .turn(BARRIER_SELECT.get().getHeading() - DUCK_ALLIANCE_HUB_SELECT.get().getHeading()) - .setVelConstraint((a, e, c, d) -> 100) - .lineTo(BARRIER_SELECT.get().vec()) - .build(), HUB_TO_PARK = b -> - b - .apply(ALLIANCE_HUB_SELECT.get()) - .setReversed(true) - .setAccelConstraint((a, e, c, d) -> 30) - .splineTo(ALLIANCE_TRENCH_SELECT.get().vec(), 0) - // .setAccelConstraint((a, e, c, d) -> 100) - // .setVelConstraint((a, e, c, d)->70) - .lineToSplineHeading(CYCLE_INTERMEDIATE_SELECT.get()) - .build(), DUCK_INTAKE_TO_HUB = b -> - b - .apply(DUCK_INTAKE_END_SELECT.get()) - .setAccelConstraint((a, e, c, d) -> 30) - .lineToLinearHeading(DUCK_ALLIANCE_HUB_SELECT.get()) - .build(), SHARED_HUB_TO_WAREHOUSE = b -> - b - .apply(SHARED_HUB_SELECT.get()) - .setReversed(true) - .setAccelConstraint((a, e, c, d) -> 30) - .splineTo( - SHARED_TRENCH_SELECT.get().vec(), - toRadians(RobotConstants.getAlliance().selectOf(-90, 90)) - ) - // .setAccelConstraint((a, e, c, d) -> 100) - // .setVelConstraint((a, e, c, d)->70) - .lineToSplineHeading(SHARED_INTAKE_SELECT.get()) - .build(), CAROUSEL_TO_DUCK_INTAKE = b -> - b - .apply(CAROUSEL_SELECT.get()) - .setAccelConstraint((a, e, c, d) -> 20) - .setVelConstraint((a, e, c, d) -> 30) - .turn(DUCK_INTAKE_START_SELECT.get().getHeading() - CAROUSEL_SELECT.get().getHeading()) - .lineToLinearHeading(DUCK_INTAKE_START_SELECT.get()) - .turn( - DUCK_INTAKE_END_SELECT.get().getHeading() - - DUCK_INTAKE_START_SELECT.get().getHeading() - ) - .lineToLinearHeading(DUCK_INTAKE_END_SELECT.get()) - .build(); - - public static final BiFunction< - Function, - Integer, - TrajectorySequence - > HUB_TO_WAREHOUSE = (b, i) -> - b - .apply(ALLIANCE_HUB_SELECT.get()) - .setReversed(true) - .setAccelConstraint((a, e, c, d) -> 30) - .splineTo(ALLIANCE_TRENCH_SELECT.get().vec(), 0) - .setAccelConstraint((a, e, c, d) -> 60) - .lineToSplineHeading(CYCLE_INTERMEDIATE_SELECT.get()) - .setVelConstraint((a, e, c, d) -> 20) - .lineToSplineHeading(CYCLE_INTAKE_SELECT.apply(i)) - .build(); - - public static final BiFunction< - Function, - Supplier, - TrajectorySequence - > WAREHOUSE_TO_HUB = (b, p) -> - b - .apply( - new Pose2d( - Math.max(p.get().getX(), ALLIANCE_TRENCH_SELECT.get().getX() + 1), - ALLIANCE_TRENCH_SELECT.get().getY(), - // p.get().getY(), - p.get().getHeading() - ) - ) - // .setTurnConstraint(toRadians(30), toRadians(30)) - .lineToSplineHeading(ALLIANCE_TRENCH_SELECT.get()) - // .splineToSplineHeading(ALLIANCE_TRENCH_SELECT.get(), toRadians(180)) - .setAccelConstraint((a, e, c, d) -> 30) - .splineTo(ALLIANCE_HUB_SELECT.get().vec(), ALLIANCE_HUB_SELECT.get().getHeading()) - .build(), WAREHOUSE_TO_SHARED_HUB = (b, p) -> - b - .apply( - new Pose2d( - p.get().getX(), - // p.get().getY(), - SHARED_TRENCH_SELECT.get().getY(), - p.get().getHeading() - ) - ) - .lineToSplineHeading(SHARED_TRENCH_SELECT.get()) - .splineToSplineHeading(SHARED_HUB_SELECT.get(), SHARED_HUB_SELECT.get().getHeading()) - .build(); -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotState.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotState.java deleted file mode 100644 index 97e1120f..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotState.java +++ /dev/null @@ -1,78 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl; - -import com.acmerobotics.dashboard.config.Config; - -public class RobotState { - - private static AllianceHubStrategy allianceHubStrategy; - private static SharedHubStrategy sharedHubStrategy; - - public static AllianceHubStrategy getAllianceStrategy() { - return allianceHubStrategy; - } - - public static SharedHubStrategy getSharedStrategy() { - return sharedHubStrategy; - } - - public static void strategy1() { - setStrategy(AllianceHubStrategy.HIGH, SharedHubStrategy.OWN); - } - - public static void strategy2() { - setStrategy(AllianceHubStrategy.MID, SharedHubStrategy.STEAL); - } - - public static void setStrategy( - AllianceHubStrategy allianceHubStrategy, - SharedHubStrategy sharedHubStrategy - ) { - RobotState.allianceHubStrategy = allianceHubStrategy; - RobotState.sharedHubStrategy = sharedHubStrategy; - } - - public enum AllianceHubStrategy { - HIGH, - MID, - } - - public enum SharedHubStrategy { - OWN, - STEAL, - } - - private static boolean depositTarget = true; - - public static void startDeposit() { - depositTarget = true; - } - - public static void stopDeposit() { - depositTarget = false; - } - - public static boolean isDepositing() { - return depositTarget; - } - - public enum Freight { - CUBE, - BALL, - DUCK, - NONE, - } - - private static Freight freight = Freight.NONE; - - public static void setFreight(Freight f) { - freight = f; - } - - public static Freight getFreight() { - return freight; - } - - public static boolean hasFreight() { - return freight != Freight.NONE; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java new file mode 100644 index 00000000..6d90a265 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.ptechnodactyl.commands; + +import com.qualcomm.robotcore.util.ElapsedTime; +import com.technototes.library.command.Command; +import com.technototes.library.control.Stick; +import com.technototes.library.logger.Loggable; +import java.util.function.DoubleSupplier; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; + +public class JoystickIncDecCmd implements Command, Loggable { + + public ClawSubsystem subsystem; + public DoubleSupplier x; + public static ElapsedTime timer; + + public static double time; + + public double interval = 1.5; + + public JoystickIncDecCmd(ClawSubsystem sub, Stick xStick) { + addRequirements(sub); + subsystem = sub; + x = xStick.getXSupplier(); + } + + // This will make the bot snap to an angle, if the 'straighten' button is pressed + // Otherwise, it just reads the rotation value from the rotation stick + + @Override + public void execute() { + double xvalue = -x.getAsDouble(); + // time = timer.time(); + // if (interval < time) + subsystem.increment(xvalue); + // timer.reset(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmAllianceCommand.java deleted file mode 100644 index ac50008e..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmAllianceCommand.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; - -public class ArmAllianceCommand extends ArmCommand { - - public ArmAllianceCommand(ArmSubsystem s) { - super(s); - } - - @Override - public void execute() { - subsystem.out(); - subsystem.carry(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmBarcodeSelectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmBarcodeSelectCommand.java deleted file mode 100644 index 96dcf7ae..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmBarcodeSelectCommand.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; - -public class ArmBarcodeSelectCommand extends ArmCommand { - - public VisionSubsystem visionSubsystem; - - public ArmBarcodeSelectCommand(ArmSubsystem s, VisionSubsystem v) { - super(s); - visionSubsystem = v; - } - - @Override - public void execute() { - subsystem.carry(); - if (visionSubsystem.barcodePipeline.zero()) subsystem.down(); - else subsystem.out(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmCommand.java deleted file mode 100644 index 9741f8f1..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public abstract class ArmCommand implements Command { - - public ArmSubsystem subsystem; - - public ArmCommand(ArmSubsystem arm) { - subsystem = arm; - addRequirements(arm); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.5; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmInCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmInCommand.java deleted file mode 100644 index 31e630d0..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmInCommand.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public class ArmInCommand extends ArmCommand { - - public ArmInCommand(ArmSubsystem s) { - super(s); - } - - @Override - public void execute() { - subsystem.in(); - subsystem.collect(); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.4; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseCommand.java deleted file mode 100644 index 195c8949..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseCommand.java +++ /dev/null @@ -1,16 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public class ArmRaiseCommand extends ArmCommand { - - public ArmRaiseCommand(ArmSubsystem s) { - super(s); - } - - @Override - public void execute() { - subsystem.carry(); - subsystem.up(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseInCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseInCommand.java deleted file mode 100644 index 4df1dace..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseInCommand.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public class ArmRaiseInCommand extends ArmCommand { - - public ArmRaiseInCommand(ArmSubsystem s) { - super(s); - } - - @Override - public void execute() { - if (getRuntime().seconds() > 0.2) subsystem.fakeCarry(); - subsystem.up(); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.3; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmSharedCommand.java deleted file mode 100644 index c6f19213..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmSharedCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public class ArmSharedCommand extends ArmCommand { - - public ArmSharedCommand(ArmSubsystem s) { - super(s); - } - - @Override - public void execute() { - subsystem.down(); - subsystem.fakeCarry(); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.5; - } - - @Override - public void end(boolean cancel) { - if (!cancel) subsystem.carry(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmTranslateCommand.java deleted file mode 100644 index 6b435d2f..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmTranslateCommand.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public class ArmTranslateCommand implements Command { - - public ArmSubsystem subsystem; - public double amount; - - public ArmTranslateCommand(ArmSubsystem s, double amt) { - subsystem = s; - amount = amt; - addRequirements(s); - } - - @Override - public void initialize() { - subsystem.translateArm(amount); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.2; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCarryCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCarryCommand.java deleted file mode 100644 index c00d95f5..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCarryCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public class BucketCarryCommand implements Command { - - public ArmSubsystem depositSubsystem; - - public BucketCarryCommand(ArmSubsystem s) { - depositSubsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - depositSubsystem.setDump(ArmSubsystem.ArmConstants.CARRY); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCollectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCollectCommand.java deleted file mode 100644 index 8c5b1c55..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCollectCommand.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public class BucketCollectCommand implements Command { - - public ArmSubsystem depositSubsystem; - - public BucketCollectCommand(ArmSubsystem s) { - depositSubsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - depositSubsystem.setDump(ArmSubsystem.ArmConstants.COLLECT); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.5; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpCommand.java deleted file mode 100644 index 69e7714a..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpCommand.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public class BucketDumpCommand implements Command { - - public ArmSubsystem depositSubsystem; - - public BucketDumpCommand(ArmSubsystem s) { - depositSubsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - depositSubsystem.dump(); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.3; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpVariableCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpVariableCommand.java deleted file mode 100644 index 2a52a9e8..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpVariableCommand.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.arm; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.*; - -import com.technototes.library.command.Command; -import java.util.function.DoubleSupplier; -import org.firstinspires.ftc.ptechnodactyl.subsystems.*; - -public class BucketDumpVariableCommand implements Command { - - public ArmSubsystem subsystem; - public DoubleSupplier supplier; - - public BucketDumpVariableCommand(ArmSubsystem s, DoubleSupplier pos) { - subsystem = s; - supplier = pos; - addRequirements(s); - } - - public BucketDumpVariableCommand(ArmSubsystem s, double pos) { - this(s, () -> pos); - } - - @Override - public void execute() { - subsystem.setDump(((supplier.getAsDouble() - 0.2) * (DUMP - AUTO_CARRY)) + AUTO_CARRY); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean cancel) { - subsystem.setDump(AUTO_CARRY); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCarouselCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCarouselCommand.java deleted file mode 100644 index eab077ac..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCarouselCommand.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.carousel.CarouselSpinCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class AutoCarouselCommand extends SequentialCommandGroup { - - public AutoCarouselCommand( - DrivebaseSubsystem drive, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension, - CarouselSubsystem carousel - ) { - super( - new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_CAROUSEL).alongWith( - new DepositCollectCommand(deposit, extension, lift) - ), - new CarouselSpinCommand(carousel).withTimeout(4) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCycleCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCycleCommand.java deleted file mode 100644 index faa599ed..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCycleCommand.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.acmerobotics.dashboard.config.Config; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.IterativeCommand; -import com.technototes.library.command.SequentialCommandGroup; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; - -@Config -public class AutoCycleCommand extends SequentialCommandGroup { - - public static int MAX_AUTO_CYCLES = 5; - public static int TIME_TO_STOP = 30; - - public AutoCycleCommand( - DrivebaseSubsystem drive, - IntakeSubsystem intake, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension, - VisionSubsystem vision - ) { - super( - () -> drive.setPoseEstimate(RobotConstants.CYCLE_START_SELECT.get()), - () -> - drive.distanceSensorLocalizer.setGyroOffset( - -RobotConstants.CYCLE_START_SELECT.get().getHeading() - ), - drive::relocalize, - new AutoCyclePreloadCommand(drive, deposit, extension, lift, vision), - new IterativeCommand( - i -> - new AutoIntakeWarehouseCommand(drive, intake, lift, deposit, extension, i) - .andThen( - new AutoDepositAllianceCommand(drive, intake, lift, deposit, extension) - ) - .onlyIf(() -> CommandScheduler.getOpModeRuntime() < TIME_TO_STOP), - MAX_AUTO_CYCLES - ), - new AutoParkWarehouseCommand(drive, lift, deposit, extension), - CommandScheduler::terminateOpMode - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCyclePreloadCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCyclePreloadCommand.java deleted file mode 100644 index a2d35cb6..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCyclePreloadCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositPreloadCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; - -public class AutoCyclePreloadCommand extends SequentialCommandGroup { - - public AutoCyclePreloadCommand( - DrivebaseSubsystem drive, - ArmSubsystem depot, - ExtensionSubsystem extension, - LiftSubsystem lift, - VisionSubsystem vision - ) { - super( - new TrajectorySequenceCommand( - drive, - RobotConstants.CYCLE_DEPOSIT_PRELOAD - )//.alongWith(new LiftBarcodeSelectCommand(lift, vision) - .alongWith(new DepositPreloadCommand(depot, extension, lift, vision)), - new BucketDumpCommand(depot) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositAllianceCommand.java deleted file mode 100644 index edbb925c..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositAllianceCommand.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.path.command.RegenerativeTrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCycleAllianceCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -@com.acmerobotics.dashboard.config.Config -public class AutoDepositAllianceCommand extends SequentialCommandGroup { - - public static double TIME = 1; - - public AutoDepositAllianceCommand( - DrivebaseSubsystem drive, - IntakeSubsystem intake, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension - ) { - super( - drive::relocalize, - new RegenerativeTrajectorySequenceCommand( - drive, - RobotConstants.WAREHOUSE_TO_HUB, - drive - ).alongWith( - deposit::slightCarry, - new WaitCommand(0.3) - .andThen(new IntakeOutCommand(intake)) - .withTimeout(TIME) - .andThen(new DepositCycleAllianceCommand(deposit, extension, lift)) - ), - new BucketDumpCommand(deposit) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositDuckCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositDuckCommand.java deleted file mode 100644 index 90c706d5..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositDuckCommand.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositAllianceCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class AutoDepositDuckCommand extends SequentialCommandGroup { - - public AutoDepositDuckCommand( - DrivebaseSubsystem drive, - ArmSubsystem depot, - ExtensionSubsystem extension, - LiftSubsystem lift, - IntakeSubsystem intake - ) { - super( - new TrajectorySequenceCommand(drive, RobotConstants.DUCK_INTAKE_TO_HUB).alongWith( - new WaitCommand(0.3).andThen(new IntakeOutCommand(intake)).withTimeout(0.6), - depot::slightCarry, - new WaitCommand(0.3).andThen(new DepositAllianceCommand(depot, extension, lift)) - ), - new BucketDumpCommand(depot).sleep(0.3) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckCommand.java deleted file mode 100644 index 7ae11190..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckCommand.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.SequentialCommandGroup; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; - -public class AutoDuckCommand extends SequentialCommandGroup { - - public AutoDuckCommand( - DrivebaseSubsystem drive, - IntakeSubsystem intake, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension, - VisionSubsystem vision, - CarouselSubsystem carousel - ) { - super( - () -> drive.setPoseEstimate(RobotConstants.DUCK_START_SELECT.get()), - () -> - drive.distanceSensorLocalizer.setGyroOffset( - -RobotConstants.DUCK_START_SELECT.get().getHeading() - ), - //drive::relocalize, - new AutoDuckPreloadCommand(drive, deposit, extension, lift, vision), - new AutoCarouselCommand(drive, lift, deposit, extension, carousel), - new AutoIntakeDuckCommand(drive, intake), - new AutoDepositDuckCommand(drive, deposit, extension, lift, intake), - new AutoParkBarrierCommand(drive, lift, deposit, extension), - CommandScheduler::terminateOpMode - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckPreloadCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckPreloadCommand.java deleted file mode 100644 index bcf61202..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckPreloadCommand.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositPreloadCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; - -public class AutoDuckPreloadCommand extends SequentialCommandGroup { - - public AutoDuckPreloadCommand( - DrivebaseSubsystem drive, - ArmSubsystem depot, - ExtensionSubsystem extension, - LiftSubsystem lift, - VisionSubsystem vision - ) { - super( - new TrajectorySequenceCommand(drive, RobotConstants.DUCK_DEPOSIT_PRELOAD).alongWith( - new DepositPreloadCommand(depot, extension, lift, vision) - ), - new BucketDumpCommand(depot) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeDuckCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeDuckCommand.java deleted file mode 100644 index 3c3ae489..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeDuckCommand.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; - -public class AutoIntakeDuckCommand extends SequentialCommandGroup { - - public AutoIntakeDuckCommand(DrivebaseSubsystem drive, IntakeSubsystem intake) { - super( - //drive::relocalize, - new TrajectorySequenceCommand(drive, RobotConstants.CAROUSEL_TO_DUCK_INTAKE).alongWith( - new IntakeInCommand(intake) - ), - new WaitCommand(1) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeWarehouseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeWarehouseCommand.java deleted file mode 100644 index 23cd5757..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeWarehouseCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeSafeCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class AutoIntakeWarehouseCommand extends SequentialCommandGroup { - - public AutoIntakeWarehouseCommand( - DrivebaseSubsystem drive, - IntakeSubsystem intake, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension, - int cycle - ) { - super( - new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_WAREHOUSE, cycle) - .alongWith(new WaitCommand(1.4).andThen(new IntakeInCommand(intake))) - .alongWith(new DepositCollectCommand(deposit, extension, lift)) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkBarrierCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkBarrierCommand.java deleted file mode 100644 index ed05c062..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkBarrierCommand.java +++ /dev/null @@ -1,28 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.ConditionalCommand; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class AutoParkBarrierCommand extends SequentialCommandGroup { - - public AutoParkBarrierCommand( - DrivebaseSubsystem drive, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension - ) { - super( - new DepositCollectCommand(deposit, extension, lift), - new ConditionalCommand(() -> CommandScheduler.getOpModeRuntime() > 26), - new TrajectorySequenceCommand(drive, RobotConstants.HUB_BARRIER_PARK) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkSquareCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkSquareCommand.java deleted file mode 100644 index e84e511e..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkSquareCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class AutoParkSquareCommand extends SequentialCommandGroup { - - public AutoParkSquareCommand( - DrivebaseSubsystem drive, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension - ) { - super( - new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_SQUARE).alongWith( - new DepositCollectCommand(deposit, extension, lift) - ) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkWarehouseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkWarehouseCommand.java deleted file mode 100644 index 956cccad..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkWarehouseCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class AutoParkWarehouseCommand extends SequentialCommandGroup { - - public AutoParkWarehouseCommand( - DrivebaseSubsystem drive, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension - ) { - super( - new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_PARK).alongWith( - new DepositCollectCommand(deposit, extension, lift) - ) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositAllianceCommand.java deleted file mode 100644 index 92204213..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositAllianceCommand.java +++ /dev/null @@ -1,57 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.path.command.RegenerativeTrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.RobotState; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositAllianceCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class TeleopDepositAllianceCommand extends SequentialCommandGroup { - - public DrivebaseSubsystem drivebaseSubsystem; - - public TeleopDepositAllianceCommand( - DrivebaseSubsystem drive, - IntakeSubsystem intake, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension - ) { - super( - new WaitCommand(0.3), - drive::relocalizeUnsafe, - new RegenerativeTrajectorySequenceCommand( - drive, - RobotConstants.WAREHOUSE_TO_HUB, - drive - ).alongWith( - new IntakeOutCommand(intake) - .withTimeout(0.3) - .andThen(new DepositAllianceCommand(deposit, extension, lift)) - ), - new BucketDumpCommand(deposit) - ); - drivebaseSubsystem = drive; - } - - @Override - public void initialize() { - super.initialize(); - RobotState.startDeposit(); - } - - @Override - public void end(boolean cancel) { - RobotState.stopDeposit(); - super.end(cancel); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositSharedCommand.java deleted file mode 100644 index d2f2b462..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositSharedCommand.java +++ /dev/null @@ -1,58 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.path.command.RegenerativeTrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.RobotState; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositSharedCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class TeleopDepositSharedCommand extends SequentialCommandGroup { - - public DrivebaseSubsystem drivebaseSubsystem; - - public TeleopDepositSharedCommand( - DrivebaseSubsystem drive, - IntakeSubsystem intake, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension - ) { - super( - new WaitCommand(0.3), - drive::relocalizeUnsafe, - new RegenerativeTrajectorySequenceCommand( - drive, - RobotConstants.WAREHOUSE_TO_SHARED_HUB, - drive - ).alongWith( - new IntakeOutCommand(intake).withTimeout(0.5), - //new WaitCommand(0.3).andThen(new DriveRelocalizeSharedCommand(drive)), - new WaitCommand(0.1).andThen(new DepositSharedCommand(deposit, extension, lift)) - ), - new WaitCommand(0.1), - new BucketDumpCommand(deposit) - ); - drivebaseSubsystem = drive; - } - - @Override - public void initialize() { - super.initialize(); - RobotState.startDeposit(); - } - - @Override - public void end(boolean cancel) { - RobotState.stopDeposit(); - super.end(cancel); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeAllianceWarehouseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeAllianceWarehouseCommand.java deleted file mode 100644 index ebe0d33a..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeAllianceWarehouseCommand.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class TeleopIntakeAllianceWarehouseCommand extends SequentialCommandGroup { - - public DrivebaseSubsystem drivebaseSubsystem; - - public TeleopIntakeAllianceWarehouseCommand( - DrivebaseSubsystem drive, - IntakeSubsystem intake, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension - ) { - super( - new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_WAREHOUSE, 4).alongWith( - new DepositCollectCommand(deposit, extension, lift).andThen( - new IntakeInCommand(intake) - ) - ), - new WaitCommand(0.3) - ); - drivebaseSubsystem = drive; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeSharedWarehouseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeSharedWarehouseCommand.java deleted file mode 100644 index 3963e2bb..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeSharedWarehouseCommand.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; - -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.path.command.TrajectorySequenceCommand; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class TeleopIntakeSharedWarehouseCommand extends SequentialCommandGroup { - - public DrivebaseSubsystem drivebaseSubsystem; - - public TeleopIntakeSharedWarehouseCommand( - DrivebaseSubsystem drive, - IntakeSubsystem intake, - LiftSubsystem lift, - ArmSubsystem deposit, - ExtensionSubsystem extension - ) { - super( - new TrajectorySequenceCommand(drive, RobotConstants.SHARED_HUB_TO_WAREHOUSE).alongWith( - new DepositCollectCommand(deposit, extension, lift) - .sleep(0.3) - .andThen(new IntakeInCommand(intake)) - ), - new WaitCommand(0.3) - ); - drivebaseSubsystem = drive; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapArmTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapArmTranslateCommand.java deleted file mode 100644 index 612c0374..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapArmTranslateCommand.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.cap; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; - -public class CapArmTranslateCommand implements Command { - - public CapSubsystem subsystem; - public double amount; - - public CapArmTranslateCommand(CapSubsystem cap, double amt) { - subsystem = cap; - addRequirements(cap); - amount = amt; - } - - @Override - public void initialize() { - subsystem.translateArm(amount); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.05; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapCloseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapCloseCommand.java deleted file mode 100644 index 14a582fd..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapCloseCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.cap; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; - -public class CapCloseCommand implements Command { - - public CapSubsystem subsystem; - - public CapCloseCommand(CapSubsystem cap) { - subsystem = cap; - addRequirements(cap); - } - - @Override - public void execute() { - subsystem.close(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapDownCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapDownCommand.java deleted file mode 100644 index 72591b4c..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapDownCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.cap; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; - -public class CapDownCommand implements Command { - - public CapSubsystem subsystem; - - public CapDownCommand(CapSubsystem cap) { - subsystem = cap; - addRequirements(cap); - } - - @Override - public void execute() { - subsystem.down(); - subsystem.close(); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.6; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOpenCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOpenCommand.java deleted file mode 100644 index cc7c27b9..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOpenCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.cap; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; - -public class CapOpenCommand implements Command { - - public CapSubsystem subsystem; - - public CapOpenCommand(CapSubsystem cap) { - subsystem = cap; - addRequirements(cap); - } - - @Override - public void execute() { - subsystem.open(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOutCommand.java deleted file mode 100644 index dc05e5f5..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOutCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.cap; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; - -public class CapOutCommand implements Command { - - public CapSubsystem subsystem; - - public CapOutCommand(CapSubsystem cap) { - subsystem = cap; - addRequirements(cap); - } - - @Override - public void execute() { - subsystem.raise(); - if (getRuntime().seconds() > 0.5) subsystem.raise2(); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 1; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapStoreCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapStoreCommand.java deleted file mode 100644 index fad63c65..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapStoreCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.cap; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; - -public class CapStoreCommand implements Command { - - public CapSubsystem subsystem; - - public CapStoreCommand(CapSubsystem cap) { - subsystem = cap; - addRequirements(cap); - } - - @Override - public void execute() { - subsystem.up(); - subsystem.close(); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.5; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapTurretTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapTurretTranslateCommand.java deleted file mode 100644 index ec318fdf..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapTurretTranslateCommand.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.cap; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; - -public class CapTurretTranslateCommand implements Command { - - public CapSubsystem subsystem; - public double amount; - - public CapTurretTranslateCommand(CapSubsystem cap, double amt) { - subsystem = cap; - addRequirements(cap); - amount = amt; - } - - @Override - public void initialize() { - subsystem.translateTurret(amount); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.1; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselLeftCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselLeftCommand.java deleted file mode 100644 index 28f322e8..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselLeftCommand.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.carousel; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.SPIN_OFFSET; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; - -public class CarouselLeftCommand implements Command { - - CarouselSubsystem subsystem; - - public CarouselLeftCommand(CarouselSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - subsystem.left(getRuntime().seconds() / SPIN_OFFSET); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean cancel) { - subsystem.stop(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselRightCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselRightCommand.java deleted file mode 100644 index 1c0bdf1d..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselRightCommand.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.carousel; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.SPIN_OFFSET; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; - -public class CarouselRightCommand implements Command { - - CarouselSubsystem subsystem; - - public CarouselRightCommand(CarouselSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - subsystem.right(getRuntime().seconds() / SPIN_OFFSET); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean cancel) { - subsystem.stop(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselSpinCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselSpinCommand.java deleted file mode 100644 index 8b71c4a6..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselSpinCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.carousel; - -import com.technototes.library.command.Command; -import com.technototes.library.util.Alliance; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; - -public class CarouselSpinCommand implements Command { - - CarouselSubsystem subsystem; - - public CarouselSpinCommand(CarouselSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - if (RobotConstants.getAlliance() == Alliance.RED) subsystem.left(); - else subsystem.right(); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean cancel) { - subsystem.stop(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselStopCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselStopCommand.java deleted file mode 100644 index 1415825b..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselStopCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.carousel; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; - -public class CarouselStopCommand implements Command { - - public CarouselSubsystem subsystem; - - public CarouselStopCommand(CarouselSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - subsystem.stop(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositAllianceCommand.java deleted file mode 100644 index db828cc5..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositAllianceCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.deposit; - -import com.technototes.library.command.ParallelCommandGroup; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmAllianceCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionOutCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevel3Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class DepositAllianceCommand extends ParallelCommandGroup { - - public DepositAllianceCommand( - ArmSubsystem arm, - ExtensionSubsystem extension, - LiftSubsystem lift - ) { - super( - new LiftLevel3Command(lift).withTimeout(1), - new WaitCommand(0).andThen(new ExtensionOutCommand(extension)), - new ArmAllianceCommand(arm) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCollectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCollectCommand.java deleted file mode 100644 index 6104c928..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCollectCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.deposit; - -import com.technototes.library.command.ParallelCommandGroup; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmInCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftCollectCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevel1Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class DepositCollectCommand extends ParallelCommandGroup { - - public DepositCollectCommand( - ArmSubsystem arm, - ExtensionSubsystem extension, - LiftSubsystem lift - ) { - super( - new WaitCommand(0.2).andThen(new ArmInCommand(arm)), - new ExtensionCollectCommand(extension), - new WaitCommand(0.2).andThen( - new LiftLevel1Command(lift) - .withTimeout(0.5) - .alongWith(new WaitCommand(0.5)) - .andThen(new LiftCollectCommand(lift).withTimeout(0.3)) - ) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCycleAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCycleAllianceCommand.java deleted file mode 100644 index e579e161..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCycleAllianceCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.deposit; - -import com.technototes.library.command.ParallelCommandGroup; -import com.technototes.library.command.WaitCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmAllianceCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionOutCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevel3Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class DepositCycleAllianceCommand extends ParallelCommandGroup { - - public DepositCycleAllianceCommand( - ArmSubsystem arm, - ExtensionSubsystem extension, - LiftSubsystem lift - ) { - super( - new LiftLevel3Command(lift).withTimeout(1), - //new WaitCommand(0).andThen(new ExtensionOutCommand(extension, RobotConstants.getAlliance().selectOf(ExtensionSubsystem.ExtensionConstants.AUTO_LEFT, ExtensionSubsystem.ExtensionConstants.AUTO_RIGHT))), - new ExtensionOutCommand(extension), - new ArmAllianceCommand(arm) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositOppositeSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositOppositeSharedCommand.java deleted file mode 100644 index 11fed43e..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositOppositeSharedCommand.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.deposit; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.OUT; - -import com.technototes.library.command.ParallelCommandGroup; -import com.technototes.library.util.Alliance; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmAllianceCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionLeftSideCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionRightSideCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftSharedCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class DepositOppositeSharedCommand extends ParallelCommandGroup { - - public DepositOppositeSharedCommand( - ArmSubsystem arm, - ExtensionSubsystem extension, - LiftSubsystem lift - ) { - super( - new LiftSharedCommand(lift), - Alliance.Selector.selectOf( - RobotConstants.getAlliance(), - new ExtensionLeftSideCommand(extension, OUT), - new ExtensionRightSideCommand(extension, OUT) - ), - new ArmAllianceCommand(arm) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositPreloadCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositPreloadCommand.java deleted file mode 100644 index e7676788..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositPreloadCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.deposit; - -import com.technototes.library.command.ParallelCommandGroup; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmBarcodeSelectCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionBarcodeSelectCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftBarcodeSelectCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; - -public class DepositPreloadCommand extends ParallelCommandGroup { - - public DepositPreloadCommand( - ArmSubsystem arm, - ExtensionSubsystem extension, - LiftSubsystem lift, - VisionSubsystem vision - ) { - super( - new LiftBarcodeSelectCommand(lift, vision).withTimeout(1), - new ArmBarcodeSelectCommand(arm, vision), - new ExtensionBarcodeSelectCommand(extension, vision) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositSharedCommand.java deleted file mode 100644 index 2c692e90..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositSharedCommand.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.deposit; - -import com.technototes.library.command.ParallelCommandGroup; -import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmSharedCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionSideCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftSharedCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class DepositSharedCommand extends ParallelCommandGroup { - - public DepositSharedCommand( - ArmSubsystem arm, - ExtensionSubsystem extension, - LiftSubsystem lift - ) { - super( - new LiftSharedCommand(lift), - new ExtensionSideCommand(extension), - new ArmSharedCommand(arm) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionBarcodeSelectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionBarcodeSelectCommand.java deleted file mode 100644 index 13471553..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionBarcodeSelectCommand.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; - -public class ExtensionBarcodeSelectCommand extends ExtensionOutCommand { - - public VisionSubsystem visionSubsystem; - - public ExtensionBarcodeSelectCommand(ExtensionSubsystem s, VisionSubsystem v) { - super(s); - visionSubsystem = v; - } - - @Override - public void execute() { - extensionSubsystem.center(); - extensionSubsystem.setSlide( - visionSubsystem.barcodePipeline.zero() - ? ExtensionSubsystem.ExtensionConstants.LOW_GOAL_AUTO - : ExtensionSubsystem.ExtensionConstants.OUT - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectCommand.java deleted file mode 100644 index 8601576f..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectCommand.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionCollectCommand extends ExtensionCommand { - - public ExtensionCollectCommand(ExtensionSubsystem subsystem) { - super(subsystem, ExtensionConstants.IN, ExtensionConstants.CENTER); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.7; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectSafeCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectSafeCommand.java deleted file mode 100644 index 882c3f48..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectSafeCommand.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionCollectSafeCommand extends ExtensionCommand { - - public ExtensionCollectSafeCommand(ExtensionSubsystem subsystem) { - super(subsystem, ExtensionConstants.IN, ExtensionConstants.CENTER); - } - - @Override - public void execute() { - extensionSubsystem.setTurret(turretTarget); - if (getRuntime().seconds() > 0.3) extensionSubsystem.setSlide(slideTarget); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.5; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCommand.java deleted file mode 100644 index fdbabd9e..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCommand.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionCommand implements Command { - - public ExtensionSubsystem extensionSubsystem; - public double slideTarget, turretTarget; - - public ExtensionCommand(ExtensionSubsystem subsystem, double slide, double turret) { - extensionSubsystem = subsystem; - addRequirements(subsystem); - slideTarget = slide; - turretTarget = turret; - } - - @Override - public void execute() { - extensionSubsystem.setTurret(turretTarget); - extensionSubsystem.setSlide(slideTarget); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftOutCommand.java deleted file mode 100644 index 74b9e321..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftOutCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionLeftOutCommand extends ExtensionLeftSideCommand { - - public ExtensionLeftOutCommand(ExtensionSubsystem subsystem) { - super(subsystem, ExtensionSubsystem.ExtensionConstants.OUT); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftSideCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftSideCommand.java deleted file mode 100644 index a7bd0acc..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftSideCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionLeftSideCommand extends ExtensionOutCommand { - - public ExtensionLeftSideCommand(ExtensionSubsystem subsystem) { - super(subsystem, ExtensionConstants.SHARED, ExtensionConstants.LEFT); - } - - public ExtensionLeftSideCommand(ExtensionSubsystem subsystem, double extension) { - super(subsystem, extension, ExtensionConstants.LEFT); - } - - @Override - public void execute() { - if (getRuntime().seconds() < 0.7) extensionSubsystem.fullyOut(); - else { - extensionSubsystem.setSlide(slideTarget); - extensionSubsystem.setTurret(turretTarget); - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionOutCommand.java deleted file mode 100644 index 9667bea3..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionOutCommand.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionOutCommand extends ExtensionCommand { - - public ExtensionOutCommand(ExtensionSubsystem subsystem, double turret) { - super(subsystem, ExtensionConstants.OUT, turret); - } - - public ExtensionOutCommand(ExtensionSubsystem subsystem, double slide, double turret) { - super(subsystem, slide, turret); - } - - public ExtensionOutCommand(ExtensionSubsystem subsystem) { - super(subsystem, ExtensionConstants.OUT, ExtensionConstants.CENTER); - } - - @Override - public void execute() { - extensionSubsystem.setSlide(slideTarget); - if (getRuntime().seconds() > 0.3) extensionSubsystem.setTurret(turretTarget); - } - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.6; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightOutCommand.java deleted file mode 100644 index c20bea4d..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightOutCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionRightOutCommand extends ExtensionRightSideCommand { - - public ExtensionRightOutCommand(ExtensionSubsystem subsystem) { - super(subsystem, ExtensionSubsystem.ExtensionConstants.OUT); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightSideCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightSideCommand.java deleted file mode 100644 index 0c422a1c..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightSideCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionRightSideCommand extends ExtensionOutCommand { - - public ExtensionRightSideCommand(ExtensionSubsystem subsystem) { - super(subsystem, ExtensionConstants.SHARED, ExtensionConstants.RIGHT); - } - - public ExtensionRightSideCommand(ExtensionSubsystem subsystem, double extension) { - super(subsystem, extension, ExtensionConstants.RIGHT); - } - - @Override - public void execute() { - if (getRuntime().seconds() < 0.7) extensionSubsystem.fullyOut(); - else { - extensionSubsystem.setSlide(slideTarget); - extensionSubsystem.setTurret(turretTarget); - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionSideCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionSideCommand.java deleted file mode 100644 index af4f2db5..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionSideCommand.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import static org.firstinspires.ftc.ptechnodactyl.RobotConstants.getAlliance; -import static org.firstinspires.ftc.ptechnodactyl.RobotState.SharedHubStrategy.OWN; -import static org.firstinspires.ftc.ptechnodactyl.RobotState.SharedHubStrategy.STEAL; -import static org.firstinspires.ftc.ptechnodactyl.RobotState.getSharedStrategy; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.LEFT; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.RIGHT; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.SHARED; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.STEAL_SHARED; - -import android.util.Pair; -import com.technototes.library.command.ChoiceCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionSideCommand extends ChoiceCommand { - - public ExtensionSideCommand(ExtensionSubsystem subsystem) { - super( - new Pair<>( - () -> getSharedStrategy() == OWN, - new ExtensionOutCommand(subsystem, SHARED, getAlliance().selectOf(RIGHT, LEFT)) - ), - new Pair<>( - () -> getSharedStrategy() == STEAL, - new ExtensionOutCommand( - subsystem, - STEAL_SHARED, - getAlliance().selectOf(LEFT, RIGHT) - ) - ) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionTranslateCommand.java deleted file mode 100644 index 11605473..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionTranslateCommand.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class ExtensionTranslateCommand extends ExtensionCommand { - - public ExtensionTranslateCommand(ExtensionSubsystem subsystem, double slide) { - super(subsystem, slide, 0); - } - - @Override - public void initialize() { - extensionSubsystem.translateSlide(slideTarget); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.05; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/TurretTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/TurretTranslateCommand.java deleted file mode 100644 index c7ced9e3..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/TurretTranslateCommand.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.extension; - -import java.util.function.BooleanSupplier; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; - -public class TurretTranslateCommand extends ExtensionCommand { - - public BooleanSupplier flipTranslate; - - public TurretTranslateCommand( - ExtensionSubsystem subsystem, - double turret, - BooleanSupplier flip - ) { - super(subsystem, 0, turret); - flipTranslate = flip; - } - - @Override - public void initialize() { - extensionSubsystem.translateTurret( - flipTranslate.getAsBoolean() ? -turretTarget : turretTarget - ); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return getRuntime().seconds() > 0.01; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeInCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeInCommand.java deleted file mode 100644 index 1fe462c7..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeInCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.intake; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; - -public class IntakeInCommand implements Command { - - IntakeSubsystem subsystem; - - public IntakeInCommand(IntakeSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - subsystem.in(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeOutCommand.java deleted file mode 100644 index 2f05c979..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeOutCommand.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.intake; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; - -public class IntakeOutCommand implements Command { - - IntakeSubsystem subsystem; - - public IntakeOutCommand(IntakeSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - if (getRuntime().seconds() % 0.2 < 0.1) subsystem.out(); - else subsystem.stop(); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean cancel) { - subsystem.stop(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeSafeCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeSafeCommand.java deleted file mode 100644 index 91562182..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeSafeCommand.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.intake; - -import org.firstinspires.ftc.ptechnodactyl.RobotState; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; - -public class IntakeSafeCommand extends IntakeInCommand { - - public IntakeSafeCommand(IntakeSubsystem s) { - super(s); - } - - @Override - public boolean isFinished() { - return RobotState.hasFreight() && getRuntime().seconds() > 0.2; - } - - @Override - public void end(boolean cancel) { - if (!cancel) subsystem.idle(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeStopCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeStopCommand.java deleted file mode 100644 index d1c4d1ef..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeStopCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.intake; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; - -public class IntakeStopCommand implements Command { - - IntakeSubsystem subsystem; - - public IntakeStopCommand(IntakeSubsystem s) { - subsystem = s; - addRequirements(s); - } - - @Override - public void execute() { - subsystem.stop(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftBarcodeSelectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftBarcodeSelectCommand.java deleted file mode 100644 index f4842384..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftBarcodeSelectCommand.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import android.util.Pair; -import com.technototes.library.command.ChoiceCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; - -public class LiftBarcodeSelectCommand extends ChoiceCommand { - - public LiftBarcodeSelectCommand(LiftSubsystem lift, VisionSubsystem vision) { - super( - new Pair<>(vision.barcodePipeline::zero, new LiftLevel1Command(lift)), - new Pair<>(vision.barcodePipeline::one, new LiftLevel2Command(lift)), - new Pair<>(vision.barcodePipeline::twoOrDefault, new LiftLevel3Command(lift)) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCollectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCollectCommand.java deleted file mode 100644 index fda5a0ae..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCollectCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class LiftCollectCommand extends LiftCommand { - - public LiftCollectCommand(LiftSubsystem l) { - super(l, LiftSubsystem.LiftConstants.COLLECT); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCommand.java deleted file mode 100644 index a0ca79e7..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCommand.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import com.technototes.library.command.Command; -import java.util.function.DoubleSupplier; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class LiftCommand implements Command { - - public LiftSubsystem liftSys; - public DoubleSupplier doubleSupplier; - - public LiftCommand(LiftSubsystem ls, DoubleSupplier ds) { - liftSys = ls; - doubleSupplier = ds; - addRequirements(ls); - } - - public LiftCommand(LiftSubsystem ls, double ds) { - this(ls, () -> ds); - } - - @Override - public void initialize() { - liftSys.setLiftPosition(doubleSupplier.getAsDouble()); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return liftSys.isAtTarget(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel1Command.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel1Command.java deleted file mode 100644 index 8820e0df..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel1Command.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class LiftLevel1Command extends LiftCommand { - - public LiftLevel1Command(LiftSubsystem l) { - super(l, LiftSubsystem.LiftConstants.LEVEL_1); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2Command.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2Command.java deleted file mode 100644 index 3df1fb24..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2Command.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class LiftLevel2Command extends LiftCommand { - - public LiftLevel2Command(LiftSubsystem l) { - super(l, LiftSubsystem.LiftConstants.LEVEL_2); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2TeleCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2TeleCommand.java deleted file mode 100644 index 42afc7e6..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2TeleCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class LiftLevel2TeleCommand extends LiftCommand { - - public LiftLevel2TeleCommand(LiftSubsystem l) { - super(l, LiftSubsystem.LiftConstants.TELEOP_LEVEL_2); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel3Command.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel3Command.java deleted file mode 100644 index 48c5524b..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel3Command.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class LiftLevel3Command extends LiftCommand { - - public LiftLevel3Command(LiftSubsystem l) { - super(l, LiftSubsystem.LiftConstants.LEVEL_3); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevelCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevelCommand.java deleted file mode 100644 index 6bd57dab..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevelCommand.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import static org.firstinspires.ftc.ptechnodactyl.RobotState.AllianceHubStrategy.HIGH; -import static org.firstinspires.ftc.ptechnodactyl.RobotState.AllianceHubStrategy.MID; -import static org.firstinspires.ftc.ptechnodactyl.RobotState.getAllianceStrategy; - -import android.util.Pair; -import com.technototes.library.command.ChoiceCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class LiftLevelCommand extends ChoiceCommand { - - public LiftLevelCommand(LiftSubsystem ls) { - super( - new Pair<>(() -> getAllianceStrategy() == HIGH, new LiftLevel3Command(ls)), - new Pair<>(() -> getAllianceStrategy() == MID, new LiftLevel2TeleCommand(ls)) - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftSharedCommand.java deleted file mode 100644 index d4c9505e..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftSharedCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class LiftSharedCommand extends LiftCommand { - - public LiftSharedCommand(LiftSubsystem l) { - super(l, LiftSubsystem.LiftConstants.NEUTRAL); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftTranslateCommand.java deleted file mode 100644 index dfda6a2c..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftTranslateCommand.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.lift; - -import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; - -public class LiftTranslateCommand extends LiftCommand { - - public LiftTranslateCommand(LiftSubsystem ls, double amt) { - super(ls, amt); - } - - @Override - public void initialize() { - liftSys.setLiftPosition(liftSys.get() + doubleSupplier.getAsDouble()); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return super.isFinished() && getRuntime().seconds() > 0.1; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/vision/VisionBarcodeCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/vision/VisionBarcodeCommand.java deleted file mode 100644 index 0734241f..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/vision/VisionBarcodeCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands.vision; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; - -public class VisionBarcodeCommand implements Command { - - public VisionSubsystem subsystem; - - public VisionBarcodeCommand(VisionSubsystem s) { - subsystem = s; - addRequirements(subsystem); - } - - @Override - public void initialize() { - subsystem.startBarcodePipeline(); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean cancel) { - subsystem.stopBarcodePipeline(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java new file mode 100644 index 00000000..2727dddc --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.ptechnodactyl.controllers; + +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.control.CommandAxis; +import com.technototes.library.control.CommandButton; +import com.technototes.library.control.CommandGamepad; +import com.technototes.library.control.Stick; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand; + +public class DriverController { + + public Robot robot; + public CommandGamepad gamepad; + + public Stick driveLeftStick, driveRightStick; + public CommandButton resetGyroButton, turboButton, snailButton; + public CommandButton override; + + public DriverController(CommandGamepad g, Robot r) { + this.robot = r; + gamepad = g; + override = g.leftTrigger.getAsButton(0.5); + + AssignNamedControllerButton(); + if (Setup.Connected.DRIVEBASE) { + bindDriveControls(); + } + } + + private void AssignNamedControllerButton() { + resetGyroButton = gamepad.ps_options; + driveLeftStick = gamepad.leftStick; + driveRightStick = gamepad.rightStick; + turboButton = gamepad.leftBumper; + snailButton = gamepad.rightBumper; + } + + public void bindDriveControls() { + CommandScheduler.scheduleJoystick( + new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) + ); + + turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem)); + turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); + snailButton.whenPressed(DrivingCommands.SnailDriving(robot.drivebaseSubsystem)); + snailButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); + resetGyroButton.whenPressed(DrivingCommands.ResetGyro(robot.drivebaseSubsystem)); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java new file mode 100644 index 00000000..20985224 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java @@ -0,0 +1,77 @@ +package org.firstinspires.ftc.ptechnodactyl.controllers; + +import android.support.v4.app.INotificationSideChannel; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.control.CommandAxis; +import com.technototes.library.control.CommandButton; +import com.technototes.library.control.CommandGamepad; +import com.technototes.library.control.Stick; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand; + +public class OneController { + + public Robot robot; + public CommandGamepad gamepad; + public CommandButton openClaw; + public CommandButton closeClaw; + public CommandButton ArmHorizontal; + public CommandButton ArmVertical; + public Stick driveLeftStick, driveRightStick; + public CommandButton resetGyroButton, turboButton, snailButton; + public CommandButton increment; + public CommandButton decrement; + + public OneController(CommandGamepad g, Robot r) { + robot = r; + gamepad = g; + AssignNamedControllerButton(); + BindControls(); + } + + private void AssignNamedControllerButton() { + openClaw = gamepad.leftBumper; + closeClaw = gamepad.rightBumper; + ArmHorizontal = gamepad.ps_square; + ArmVertical = gamepad.ps_triangle; + increment = gamepad.ps_circle; + decrement = gamepad.ps_cross; + resetGyroButton = gamepad.ps_options; + driveLeftStick = gamepad.leftStick; + driveRightStick = gamepad.rightStick; + turboButton = gamepad.leftBumper; + snailButton = gamepad.rightBumper; + } + + public void BindControls() { + if (Setup.Connected.CLAWSUBSYSTEM) { + bindClawSubsystemControls(); + } + if (Setup.Connected.DRIVEBASE) { + bindDriveControls(); + } + } + + public void bindClawSubsystemControls() { + openClaw.whenPressed(robot.clawSubsystem::openClaw); + closeClaw.whenPressed(robot.clawSubsystem::closeClaw); + ArmVertical.whenPressed(robot.clawSubsystem::setArmVertical); + ArmHorizontal.whenPressed(robot.clawSubsystem::setArmHorizontal); + increment.whenPressed(robot.clawSubsystem::incrementn); + decrement.whenPressed(robot.clawSubsystem::decrement); + } + + public void bindDriveControls() { + CommandScheduler.scheduleJoystick( + new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) + ); + + turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem)); + turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); + snailButton.whenPressed(DrivingCommands.SnailDriving(robot.drivebaseSubsystem)); + snailButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); + resetGyroButton.whenPressed(DrivingCommands.ResetGyro(robot.drivebaseSubsystem)); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java new file mode 100644 index 00000000..0cd94954 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.ptechnodactyl.controllers; + +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.control.CommandButton; +import com.technototes.library.control.CommandGamepad; +import com.technototes.library.control.Stick; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; + +public class OperatorController { + + public Robot robot; + public CommandGamepad gamepad; + public CommandButton openClaw; + public CommandButton closeClaw; + public Stick armStick; + public CommandButton ArmHorizontal; + public CommandButton ArmVertical; + public CommandButton intake; + + public OperatorController(CommandGamepad g, Robot r) { + robot = r; + gamepad = g; + AssignNamedControllerButton(); + BindControls(); + } + + private void AssignNamedControllerButton() { + openClaw = gamepad.leftBumper; + closeClaw = gamepad.rightBumper; + armStick = gamepad.rightStick; + ArmHorizontal = gamepad.ps_circle; + ArmVertical = gamepad.ps_triangle; + intake = gamepad.dpadRight; + } + + public void BindControls() { + if (Setup.Connected.CLAWSUBSYSTEM) { + bindClawSubsystemControls(); + } + } + + public void bindClawSubsystemControls() { + openClaw.whenPressed(robot.clawSubsystem::openClaw); + closeClaw.whenPressed(robot.clawSubsystem::closeClaw); + ArmVertical.whenPressed(robot.clawSubsystem::setArmVertical); + ArmHorizontal.whenPressed(robot.clawSubsystem::setArmHorizontal); + intake.whenPressed(robot.clawSubsystem::setArmIntake); + CommandScheduler.scheduleJoystick(new JoystickIncDecCmd(robot.clawSubsystem, armStick)); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java new file mode 100644 index 00000000..e9ddeaf9 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.ptechnodactyl.controllers; + +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.control.CommandButton; +import com.technototes.library.control.CommandGamepad; +import com.technototes.library.control.Stick; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; + +public class TestController { + + public Robot robot; + public CommandGamepad gamepad; + public CommandButton testPower2; + public CommandButton testPower; + public CommandButton armHorizontal; + public Stick armStick; + + public TestController(CommandGamepad g, Robot r) { + robot = r; + gamepad = g; + AssignNamedControllerButton(); + BindControls(); + } + + private void AssignNamedControllerButton() { + testPower = gamepad.leftBumper; + testPower2 = gamepad.rightBumper; + armStick = gamepad.rightStick; + armHorizontal = gamepad.ps_circle; + } + + public void BindControls() { + bindTestControls(); + } + + public void bindTestControls() { + testPower.whenPressed(robot.clawSubsystem::powIncrement); + testPower2.whenPressed(robot.clawSubsystem::powDecrement); + CommandScheduler.scheduleJoystick(new JoystickIncDecCmd(robot.clawSubsystem, armStick)); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoCycleBase.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoCycleBase.java deleted file mode 100644 index f3c8bd1e..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoCycleBase.java +++ /dev/null @@ -1,56 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.technototes.library.command.Command; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import com.technototes.library.util.Alliance; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.autonomous.AutoCycleCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.vision.VisionBarcodeCommand; - -public abstract class AutoCycleBase extends CommandOpMode implements Loggable { - - public Robot robot; - public Hardware hardware; - - @Override - public void uponInit() { - RobotConstants.updateAlliance(Alliance.get(getClass())); - hardware = new Hardware(); - robot = new Robot(hardware); - CommandScheduler.scheduleInit(new VisionBarcodeCommand(robot.visionSubsystem)); - CommandScheduler.scheduleOnceForState( - new AutoCycleCommand( - robot.drivebaseSubsystem, - robot.intakeSubsystem, - robot.liftSubsystem, - robot.armSubsystem, - robot.extensionSubsystem, - robot.visionSubsystem - ), - OpModeState.RUN - ); - } - - @Override - public void uponStart() { - if (Robot.SubsystemConstants.CAP_ENABLED) robot.capSubsystem.up(); - } - - @Override - public void end() {} - - @Autonomous(name = "\uD83D\uDD35 ♻️ Blue Cycle") - @Alliance.Blue - public static class CycleBlueAuto extends AutoCycleBase {} - - @Autonomous(name = "\uD83D\uDFE5 ♻️ Red Cycle") - @Alliance.Red - public static class CycleRedAuto extends AutoCycleBase {} -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoDuckBase.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoDuckBase.java deleted file mode 100644 index a383a111..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoDuckBase.java +++ /dev/null @@ -1,56 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import com.technototes.library.util.Alliance; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; -import org.firstinspires.ftc.ptechnodactyl.commands.autonomous.AutoDuckCommand; -import org.firstinspires.ftc.ptechnodactyl.commands.vision.VisionBarcodeCommand; - -public abstract class AutoDuckBase extends CommandOpMode implements Loggable { - - public Robot robot; - public Hardware hardware; - - @Override - public void uponInit() { - RobotConstants.updateAlliance(Alliance.get(getClass())); - hardware = new Hardware(); - robot = new Robot(hardware); - CommandScheduler.scheduleInit(new VisionBarcodeCommand(robot.visionSubsystem)); - CommandScheduler.scheduleOnceForState( - new AutoDuckCommand( - robot.drivebaseSubsystem, - robot.intakeSubsystem, - robot.liftSubsystem, - robot.armSubsystem, - robot.extensionSubsystem, - robot.visionSubsystem, - robot.carouselSubsystem - ), - OpModeState.RUN - ); - } - - @Override - public void uponStart() { - if (Robot.SubsystemConstants.CAP_ENABLED) robot.capSubsystem.up(); - } - - @Override - public void end() {} - - @Autonomous(name = "\uD83D\uDD35 \uD83E\uDD86 Blue Duck") - @Alliance.Blue - public static class DuckBlueAuto extends AutoDuckBase {} - - @Autonomous(name = "\uD83D\uDFE5 \uD83E\uDD86 Red Duck") - @Alliance.Red - public static class DuckRedAuto extends AutoDuckBase {} -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java new file mode 100644 index 00000000..a77ee77b --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import com.technototes.path.command.TrajectorySequenceCommand; +import com.technototes.path.geometry.ConfigurablePoseD; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; + +/* + * Some Emojis for opmode names: + * Copy them and paste them in the 'name' section of the @Autonomous tag + * Red alliance: 🟥 + * Blue alliance: 🟦 + * Wing position: 🪶 + * Backstage pos: 🎦 + */ +//@Config +@Autonomous(name = "Basic Auto", preselectTeleOp = "PlainDrive") +@SuppressWarnings("unused") +public class Basic extends CommandOpMode implements Loggable { + + public static int AUTO_TIME = 1; + public Hardware hardware; + public Robot robot; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + new TrajectorySequenceCommand(robot.drivebaseSubsystem, b -> + b + .apply(new ConfigurablePoseD(0, 0, 0).toPose()) + .lineTo(new Vector2d(5, 5)) + .build() + ), + // new TurboCommand(robot.drivebaseSubsystem), + // new StartSpinningCmd(robot.spinner), + new WaitCommand(AUTO_TIME), + new TrajectorySequenceCommand(robot.drivebaseSubsystem, b -> + b + .apply(new ConfigurablePoseD(5, 5, 0).toPose()) + .lineTo(new Vector2d(0, 0)) + .build() + ), + // new StopSpinningCmd(robot.spinner), + CommandScheduler::terminateOpMode + ), + CommandOpMode.OpModeState.RUN + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/LoopTimeTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/LoopTimeTest.java deleted file mode 100644 index 3e988799..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/LoopTimeTest.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.hardware.sensor.ColorDistanceSensor; -import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; -import com.technototes.library.logger.Log; -import com.technototes.library.structure.CommandOpMode; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; - -@TeleOp(name = "looptimetest") -public class LoopTimeTest extends CommandOpMode { - - public ColorDistanceSensor sensor; - - @Log.Number(index = 0, name = "sensor calls per loop") - public double sensorLoops = 0; - - @Log.Number(index = 1, name = "looptime in hz") - public double hz; - - @Log.Number(index = 2, name = "avg sensor call time") - public double timePerSensorCall; - - @Log.Number(index = 3, name = "sensor distance inches") - public double distance; - - @Log.Number(index = 4, name = "sensor raw light") - public double light; - - private long pastTime; - - @Override - public void uponInit() { - sensor = new ColorDistanceSensor("irange").onUnit(DistanceUnit.INCH); - - driverGamepad.dpadUp.whenPressed(() -> sensorLoops++); - driverGamepad.dpadDown.whenPressed(() -> sensorLoops--); - } - - @Override - public void runLoop() { - long l = System.currentTimeMillis(); - for (int i = 0; i < sensorLoops; i++) { - distance = sensor.getDistance(); - light = sensor.getLight(); - } - if (sensorLoops != 0) timePerSensorCall = (System.currentTimeMillis() - l) / sensorLoops; - hz = 1000.0 / (System.currentTimeMillis() - pastTime); - pastTime = System.currentTimeMillis(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/BackAndForth.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/BackAndForth.java deleted file mode 100644 index 4d47a8fa..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/BackAndForth.java +++ /dev/null @@ -1,59 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.technototes.library.hardware.HardwareDevice; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -/* - * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base - * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the - * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer - * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're - * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once - * you've successfully connected, start the program, and your robot will begin moving forward and - * backward. You should observe the target position (green) and your pose estimate (blue) and adjust - * your follower PID coefficients such that you follow the target position as accurately as possible. - * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. - * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. - * These coefficients can be tuned live in dashboard. - * - * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It - * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. - */ -@Disabled -@Config -@Autonomous(group = "drive") -public class BackAndForth extends LinearOpMode { - - public static double DISTANCE = 40; - - @Override - public void runOpMode() throws InterruptedException { - HardwareDevice.hardwareMap = hardwareMap; - DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); - // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - Trajectory trajectoryForward = drive - .trajectoryBuilder(new Pose2d()) - .forward(DISTANCE) - .build(); - - Trajectory trajectoryBackward = drive - .trajectoryBuilder(trajectoryForward.end()) - .back(DISTANCE) - .build(); - - waitForStart(); - - while (opModeIsActive() && !isStopRequested()) { - drive.followTrajectory(trajectoryForward); - drive.followTrajectory(trajectoryBackward); - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/DriveVelocityPIDTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/DriveVelocityPIDTuner.java deleted file mode 100644 index 352cdf17..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/DriveVelocityPIDTuner.java +++ /dev/null @@ -1,174 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.*; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.profile.MotionProfile; -import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; -import com.acmerobotics.roadrunner.profile.MotionState; -import com.acmerobotics.roadrunner.util.NanoClock; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.util.RobotLog; -import com.technototes.library.hardware.HardwareDevice; -import java.util.List; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -/* - * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed- - * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as - * important as the positional parameters. Like the other manual tuning routines, this op mode - * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's - * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC - * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully - * connected, start the program, and your robot will begin moving forward and backward according to - * a motion profile. Your job is to graph the velocity errors over time and adjust the PID - * coefficients (note: the tuning variable will not appear until the op mode finishes initializing). - * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the - * MOTOR_VELO_PID field. - * - * Recommended tuning process: - * - * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to - * mitigate oscillations. - * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached. - * 3. Back off kP and kD a little until the response is less oscillatory (but without lag). - * - * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the - * user to reset the position of the bot in the event that it drifts off the path. - * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. - */ -@Disabled -@Config -@Autonomous(group = "drive") -public class DriveVelocityPIDTuner extends LinearOpMode { - - public static double DISTANCE = 70; // in - - enum Mode { - DRIVER_MODE, - TUNING_MODE, - } - - private static MotionProfile generateProfile(boolean movingForward) { - MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); - MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); - return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); - } - - @Override - public void runOpMode() { - HardwareDevice.hardwareMap = hardwareMap; - - if (!RUN_USING_ENCODER) { - RobotLog.setGlobalErrorMsg( - "%s does not need to be run if the built-in motor velocity" + "PID is not in use", - getClass().getSimpleName() - ); - } - - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); - - Mode mode = Mode.TUNING_MODE; - - double lastKp = MOTOR_VELO_PID.p; - double lastKi = MOTOR_VELO_PID.i; - double lastKd = MOTOR_VELO_PID.d; - double lastKf = MOTOR_VELO_PID.f; - - drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); - - NanoClock clock = NanoClock.system(); - - telemetry.addLine("Ready!"); - telemetry.update(); - telemetry.clearAll(); - - waitForStart(); - - if (isStopRequested()) return; - - boolean movingForwards = true; - MotionProfile activeProfile = generateProfile(true); - double profileStart = clock.seconds(); - - while (!isStopRequested()) { - telemetry.addData("mode", mode); - - switch (mode) { - case TUNING_MODE: - if (gamepad1.y) { - mode = Mode.DRIVER_MODE; - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - } - - // calculate and set the motor power - double profileTime = clock.seconds() - profileStart; - - if (profileTime > activeProfile.duration()) { - // generate a new profile - movingForwards = !movingForwards; - activeProfile = generateProfile(movingForwards); - profileStart = clock.seconds(); - } - - MotionState motionState = activeProfile.get(profileTime); - double targetPower = kV * motionState.getV(); - drive.setDrivePower(new Pose2d(targetPower, 0, 0)); - - List velocities = drive.getWheelVelocities(); - - // update telemetry - telemetry.addData("targetVelocity", motionState.getV()); - for (int i = 0; i < velocities.size(); i++) { - telemetry.addData("measuredVelocity" + i, velocities.get(i)); - telemetry.addData("error" + i, motionState.getV() - velocities.get(i)); - } - break; - case DRIVER_MODE: - if (gamepad1.b) { - drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - mode = Mode.TUNING_MODE; - movingForwards = true; - activeProfile = generateProfile(movingForwards); - profileStart = clock.seconds(); - } - - drive.setWeightedDrivePower( - new Pose2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x, - -gamepad1.right_stick_x - ) - ); - break; - } - - if ( - lastKp != MOTOR_VELO_PID.p || - lastKd != MOTOR_VELO_PID.d || - lastKi != MOTOR_VELO_PID.i || - lastKf != MOTOR_VELO_PID.f - ) { - drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); - - lastKp = MOTOR_VELO_PID.p; - lastKi = MOTOR_VELO_PID.i; - lastKd = MOTOR_VELO_PID.d; - lastKf = MOTOR_VELO_PID.f; - } - - telemetry.update(); - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/FollowerPIDTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/FollowerPIDTuner.java deleted file mode 100644 index 920ed20d..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/FollowerPIDTuner.java +++ /dev/null @@ -1,55 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.technototes.library.hardware.HardwareDevice; - -/* - * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base - * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the - * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer - * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're - * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once - * you've successfully connected, start the program, and your robot will begin driving in a square. - * You should observe the target position (green) and your pose estimate (blue) and adjust your - * follower PID coefficients such that you follow the target position as accurately as possible. - * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. - * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. - * These coefficients can be tuned live in dashboard. - */ -@Disabled -@Config -@Autonomous(group = "drive") -public class FollowerPIDTuner extends LinearOpMode { - - public static double DISTANCE = 48; // in - - @Override - public void runOpMode() throws InterruptedException { - HardwareDevice.hardwareMap = hardwareMap; - - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - //DrivebaseSubsystem drive = new DrivebaseSubsystem(); - - Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0); - - drive.setPoseEstimate(startPose); - - waitForStart(); - - if (isStopRequested()) return; - - while (!isStopRequested()) { - Trajectory traj = drive.trajectoryBuilder(startPose).forward(DISTANCE).build(); - drive.followTrajectory(traj); - drive.turn(Math.toRadians(90)); - - startPose = traj.end().plus(new Pose2d(0, 0, Math.toRadians(90))); - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/LocalizationTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/LocalizationTest.java deleted file mode 100644 index bab57fac..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/LocalizationTest.java +++ /dev/null @@ -1,55 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.technototes.library.hardware.HardwareDevice; - -/** - * This is a simple teleop routine for testing localization. Drive the robot around like a normal - * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight - * errors are not out of the ordinary, especially with sudden drive motions). The goal of this - * exercise is to ascertain whether the localizer has been configured properly (note: the pure - * encoder localizer heading may be significantly off if the track width has not been tuned). - */ -@Disabled -@TeleOp(group = "drive") -public class LocalizationTest extends LinearOpMode { - - @Override - public void runOpMode() throws InterruptedException { - HardwareDevice.hardwareMap = hardwareMap; - - //DrivebaseSubsystem drive = new DrivebaseSubsystem(); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - waitForStart(); - - while (!isStopRequested()) { - drive.setWeightedDrivePower( - new Pose2d(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x) - ); - System.out.println( - drive.leftFront.getCurrentPosition() + - " " + - drive.leftRear.getCurrentPosition() + - " " + - drive.rightFront.getCurrentPosition() + - " " + - drive.rightRear.getCurrentPosition() - ); - - drive.update(); - - Pose2d poseEstimate = drive.getPoseEstimate(); - telemetry.addData("x", poseEstimate.getX()); - telemetry.addData("y", poseEstimate.getY()); - telemetry.addData("heading", poseEstimate.getHeading()); - telemetry.update(); - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/ManualFeedforwardTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/ManualFeedforwardTuner.java deleted file mode 100644 index 294e253f..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/ManualFeedforwardTuner.java +++ /dev/null @@ -1,156 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.*; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.kinematics.Kinematics; -import com.acmerobotics.roadrunner.profile.MotionProfile; -import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; -import com.acmerobotics.roadrunner.profile.MotionState; -import com.acmerobotics.roadrunner.util.NanoClock; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.util.RobotLog; -import com.technototes.library.hardware.HardwareDevice; -import java.util.Objects; - -/* - * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary, - * tuning these coefficients is just as important as the positional parameters. Like the other - * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard, - * connect your computer to the RC's WiFi network. In your browser, navigate to - * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if - * you are using the Control Hub. Once you've successfully connected, start the program, and your - * robot will begin moving forward and backward according to a motion profile. Your job is to graph - * the velocity errors over time and adjust the feedforward coefficients. Once you've found a - * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file. - * - * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the - * user to reset the position of the bot in the event that it drifts off the path. - * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. - */ -@Disabled -@Config -@Autonomous(group = "drive") -public class ManualFeedforwardTuner extends LinearOpMode { - - public static double DISTANCE = 72; // in - - private FtcDashboard dashboard = FtcDashboard.getInstance(); - - // private DrivebaseSubsystem drive; - SampleMecanumDrive drive; - - enum Mode { - DRIVER_MODE, - TUNING_MODE, - } - - private Mode mode; - - private static MotionProfile generateProfile(boolean movingForward) { - MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); - MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); - return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); - } - - @Override - public void runOpMode() { - HardwareDevice.hardwareMap = hardwareMap; - - if (RUN_USING_ENCODER) { - RobotLog.setGlobalErrorMsg( - "Feedforward constants usually don't need to be tuned " + - "when using the built-in drive motor velocity PID." - ); - } - - telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry()); - - //drive = new DrivebaseSubsystem(); - drive = new SampleMecanumDrive(hardwareMap); - - mode = Mode.TUNING_MODE; - - NanoClock clock = NanoClock.system(); - - telemetry.addLine("Ready!"); - telemetry.update(); - telemetry.clearAll(); - - waitForStart(); - - if (isStopRequested()) return; - - boolean movingForwards = true; - MotionProfile activeProfile = generateProfile(true); - double profileStart = clock.seconds(); - - while (!isStopRequested()) { - telemetry.addData("mode", mode); - - switch (mode) { - case TUNING_MODE: - if (gamepad1.y) { - mode = Mode.DRIVER_MODE; - } - - // calculate and set the motor power - double profileTime = clock.seconds() - profileStart; - - if (profileTime > activeProfile.duration()) { - // generate a new profile - movingForwards = !movingForwards; - activeProfile = generateProfile(movingForwards); - profileStart = clock.seconds(); - } - - MotionState motionState = activeProfile.get(profileTime); - double targetPower = Kinematics.calculateMotorFeedforward( - motionState.getV(), - motionState.getA(), - kV, - kA, - kStatic - ); - - drive.setDrivePower(new Pose2d(targetPower, 0, 0)); - drive.updatePoseEstimate(); - - Pose2d poseVelo = Objects.requireNonNull( - drive.getPoseVelocity(), - "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." - ); - double currentVelo = poseVelo.getX(); - - // update telemetry - telemetry.addData("targetVelocity", motionState.getV()); - telemetry.addData("measuredVelocity", currentVelo); - telemetry.addData("error", motionState.getV() - currentVelo); - break; - case DRIVER_MODE: - if (gamepad1.b) { - mode = Mode.TUNING_MODE; - movingForwards = true; - activeProfile = generateProfile(movingForwards); - profileStart = clock.seconds(); - } - - drive.setWeightedDrivePower( - new Pose2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x, - -gamepad1.right_stick_x - ) - ); - break; - } - - telemetry.update(); - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxAngularVeloTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxAngularVeloTuner.java deleted file mode 100644 index 28a7b351..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxAngularVeloTuner.java +++ /dev/null @@ -1,74 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.util.ElapsedTime; -import java.util.Objects; - -/** - * This routine is designed to calculate the maximum angular velocity your bot can achieve under load. - *

- * Upon pressing start, your bot will turn at max power for RUNTIME seconds. - *

- * Further fine tuning of MAX_ANG_VEL may be desired. - */ -@Disabled -@Config -@Autonomous(group = "drive") -public class MaxAngularVeloTuner extends LinearOpMode { - - public static double RUNTIME = 4.0; - - private ElapsedTime timer; - private double maxAngVelocity = 0.0; - - @Override - public void runOpMode() throws InterruptedException { - //DrivebaseSubsystem drive = new DrivebaseSubsystem(); - - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - - telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds."); - telemetry.addLine("Please ensure you have enough space cleared."); - telemetry.addLine(""); - telemetry.addLine("Press start when ready."); - telemetry.update(); - - waitForStart(); - - telemetry.clearAll(); - telemetry.update(); - - drive.setDrivePower(new Pose2d(0, 0, 1)); - timer = new ElapsedTime(); - - while (!isStopRequested() && timer.seconds() < RUNTIME) { - drive.updatePoseEstimate(); - - Pose2d poseVelo = Objects.requireNonNull( - drive.getPoseVelocity(), - "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." - ); - - maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity); - } - - drive.setDrivePower(new Pose2d()); - - telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity); - telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity)); - telemetry.update(); - - while (!isStopRequested()) idle(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxVelocityTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxVelocityTuner.java deleted file mode 100644 index 8f7d6fbf..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxVelocityTuner.java +++ /dev/null @@ -1,101 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.TICKS_PER_REV; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.WHEEL_RADIUS; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.VoltageSensor; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.technototes.library.hardware.HardwareDevice; -import com.technototes.path.subsystem.MecanumConstants; -import java.util.Objects; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -/** - * This routine is designed to calculate the maximum velocity your bot can achieve under load. It - * will also calculate the effective kF value for your velocity PID. - *

- * Upon pressing start, your bot will run at max power for RUNTIME seconds. - *

- * Further fine tuning of kF may be desired. - */ -@Disabled -@Config -@Autonomous(group = "drive") -public class MaxVelocityTuner extends LinearOpMode { - - public static double RUNTIME = 2.0; - - private ElapsedTime timer; - private double maxVelocity = 0.0; - - private VoltageSensor batteryVoltageSensor; - - @Override - public void runOpMode() throws InterruptedException { - HardwareDevice.hardwareMap = hardwareMap; - - // DrivebaseSubsystem drive = new DrivebaseSubsystem(); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); - - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - - telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds."); - telemetry.addLine("Please ensure you have enough space cleared."); - telemetry.addLine(""); - telemetry.addLine("Press start when ready."); - telemetry.update(); - - waitForStart(); - - telemetry.clearAll(); - telemetry.update(); - - drive.setDrivePower(new Pose2d(1, 0, 0)); - timer = new ElapsedTime(); - - while (!isStopRequested() && timer.seconds() < RUNTIME) { - drive.updatePoseEstimate(); - - Pose2d poseVelo = Objects.requireNonNull( - drive.getPoseVelocity(), - "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." - ); - - maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity); - } - - drive.setDrivePower(new Pose2d()); - - double effectiveKf = MecanumConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity)); - - telemetry.addData("Max Velocity", maxVelocity); - telemetry.addData( - "Voltage Compensated kF", - (effectiveKf * batteryVoltageSensor.getVoltage()) / 12 - ); - telemetry.update(); - - while (!isStopRequested() && opModeIsActive()) idle(); - } - - private double veloInchesToTicks(double inchesPerSec) { - return ( - (inchesPerSec / - (2 * Math.PI * WHEEL_RADIUS) / - DrivebaseSubsystem.DriveConstants.GEAR_RATIO) * - TICKS_PER_REV - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MotorDirectionDebugger.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MotorDirectionDebugger.java deleted file mode 100644 index 8d0dbdb8..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MotorDirectionDebugger.java +++ /dev/null @@ -1,105 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.hardware.HardwareDevice; -import org.firstinspires.ftc.robotcore.external.Telemetry; - -/** - * This is a simple teleop routine for debugging your motor configuration. - * Pressing each of the buttons will power its respective motor. - * - * Button Mappings: - * - * Xbox/PS4 Button - Motor - * X / ▢ - Front Left - * Y / Δ - Front Right - * B / O - Rear Right - * A / X - Rear Left - * The buttons are mapped to match the wheels spatially if you - * were to rotate the gamepad 45deg°. x/square is the front left - * ________ and each button corresponds to the wheel as you go clockwise - * / ______ \ - * ------------.-' _ '-..+ Front of Bot - * / _ ( Y ) _ \ ^ - * | ( X ) _ ( B ) | Front Left \ Front Right - * ___ '. ( A ) /| Wheel \ Wheel - * .' '. '-._____.-' .' (x/▢) \ (Y/Δ) - * | | | \ - * '.___.' '. | Rear Left \ Rear Right - * '. / Wheel \ Wheel - * \. .' (A/X) \ (B/O) - * \________/ - * - * Uncomment the @Disabled tag below to use this opmode. - */ -@Disabled -@Config -@TeleOp(group = "drive") -public class MotorDirectionDebugger extends LinearOpMode { - - public static double MOTOR_POWER = 0.7; - - @Override - public void runOpMode() throws InterruptedException { - HardwareDevice.hardwareMap = hardwareMap; - - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - - //DrivebaseSubsystem drive = new DrivebaseSubsystem(); - - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - telemetry.addLine("Press play to begin the debugging opmode"); - telemetry.update(); - - waitForStart(); - - if (isStopRequested()) return; - - telemetry.clearAll(); - telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); - - while (!isStopRequested()) { - telemetry.addLine("Press each button to turn on its respective motor"); - telemetry.addLine(); - telemetry.addLine("Xbox/PS4 Button - Motor"); - telemetry.addLine( - "  X / ▢         - Front Left" - ); - telemetry.addLine( - "  Y / Δ         - Front Right" - ); - telemetry.addLine( - "  B / O         - Rear  Right" - ); - telemetry.addLine( - "  A / X         - Rear  Left" - ); - telemetry.addLine(); - - if (gamepad1.x) { - drive.setMotorPowers(MOTOR_POWER, 0, 0, 0); - telemetry.addLine("Running Motor: Front Left"); - } else if (gamepad1.y) { - drive.setMotorPowers(0, 0, 0, MOTOR_POWER); - telemetry.addLine("Running Motor: Front Right"); - } else if (gamepad1.b) { - drive.setMotorPowers(0, 0, MOTOR_POWER, 0); - telemetry.addLine("Running Motor: Rear Right"); - } else if (gamepad1.a) { - drive.setMotorPowers(0, MOTOR_POWER, 0, 0); - telemetry.addLine("Running Motor: Rear Left"); - } else { - drive.setMotorPowers(0, 0, 0, 0); - telemetry.addLine("Running Motor: None"); - } - - telemetry.update(); - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/OpenCVTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/OpenCVTest.java deleted file mode 100644 index 15335649..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/OpenCVTest.java +++ /dev/null @@ -1,373 +0,0 @@ -/* - * Copyright (c) 2020 OpenFTC Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.ptechnodactyl.subsystems.BarcodePipeline; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.opencv.core.Core; -import org.opencv.core.Mat; -import org.opencv.core.Point; -import org.opencv.core.Rect; -import org.opencv.core.Scalar; -import org.opencv.imgproc.Imgproc; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvInternalCamera; -import org.openftc.easyopencv.OpenCvPipeline; -import org.openftc.easyopencv.OpenCvWebcam; - -/* - * This sample demonstrates a basic (but battle-tested and essentially - * 100% accurate) method of detecting the skystone when lined up with - * the sample regions over the first 3 stones. - */ -@Disabled -@TeleOp -public class OpenCVTest extends LinearOpMode { - - OpenCvWebcam webcam; - BarcodePipeline pipeline; - - @Override - public void runOpMode() { - /** - * NOTE: Many comments have been omitted from this sample for the - * sake of conciseness. If you're just starting out with EasyOpenCv, - * you should take a look at {@link InternalCamera1Example} or its - * webcam counterpart, {@link WebcamExample} first. - */ - - int cameraMonitorViewId = hardwareMap.appContext - .getResources() - .getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - webcam = OpenCvCameraFactory.getInstance() - .createWebcam(hardwareMap.get(WebcamName.class, "webcam"), cameraMonitorViewId); - pipeline = new BarcodePipeline(); - webcam.setPipeline(pipeline); - - webcam.openCameraDeviceAsync( - new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); - } - - @Override - public void onError(int errorCode) { - /* - * This will be called if the camera could not be opened - */ - } - } - ); - - waitForStart(); - - while (opModeIsActive()) { - telemetry.addData("Analysis", pipeline.get()); - telemetry.update(); - - // Don't burn CPU cycles busy-looping in this sample - sleep(50); - } - } - - public static class SkystoneDeterminationPipeline extends OpenCvPipeline { - - /* - * An enum to define the skystone position - */ - public enum SkystonePosition { - LEFT, - CENTER, - RIGHT, - } - - /* - * Some color constants - */ - static final Scalar BLUE = new Scalar(0, 0, 255); - static final Scalar GREEN = new Scalar(0, 255, 0); - - /* - * The core values which define the location and size of the sample regions - */ - static final Point REGION1_TOPLEFT_ANCHOR_POINT = new Point(109, 98); - static final Point REGION2_TOPLEFT_ANCHOR_POINT = new Point(181, 98); - static final Point REGION3_TOPLEFT_ANCHOR_POINT = new Point(253, 98); - static final int REGION_WIDTH = 20; - static final int REGION_HEIGHT = 20; - - /* - * Points which actually define the sample region rectangles, derived from above values - * - * Example of how points A and B work to define a rectangle - * - * ------------------------------------ - * | (0,0) Point A | - * | | - * | | - * | | - * | | - * | | - * | | - * | Point B (70,50) | - * ------------------------------------ - * - */ - Point region1_pointA = new Point( - REGION1_TOPLEFT_ANCHOR_POINT.x, - REGION1_TOPLEFT_ANCHOR_POINT.y - ); - Point region1_pointB = new Point( - REGION1_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH, - REGION1_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT - ); - Point region2_pointA = new Point( - REGION2_TOPLEFT_ANCHOR_POINT.x, - REGION2_TOPLEFT_ANCHOR_POINT.y - ); - Point region2_pointB = new Point( - REGION2_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH, - REGION2_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT - ); - Point region3_pointA = new Point( - REGION3_TOPLEFT_ANCHOR_POINT.x, - REGION3_TOPLEFT_ANCHOR_POINT.y - ); - Point region3_pointB = new Point( - REGION3_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH, - REGION3_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT - ); - - /* - * Working variables - */ - Mat region1_Cb, region2_Cb, region3_Cb; - Mat YCrCb = new Mat(); - Mat Cb = new Mat(); - int avg1, avg2, avg3; - - // Volatile since accessed by OpMode thread w/o synchronization - private volatile SkystonePosition position = SkystonePosition.LEFT; - - /* - * This function takes the RGB frame, converts to YCrCb, - * and extracts the Cb channel to the 'Cb' variable - */ - void inputToCb(Mat input) { - Imgproc.cvtColor(input, YCrCb, Imgproc.COLOR_RGB2YCrCb); - Core.extractChannel(YCrCb, Cb, 2); - } - - @Override - public void init(Mat firstFrame) { - /* - * We need to call this in order to make sure the 'Cb' - * object is initialized, so that the submats we make - * will still be linked to it on subsequent frames. (If - * the object were to only be initialized in processFrame, - * then the submats would become delinked because the backing - * buffer would be re-allocated the first time a real frame - * was crunched) - */ - inputToCb(firstFrame); - - /* - * Submats are a persistent reference to a region of the parent - * buffer. Any changes to the child affect the parent, and the - * reverse also holds true. - */ - region1_Cb = Cb.submat(new Rect(region1_pointA, region1_pointB)); - region2_Cb = Cb.submat(new Rect(region2_pointA, region2_pointB)); - region3_Cb = Cb.submat(new Rect(region3_pointA, region3_pointB)); - } - - @Override - public Mat processFrame(Mat input) { - /* - * Overview of what we're doing: - * - * We first convert to YCrCb color space, from RGB color space. - * Why do we do this? Well, in the RGB color space, chroma and - * luma are intertwined. In YCrCb, chroma and luma are separated. - * YCrCb is a 3-channel color space, just like RGB. YCrCb's 3 channels - * are Y, the luma channel (which essentially just a B&W image), the - * Cr channel, which records the difference from red, and the Cb channel, - * which records the difference from blue. Because chroma and luma are - * not related in YCrCb, vision code written to look for certain values - * in the Cr/Cb channels will not be severely affected by differing - * light intensity, since that difference would most likely just be - * reflected in the Y channel. - * - * After we've converted to YCrCb, we extract just the 2nd channel, the - * Cb channel. We do this because stones are bright yellow and contrast - * STRONGLY on the Cb channel against everything else, including SkyStones - * (because SkyStones have a black label). - * - * We then take the average pixel value of 3 different regions on that Cb - * channel, one positioned over each stone. The brightest of the 3 regions - * is where we assume the SkyStone to be, since the normal stones show up - * extremely darkly. - * - * We also draw rectangles on the screen showing where the sample regions - * are, as well as drawing a solid rectangle over top the sample region - * we believe is on top of the SkyStone. - * - * In order for this whole process to work correctly, each sample region - * should be positioned in the center of each of the first 3 stones, and - * be small enough such that only the stone is sampled, and not any of the - * surroundings. - */ - - /* - * Get the Cb channel of the input frame after conversion to YCrCb - */ - inputToCb(input); - - /* - * Compute the average pixel value of each submat region. We're - * taking the average of a single channel buffer, so the value - * we need is at index 0. We could have also taken the average - * pixel value of the 3-channel image, and referenced the value - * at index 2 here. - */ - avg1 = (int) Core.mean(region1_Cb).val[0]; - avg2 = (int) Core.mean(region2_Cb).val[0]; - avg3 = (int) Core.mean(region3_Cb).val[0]; - - /* - * Draw a rectangle showing sample region 1 on the screen. - * Simply a visual aid. Serves no functional purpose. - */ - Imgproc.rectangle( - input, // Buffer to draw on - region1_pointA, // First point which defines the rectangle - region1_pointB, // Second point which defines the rectangle - BLUE, // The color the rectangle is drawn in - 2 - ); // Thickness of the rectangle lines - - /* - * Draw a rectangle showing sample region 2 on the screen. - * Simply a visual aid. Serves no functional purpose. - */ - Imgproc.rectangle( - input, // Buffer to draw on - region2_pointA, // First point which defines the rectangle - region2_pointB, // Second point which defines the rectangle - BLUE, // The color the rectangle is drawn in - 2 - ); // Thickness of the rectangle lines - - /* - * Draw a rectangle showing sample region 3 on the screen. - * Simply a visual aid. Serves no functional purpose. - */ - Imgproc.rectangle( - input, // Buffer to draw on - region3_pointA, // First point which defines the rectangle - region3_pointB, // Second point which defines the rectangle - BLUE, // The color the rectangle is drawn in - 2 - ); // Thickness of the rectangle lines - - /* - * Find the max of the 3 averages - */ - int maxOneTwo = Math.max(avg1, avg2); - int max = Math.max(maxOneTwo, avg3); - - /* - * Now that we found the max, we actually need to go and - * figure out which sample region that value was from - */ - if ( - max == avg1 - ) { // Was it from region 1? - position = SkystonePosition.LEFT; // Record our analysis - - /* - * Draw a solid rectangle on top of the chosen region. - * Simply a visual aid. Serves no functional purpose. - */ - Imgproc.rectangle( - input, // Buffer to draw on - region1_pointA, // First point which defines the rectangle - region1_pointB, // Second point which defines the rectangle - GREEN, // The color the rectangle is drawn in - -1 - ); // Negative thickness means solid fill - } else if ( - max == avg2 - ) { // Was it from region 2? - position = SkystonePosition.CENTER; // Record our analysis - - /* - * Draw a solid rectangle on top of the chosen region. - * Simply a visual aid. Serves no functional purpose. - */ - Imgproc.rectangle( - input, // Buffer to draw on - region2_pointA, // First point which defines the rectangle - region2_pointB, // Second point which defines the rectangle - GREEN, // The color the rectangle is drawn in - -1 - ); // Negative thickness means solid fill - } else if ( - max == avg3 - ) { // Was it from region 3? - position = SkystonePosition.RIGHT; // Record our analysis - - /* - * Draw a solid rectangle on top of the chosen region. - * Simply a visual aid. Serves no functional purpose. - */ - Imgproc.rectangle( - input, // Buffer to draw on - region3_pointA, // First point which defines the rectangle - region3_pointB, // Second point which defines the rectangle - GREEN, // The color the rectangle is drawn in - -1 - ); // Negative thickness means solid fill - } - - /* - * Render the 'input' buffer to the viewport. But note this is not - * simply rendering the raw camera feed, because we called functions - * to add some annotations to this buffer earlier up. - */ - return input; - } - - /* - * Call this from the OpMode thread to obtain the latest analysis - */ - public SkystonePosition getAnalysis() { - return position; - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SampleMecanumDrive.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SampleMecanumDrive.java deleted file mode 100644 index e2bf55f1..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SampleMecanumDrive.java +++ /dev/null @@ -1,496 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import androidx.annotation.NonNull; -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.canvas.Canvas; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.control.PIDCoefficients; -import com.acmerobotics.roadrunner.control.PIDFController; -import com.acmerobotics.roadrunner.drive.DriveSignal; -import com.acmerobotics.roadrunner.drive.MecanumDrive; -import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower; -import com.acmerobotics.roadrunner.followers.TrajectoryFollower; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.profile.MotionProfile; -import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; -import com.acmerobotics.roadrunner.profile.MotionState; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; -import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; -import com.acmerobotics.roadrunner.util.NanoClock; -import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import com.qualcomm.robotcore.hardware.VoltageSensor; -import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; -import com.technototes.path.util.AxesSigns; -import com.technototes.path.util.BNO055IMUUtil; -import com.technototes.path.util.DashboardUtil; -import com.technototes.path.util.LynxModuleUtil; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.LinkedList; -import java.util.List; -import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; - -/* - * Simple mecanum drive hardware implementation for REV hardware. - */ -@SuppressWarnings("unused") -@Disabled -@Config -public class SampleMecanumDrive extends MecanumDrive { - - /* - * These are motor constants that should be listed online for your motors. - */ - public static final double TICKS_PER_REV = 28; - public static final double MAX_RPM = 6000; - - /* - * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. - * Set this flag to false if drive encoders are not present and an alternative localization - * method is in use (e.g., tracking wheels). - * - * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients - * from DriveVelocityPIDTuner. - */ - public static final boolean RUN_USING_ENCODER = true; - public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients( - 10, - 0, - 0, - getMotorVelocityF((MAX_RPM / 60) * TICKS_PER_REV) - ); - - /* - * These are physical constants that can be determined from your robot (including the track - * width; it will be tune empirically later although a rough estimate is important). Users are - * free to chose whichever linear distance unit they would like so long as it is consistently - * used. The default values were selected with inches in mind. Road runner uses radians for - * angular distances although most angular parameters are wrapped in Math.toRadians() for - * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. - */ - public static double WHEEL_RADIUS = 1.88976; // in - public static double GEAR_RATIO = 1 / 15.6; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 16.4; // in - - /* - * These are the feedforward parameters used to model the drive motor behavior. If you are using - * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive - * motor encoders or have elected not to use them for velocity control, these values should be - * empirically tuned. - */ - public static double kV = 1.0 / rpmToVelocity(MAX_RPM); - public static double kA = 0; - public static double kStatic = 0; - - /* - * These values are used to generate the trajectories for you robot. To ensure proper operation, - * the constraints should never exceed ~80% of the robot's actual capabilities. While Road - * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start - * small and gradually increase them later after everything is working. All distance units are - * inches. - */ - public static double MAX_VEL = 40; - public static double MAX_ACCEL = 30; - public static double MAX_ANG_VEL = Math.toRadians(60); - public static double MAX_ANG_ACCEL = Math.toRadians(60); - - public static double encoderTicksToInches(double ticks) { - return (WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks) / TICKS_PER_REV; - } - - public static double rpmToVelocity(double rpm) { - return (rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS) / 60.0; - } - - public static double getMotorVelocityF(double ticksPerSecond) { - // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx - return 32767 / ticksPerSecond; - } - - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); - - public static double LATERAL_MULTIPLIER = 1; - - public static double VX_WEIGHT = 1; - public static double VY_WEIGHT = 1; - public static double OMEGA_WEIGHT = 1; - - public static int POSE_HISTORY_LIMIT = 100; - - public enum Mode { - IDLE, - TURN, - FOLLOW_TRAJECTORY, - } - - private FtcDashboard dashboard; - private NanoClock clock; - - private Mode mode; - - private PIDFController turnController; - private MotionProfile turnProfile; - private double turnStart; - - private TrajectoryVelocityConstraint velConstraint; - private TrajectoryAccelerationConstraint accelConstraint; - private TrajectoryFollower follower; - - private LinkedList poseHistory; - - public DcMotorEx leftFront, leftRear, rightRear, rightFront; - private List motors; - private BNO055IMU imu; - - private VoltageSensor batteryVoltageSensor; - - private Pose2d lastPoseOnTurn; - - public SampleMecanumDrive(HardwareMap hardwareMap) { - super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER); - dashboard = FtcDashboard.getInstance(); - dashboard.setTelemetryTransmissionInterval(25); - - clock = NanoClock.system(); - - mode = Mode.IDLE; - - turnController = new PIDFController(HEADING_PID); - turnController.setInputBounds(0, 2 * Math.PI); - - velConstraint = new MinVelocityConstraint( - Arrays.asList( - new AngularVelocityConstraint(MAX_ANG_VEL), - new MecanumVelocityConstraint(MAX_VEL, TRACK_WIDTH) - ) - ); - accelConstraint = new ProfileAccelerationConstraint(MAX_ACCEL); - follower = new HolonomicPIDVAFollower( - TRANSLATIONAL_PID, - TRANSLATIONAL_PID, - HEADING_PID, - new Pose2d(0.5, 0.5, Math.toRadians(5.0)), - 0.5 - ); - - poseHistory = new LinkedList<>(); - - LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); - - batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); - - for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { - module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); - } - - imu = hardwareMap.get(BNO055IMU.class, "imu"); - BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); - parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; - imu.initialize(parameters); - - BNO055IMUUtil.remapAxes(imu, AxesOrder.YXZ, AxesSigns.NNN); - - rightFront = hardwareMap.get(DcMotorEx.class, "frMotor"); - leftFront = hardwareMap.get(DcMotorEx.class, "flMotor"); - leftRear = hardwareMap.get(DcMotorEx.class, "rlMotor"); - rightRear = hardwareMap.get(DcMotorEx.class, "rrMotor"); - - motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); - - for (DcMotorEx motor : motors) { - MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); - motorConfigurationType.setAchieveableMaxRPMFraction(1.0); - motor.setMotorType(motorConfigurationType); - } - - if (RUN_USING_ENCODER) { - setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - - setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { - setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); - } - - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); - //setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); - } - - public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { - return new TrajectoryBuilder(startPose, velConstraint, accelConstraint); - } - - public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { - return new TrajectoryBuilder(startPose, reversed, velConstraint, accelConstraint); - } - - public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { - return new TrajectoryBuilder(startPose, startHeading, velConstraint, accelConstraint); - } - - public void turnAsync(double angle) { - double heading = getPoseEstimate().getHeading(); - - lastPoseOnTurn = getPoseEstimate(); - - turnProfile = MotionProfileGenerator.generateSimpleMotionProfile( - new MotionState(heading, 0, 0, 0), - new MotionState(heading + angle, 0, 0, 0), - MAX_ANG_VEL, - MAX_ANG_ACCEL - ); - - turnStart = clock.seconds(); - mode = Mode.TURN; - } - - public void turn(double angle) { - turnAsync(angle); - waitForIdle(); - } - - public void followTrajectoryAsync(Trajectory trajectory) { - follower.followTrajectory(trajectory); - mode = Mode.FOLLOW_TRAJECTORY; - } - - public void followTrajectory(Trajectory trajectory) { - followTrajectoryAsync(trajectory); - waitForIdle(); - } - - public Pose2d getLastError() { - switch (mode) { - case FOLLOW_TRAJECTORY: - return follower.getLastError(); - case TURN: - return new Pose2d(0, 0, turnController.getLastError()); - case IDLE: - return new Pose2d(); - } - throw new AssertionError(); - } - - public void update() { - updatePoseEstimate(); - - Pose2d currentPose = getPoseEstimate(); - Pose2d lastError = getLastError(); - - poseHistory.add(currentPose); - - if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) { - poseHistory.removeFirst(); - } - - TelemetryPacket packet = new TelemetryPacket(); - Canvas fieldOverlay = packet.fieldOverlay(); - - packet.put("mode", mode); - - packet.put("x", currentPose.getX()); - packet.put("y", currentPose.getY()); - packet.put("heading (deg)", Math.toDegrees(currentPose.getHeading())); - - packet.put("xError", lastError.getX()); - packet.put("yError", lastError.getY()); - packet.put("headingError (deg)", Math.toDegrees(lastError.getHeading())); - - switch (mode) { - case IDLE: - // do nothing - break; - case TURN: { - double t = clock.seconds() - turnStart; - - MotionState targetState = turnProfile.get(t); - - turnController.setTargetPosition(targetState.getX()); - - double correction = turnController.update(currentPose.getHeading()); - - double targetOmega = targetState.getV(); - double targetAlpha = targetState.getA(); - setDriveSignal( - new DriveSignal( - new Pose2d(0, 0, targetOmega + correction), - new Pose2d(0, 0, targetAlpha) - ) - ); - - Pose2d newPose = lastPoseOnTurn.copy( - lastPoseOnTurn.getX(), - lastPoseOnTurn.getY(), - targetState.getX() - ); - - fieldOverlay.setStroke("#4CAF50"); - DashboardUtil.drawRobot(fieldOverlay, newPose); - - if (t >= turnProfile.duration()) { - mode = Mode.IDLE; - setDriveSignal(new DriveSignal()); - } - - break; - } - case FOLLOW_TRAJECTORY: { - setDriveSignal(follower.update(currentPose, getPoseVelocity())); - - Trajectory trajectory = follower.getTrajectory(); - - fieldOverlay.setStrokeWidth(1); - fieldOverlay.setStroke("#4CAF50"); - DashboardUtil.drawSampledPath(fieldOverlay, trajectory.getPath()); - double t = follower.elapsedTime(); - DashboardUtil.drawRobot(fieldOverlay, trajectory.get(t)); - - fieldOverlay.setStroke("#3F51B5"); - DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory); - - if (!follower.isFollowing()) { - mode = Mode.IDLE; - setDriveSignal(new DriveSignal()); - } - - break; - } - } - - fieldOverlay.setStroke("#3F51B5"); - DashboardUtil.drawRobot(fieldOverlay, currentPose); - - dashboard.sendTelemetryPacket(packet); - } - - public void waitForIdle() { - while (!Thread.currentThread().isInterrupted() && isBusy()) { - update(); - } - } - - public boolean isBusy() { - return mode != Mode.IDLE; - } - - public void setMode(DcMotor.RunMode runMode) { - for (DcMotorEx motor : motors) { - motor.setMode(runMode); - } - } - - public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { - for (DcMotorEx motor : motors) { - motor.setZeroPowerBehavior(zeroPowerBehavior); - } - } - - public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { - PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( - coefficients.p, - coefficients.i, - coefficients.d, - (coefficients.f * 12) / batteryVoltageSensor.getVoltage() - ); - for (DcMotorEx motor : motors) { - motor.setPIDFCoefficients(runMode, compensatedCoefficients); - } - } - - public void setWeightedDrivePower(Pose2d drivePower) { - Pose2d vel = drivePower; - - if ( - Math.abs(drivePower.getX()) + - Math.abs(drivePower.getY()) + - Math.abs(drivePower.getHeading()) > - 1 - ) { - // re-normalize the powers according to the weights - double denom = - VX_WEIGHT * Math.abs(drivePower.getX()) + - VY_WEIGHT * Math.abs(drivePower.getY()) + - OMEGA_WEIGHT * Math.abs(drivePower.getHeading()); - - vel = new Pose2d( - VX_WEIGHT * drivePower.getX(), - VY_WEIGHT * drivePower.getY(), - OMEGA_WEIGHT * drivePower.getHeading() - ).div(denom); - } - - setDrivePower(vel); - } - - @NonNull - @Override - public List getWheelPositions() { - List wheelPositions = new ArrayList<>(); - for (DcMotorEx motor : motors) { - wheelPositions.add(encoderTicksToInches(motor.getCurrentPosition())); - } - return wheelPositions; - } - - @Override - public List getWheelVelocities() { - List wheelVelocities = new ArrayList<>(); - for (DcMotorEx motor : motors) { - wheelVelocities.add(encoderTicksToInches(motor.getVelocity())); - } - return wheelVelocities; - } - - @Override - public void setMotorPowers(double v, double v1, double v2, double v3) { - leftFront.setPower(v); - leftRear.setPower(v1); - rightRear.setPower(v2); - rightFront.setPower(v3); - } - - @Override - public double getRawExternalHeading() { - return imu.getAngularOrientation().firstAngle; - } - - @Override - public Double getExternalHeadingVelocity() { - // TODO: This must be changed to match your configuration - // | Z axis - // | - // (Motor Port Side) | / X axis - // ____|__/____ - // Y axis / * | / /| (IO Side) - // _________ /______|/ // I2C - // /___________ // Digital - // |____________|/ Analog - // - // (Servo Port Side) - // - // The positive x axis points toward the USB port(s) - // - // Adjust the axis rotation rate as necessary - // Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying - // flat on a surface - - return (double) imu.getAngularVelocity().xRotationRate; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SplineTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SplineTest.java deleted file mode 100644 index 11b28d5d..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SplineTest.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.technototes.library.hardware.HardwareDevice; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -/* - * This is an example of a more complex path to really test the tuning. - */ -@Disabled -@Autonomous(group = "drive") -public class SplineTest extends LinearOpMode { - - @Override - public void runOpMode() throws InterruptedException { - HardwareDevice.hardwareMap = hardwareMap; - - DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); - // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - waitForStart(); - - if (isStopRequested()) return; - - Trajectory traj = drive - .trajectoryBuilder(new Pose2d()) - .splineTo(new Vector2d(70, 10), 0) - .build(); - - drive.followTrajectory(traj); - sleep(2000); - - drive.followTrajectory( - drive - .trajectoryBuilder(traj.end(), true) - .splineTo(new Vector2d(0, 0), Math.toRadians(180)) - .build() - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StrafeTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StrafeTest.java deleted file mode 100644 index 06974ced..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StrafeTest.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.technototes.library.hardware.HardwareDevice; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -/* - * This is a simple routine to test translational drive capabilities. - */ -@Disabled -@Config -@Autonomous(group = "drive") -public class StrafeTest extends LinearOpMode { - - public static double DISTANCE = 60; // in - - @Override - public void runOpMode() throws InterruptedException { - HardwareDevice.hardwareMap = hardwareMap; - - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - - DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); - // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()).strafeRight(DISTANCE).build(); - - waitForStart(); - - if (isStopRequested()) return; - - drive.followTrajectory(trajectory); - - Pose2d poseEstimate = drive.getPoseEstimate(); - telemetry.addData("finalX", poseEstimate.getX()); - telemetry.addData("finalY", poseEstimate.getY()); - telemetry.addData("finalHeading", poseEstimate.getHeading()); - telemetry.update(); - - while (!isStopRequested() && opModeIsActive()); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StraightTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StraightTest.java deleted file mode 100644 index bd1cb059..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StraightTest.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.technototes.library.hardware.HardwareDevice; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -/* - * This is a simple routine to test translational drive capabilities. - */ -@Disabled -@Config -@Autonomous(group = "drive") -public class StraightTest extends LinearOpMode { - - public static double DISTANCE = 60; // in - - @Override - public void runOpMode() throws InterruptedException { - HardwareDevice.hardwareMap = hardwareMap; - - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - - DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); - // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()).forward(DISTANCE).build(); - - waitForStart(); - - if (isStopRequested()) return; - - drive.followTrajectory(trajectory); - - Pose2d poseEstimate = drive.getPoseEstimate(); - telemetry.addData("finalX", poseEstimate.getX()); - telemetry.addData("finalY", poseEstimate.getY()); - telemetry.addData("finalHeading", poseEstimate.getHeading()); - telemetry.update(); - - while (!isStopRequested() && opModeIsActive()); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/TurnTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/TurnTest.java deleted file mode 100644 index 051bc940..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/TurnTest.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.technototes.library.hardware.HardwareDevice; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -/* - * This is a simple routine to test turning capabilities. - */ -@Disabled -@Config -@Autonomous(group = "drive") -public class TurnTest extends LinearOpMode { - - public static double ANGLE = 90; // deg - - @Override - public void runOpMode() throws InterruptedException { - HardwareDevice.hardwareMap = hardwareMap; - - DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); - // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - waitForStart(); - - if (isStopRequested()) return; - - drive.turn(Math.toRadians(ANGLE)); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java new file mode 100644 index 00000000..6302c59d --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; +import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; +import org.firstinspires.ftc.ptechnodactyl.controllers.OperatorController; +import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; + +@TeleOp(name = "PT Driving w/Turbo!") +@SuppressWarnings("unused") +public class JustDrivingTeleOp extends CommandOpMode implements Loggable { + + public Robot robot; + public DriverController controlsDriver; + public OperatorController controlsOperator; + public Hardware hardware; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + controlsOperator = new OperatorController(codriverGamepad, robot); + if (Setup.Connected.DRIVEBASE) { + controlsDriver = new DriverController(driverGamepad, robot); + CommandScheduler.scheduleForState( + EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem), + OpModeState.INIT + ); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java new file mode 100644 index 00000000..d65a0b52 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; +import org.firstinspires.ftc.ptechnodactyl.controllers.TestController; + +@TeleOp(name = "MotorTest") +@SuppressWarnings("unused") +public class MotorTest extends CommandOpMode implements Loggable { + + public Hardware hardware; + public Robot robot; + + public TestController test; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + test = new TestController(driverGamepad, robot); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java new file mode 100644 index 00000000..a6501b33 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; + +@TeleOp(name = "PlainDrive") +@SuppressWarnings("unused") +public class PlainDrive extends CommandOpMode implements Loggable { + + public Hardware hardware; + public Robot robot; + + public DriverController driver; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + driver = new DriverController(driverGamepad, robot); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/TeleOpBase.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/TeleOpBase.java deleted file mode 100644 index 122fe08c..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/TeleOpBase.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; - -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import com.technototes.library.subsystem.Subsystem; -import com.technototes.library.util.Alliance; -import org.firstinspires.ftc.ptechnodactyl.Controls; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; - -public abstract class TeleOpBase extends CommandOpMode implements Loggable { - - public Robot robot; - public Hardware hardware; - public Controls controls; - - @Override - public void uponInit() { - RobotConstants.updateAlliance(Alliance.get(getClass())); - hardware = new Hardware(); - robot = new Robot(hardware); - controls = new Controls(robot, driverGamepad, codriverGamepad); - } - - @Override - public void uponStart() { - if (Robot.SubsystemConstants.CAP_ENABLED) robot.capSubsystem.up(); - if (Robot.SubsystemConstants.DRIVE_ENABLED) { - robot.drivebaseSubsystem.resetGyro(); - robot.drivebaseSubsystem.setExternalHeading(Math.toRadians(180)); - } - } - - @TeleOp(name = "\uD83D\uDFE5 \uD83C\uDFAE Red TeleOp") - @Alliance.Red - public static class RedTeleOp extends TeleOpBase {} - - @TeleOp(name = "\uD83D\uDD35 \uD83C\uDFAE Blue Teleop") - @Alliance.Blue - public static class BlueTeleOp extends TeleOpBase {} -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java new file mode 100644 index 00000000..69cd1dc5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; +import org.firstinspires.ftc.ptechnodactyl.controllers.OneController; + +@TeleOp(name = "OneController") +@SuppressWarnings("unused") +public class oneController extends CommandOpMode implements Loggable { + + public Hardware hardware; + public Robot robot; + + public OneController driver; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + driver = new OneController(driverGamepad, robot); + CommandScheduler.register(robot.clawSubsystem); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java deleted file mode 100644 index 5da6730a..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java +++ /dev/null @@ -1,155 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.ARM_CONSTRAINTS; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.CARRY; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.COLLECT; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DIFFERENTIAL; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DOWN; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DUMP; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.FAKE_CARRY; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.IN; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.OUT; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.SLIGHT_CARRY; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.UP; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.servo.Servo; -import com.technototes.library.hardware.servo.ServoProfiler; -import com.technototes.library.subsystem.Subsystem; -import java.util.function.Supplier; - -@SuppressWarnings("unused") -public class ArmSubsystem implements Subsystem, Supplier { - - /** - * Deposit is am arm to hold and drop freight - * - * @Config allows you to mess with variables without messing with the code - * Especially good when trying to set the position of different mechanisms - */ - @Config - public static class ArmConstants { - - //public static double MIN = 0, MAX = 0.5; - public static double DUMP = 0.55, CARRY = 0.25, FAKE_CARRY = 0.15, COLLECT = - 0.03, AUTO_CARRY = 0.3, SLIGHT_CARRY = 0.15; - public static double IN = 0.02, UP = 0.3, OUT = 0.6, DOWN = 0.75; - public static double DIFFERENTIAL = 2.8; - public static ServoProfiler.Constraints ARM_CONSTRAINTS = new ServoProfiler.Constraints( - 5, - 5, - 5 - ); - } - - public Servo dumpServo; - public Servo armServo; - public ServoProfiler armController; - - public ArmSubsystem(Servo l, Servo r) { - dumpServo = l; - armServo = r; - armController = new ServoProfiler(armServo) - .setConstraints(ARM_CONSTRAINTS) - .setTargetPosition(UP); - } - - /** - * Sets the servo controlling the cup to a constant value which dumps the freight - * to a target - */ - public void dump() { - setDump(DUMP); - } - - /** - * Sets a servo controlling the cup to a constant value which carries the freight while - * the robot is moving - */ - public void carry() { - setDump(CARRY); - } - - public void fakeCarry() { - setDump(FAKE_CARRY); - } - - /** - * Sets a servo to a constant value which puts the cup in position to take the freight once - * it's intaked - */ - public void collect() { - setDump(COLLECT); - } - - /** - * Sets a servo to a custom constant value based off the specific position the driver wants the cup - * to be when dumping the freight - */ - public void setDump(double v) { - targetDumpPosition = Range.clip(v, COLLECT, DUMP); - } - - /** - * Sets the arm servo to a constant value to retract the arm of the robot - */ - public void up() { - setArm(UP); - } - - /** - * Sets the arm servo to a constant value to retract the arm of the robot - */ - public void in() { - setArm(IN); - } - - /** - * Sets the arm servo to a constant value to extend the arm of the robot - */ - public void out() { - setArm(OUT); - } - - public void down() { - setArm(DOWN); - } - - /** - * Sets the arm servo to a custom constant value to extend the arm of the robot - */ - public void setArm(double v) { - armController.setTargetPosition(Range.clip(v, IN, DOWN)); - } - - /** - * Sets a custom value that will add to the Arm servo's value, setting it to a new position - * for extension - */ - public void translateArm(double v) { - setArm(armServo.getPosition() + v); - } - - /** - * Method to display the extension and dump value on the telemetry to the driver knows - * how much they are extending the arm and positioning the cup - */ - @Override - public String get() { - return "ARM: " + armServo.getPosition() + ", DUMP: " + targetDumpPosition; - //return "EXTENSION: "+differential.getAverage()+", DUMP: "+differential.getDeviation(); - } - - public void slightCarry() { - setDump(SLIGHT_CARRY); - } - - private double targetDumpPosition = 0.2; - - @Override - public void periodic() { - armController.update(); - dumpServo.setPosition(targetDumpPosition + armServo.getPosition() / DIFFERENTIAL); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BarcodePipeline.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BarcodePipeline.java deleted file mode 100644 index 50067076..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BarcodePipeline.java +++ /dev/null @@ -1,192 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.opencv.core.Core; -import org.opencv.core.Mat; -import org.opencv.core.MatOfPoint; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.Point; -import org.opencv.core.Rect; -import org.opencv.core.Scalar; -import org.opencv.core.Size; -import org.opencv.imgproc.Imgproc; -import org.openftc.easyopencv.OpenCvPipeline; - -import java.util.ArrayList; -import java.util.List; -import java.util.function.Supplier; -import com.acmerobotics.dashboard.config.Config; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.BarcodePipeline.BarcodeConstants.*; -public class BarcodePipeline extends OpenCvPipeline implements Supplier { - @Config - public static class BarcodeConstants { - public static boolean DISPLAY = true; - public static Scalar DISPLAY_COLOR = new Scalar(200, 0, 0); - public static Scalar LOWER_LIMIT = new Scalar(80.0, 0.0, 0.0, 0.0); - public static Scalar UPPER_LIMIT = new Scalar(255.0, 80.0, 80.0, 255.0); - public static int BORDER_LEFT_X = 0; //amount of pixels from the left side of the cam to skip - public static int BORDER_RIGHT_X = 0; //amount of pixels from the right of the cam to skip - public static int BORDER_TOP_Y = 90; //amount of pixels from the top of the cam to skip - public static int BORDER_BOTTOM_Y = 50; //amount of pixels from the bottom of the cam to skip - - //y is fot the outpiut - public static Point LEFT = new Point(50, 140); - public static Point CENTER = new Point(160, 140); - public static Point RIGHT = new Point(270, 140); - - public static int VARIANCE = 50; - public static double MIN_AREA = 1000; - - - } - - public Exception debug; - - - private int loopcounter = 0; - private int ploopcounter = 0; - - private final Mat mat = new Mat(); - private final Mat processed = new Mat(); - - private Rect maxRect = new Rect(); - - private double maxArea = 0; - private boolean first = false; - - public Telemetry telemetry; - - public BarcodePipeline(Telemetry t){ - telemetry = t; - } - public BarcodePipeline(){ - } - - @Override - public Mat processFrame(Mat input) - { - try - { - // Process Image - Imgproc.cvtColor(input, mat, Imgproc.COLOR_RGB2RGBA); - Core.inRange(mat, LOWER_LIMIT, UPPER_LIMIT, processed); - // Core.bitwise_and(input, input, output, processed); - - // Remove Noise - Imgproc.morphologyEx(processed, processed, Imgproc.MORPH_OPEN, new Mat()); - Imgproc.morphologyEx(processed, processed, Imgproc.MORPH_CLOSE, new Mat()); - // GaussianBlur - Imgproc.GaussianBlur(processed, processed, new Size(5.0, 15.0), 0.00); - // Find Contours - List contours = new ArrayList<>(); - Imgproc.findContours(processed, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE); - - // Draw Contours - if(DISPLAY) Imgproc.drawContours(input, contours, -1, DISPLAY_COLOR); - - // Loop Through Contours - for (MatOfPoint contour : contours) - { - Point[] contourArray = contour.toArray(); - - // Bound Rectangle if Contour is Large Enough - if (contourArray.length >= 15) - { - MatOfPoint2f areaPoints = new MatOfPoint2f(contourArray); - Rect rect = Imgproc.boundingRect(areaPoints); - - // if rectangle is larger than previous cycle or if rectangle is not larger than previous 6 cycles > then replace - if (rect.area() > maxArea - && rect.x > BORDER_LEFT_X && rect.x + rect.width < input.width() - BORDER_RIGHT_X - && rect.y > BORDER_TOP_Y && rect.y + rect.height < input.height() - BORDER_BOTTOM_Y - || loopcounter - ploopcounter > 6) - { - maxArea = rect.area(); - maxRect = rect; - ploopcounter++; - loopcounter = ploopcounter; - first = true; - } - areaPoints.release(); - } - contour.release(); - } - mat.release(); - processed.release(); - if (contours.isEmpty()) - { - maxRect = new Rect(); - } - if (first && maxRect.area() > MIN_AREA) - { - if(DISPLAY) Imgproc.rectangle(input, maxRect, DISPLAY_COLOR, 2); - } - // Draw Borders - if(DISPLAY) { - Imgproc.rectangle(input, new Rect(BORDER_LEFT_X, BORDER_TOP_Y, input.width() - BORDER_RIGHT_X - BORDER_LEFT_X, input.height() - BORDER_BOTTOM_Y - BORDER_TOP_Y), DISPLAY_COLOR, 2); - Imgproc.circle(input, LEFT, VARIANCE, DISPLAY_COLOR); - Imgproc.circle(input, CENTER, VARIANCE, DISPLAY_COLOR); - Imgproc.circle(input, RIGHT, VARIANCE, DISPLAY_COLOR); - - // Display Data - - Imgproc.putText(input, "Area: " + getRectArea() + " Midpoint: " + getRectMidpointXY().x + " , " + getRectMidpointXY().y+ " Selection: "+get(), new Point(20, input.height() - 20), Imgproc.FONT_HERSHEY_PLAIN, 0.6, DISPLAY_COLOR, 1); - } - loopcounter++; - } catch (Exception e) { - debug = e; - boolean error = true; - } - if(telemetry != null){ - telemetry.addLine(get().toString()); - telemetry.update(); - } - - try { - Thread.sleep(100); - } catch (InterruptedException ignored) { - } - - return input; - } - public int getRectHeight(){return maxRect.height;} - public int getRectWidth(){ return maxRect.width; } - public int getRectX(){ return maxRect.x; } - public int getRectY(){ return maxRect.y; } - public double getRectMidpointX(){ return getRectX() + (getRectWidth()/2.0); } - public double getRectMidpointY(){ return getRectY() + (getRectHeight()/2.0); } - public Point getRectMidpointXY(){ return new Point(getRectMidpointX(), getRectMidpointY());} - public double getRectArea(){ return maxRect.area(); } - - private int last = -1; - @Override - public synchronized Integer get() { - if(getRectArea()>MIN_AREA) { - Point p = getRectMidpointXY(); - if (Math.abs(p.x - LEFT.x) < VARIANCE && Math.abs(p.y - LEFT.y) < VARIANCE) last = 0; - if (Math.abs(p.x - CENTER.x) < VARIANCE && Math.abs(p.y - CENTER.y) < VARIANCE) last = 1; - if (Math.abs(p.x - RIGHT.x) < VARIANCE && Math.abs(p.y - RIGHT.y) < VARIANCE) last = 2; - } - return last; - } - - public boolean none(){ - return get() == -1; - } - public boolean zero(){ - return get() == 0; - } - public boolean one(){ - return get() == 1; - } - public boolean two(){ - return get() == 2; - } - - public boolean twoOrDefault(){ - return none() || two(); - } - - -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java deleted file mode 100644 index cad54e6c..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import com.technototes.library.hardware.servo.Servo; -import com.technototes.library.subsystem.Subsystem; -import java.util.function.Supplier; - -public class BrakeSubsystem implements Subsystem, Supplier { - - @com.acmerobotics.dashboard.config.Config - public static class BrakeConstants { - - public static double UP = 0.02, DOWN = 0.34; - } - - private boolean isBraking; - public Servo servo; - - public BrakeSubsystem(Servo s) { - servo = s; - isBraking = false; - } - - public void raise() { - servo.setPosition(BrakeConstants.UP); - isBraking = false; - } - - public void lower() { - servo.setPosition(BrakeConstants.DOWN); - isBraking = true; - } - - @Override - public Boolean get() { - return isBraking; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java deleted file mode 100644 index d20e9640..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java +++ /dev/null @@ -1,107 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem.CapConstants.*; - -import com.acmerobotics.dashboard.config.Config; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.hardware.servo.Servo; -import com.technototes.library.hardware.servo.ServoProfiler; -import com.technototes.library.subsystem.Subsystem; -import java.util.function.Supplier; - -public class CapSubsystem implements Subsystem, Supplier { - - @Config - public static class CapConstants { - - public static double TURRET_INIT = 0.8, TURRET_PICKUP = 0.1, TURRET_CARRY = - 0.8, TURRET_CAP = 0.5; - public static double CLAW_OPEN = 0.1, CLAW_CLOSE = 0.53; - public static double ARM_UP = 1, ARM_CAP = 0.85, ARM_INIT = 0.4, ARM_DOWN = 0.05; - public static ServoProfiler.Constraints ARM_CONSTRAINTS = new ServoProfiler.Constraints( - 5, - 5, - 5 - ); - public static ServoProfiler.Constraints TURRET_CONSTRAINTS = new ServoProfiler.Constraints( - 3, - 2, - 2 - ); - } - - public Servo armServo; - public Servo clawServo; - - public Servo turretServo; - - public ServoProfiler armProfiler; - public ServoProfiler turretProfiler; - - public CapSubsystem(Servo arm, Servo claw, Servo turret) { - CommandScheduler.register(this); - armServo = arm; - clawServo = claw; - turretServo = turret; - armProfiler = new ServoProfiler(armServo) - .setServoRange(0.4) - .setConstraints(ARM_CONSTRAINTS) - .setTargetPosition(ARM_INIT); - turretProfiler = new ServoProfiler(turretServo) - .setConstraints(TURRET_CONSTRAINTS) - .setTargetPosition(TURRET_INIT); - } - - public void open() { - clawServo.setPosition(CLAW_OPEN); - } - - public void close() { - clawServo.setPosition(CLAW_CLOSE); - } - - public void up() { - armProfiler.setTargetPosition(ARM_UP); - turretProfiler.setTargetPosition(TURRET_CARRY); - clawServo.setPosition(CLAW_CLOSE); - } - - public void raise() { - armProfiler.setTargetPosition(ARM_CAP); - } - - public void raise2() { - turretProfiler.setTargetPosition(TURRET_CAP); - } - - public void down() { - armProfiler.setTargetPosition(ARM_DOWN); - turretProfiler.setTargetPosition(TURRET_PICKUP); - } - - public void translateArm(double translation) { - armProfiler.translateTargetPosition(translation); - } - - public void translateTurret(double translation) { - turretProfiler.translateTargetPosition(translation); - } - - @Override - public void periodic() { - turretProfiler.update(); - armProfiler.update(); - } - - @Override - public String get() { - return ( - "CLAW: " + - clawServo.getPosition() + - ", ARM: " + - armProfiler.getTargetPosition() + - ", TURRET: " + - turretProfiler.getTargetPosition() - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CarouselSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CarouselSubsystem.java deleted file mode 100644 index 1f3c907f..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CarouselSubsystem.java +++ /dev/null @@ -1,70 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.technototes.library.hardware.motor.Motor; -import com.technototes.library.subsystem.Subsystem; - -import java.util.function.Supplier; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.AUTO_SPEED; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.CAROUSEL_STOP_SPEED; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.MAX_SPEED; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.MIN_SPEED; - - -@SuppressWarnings("unused") - -public class CarouselSubsystem implements Subsystem, Supplier { - - /** - * Carousel Constants for measuring speed of carousel motor - */ - - @Config - public static class CarouselConstants{ - public static double MAX_SPEED = 1; - public static double MIN_SPEED = 0.2; - public static double AUTO_SPEED = 0.2; - - public static double CAROUSEL_STOP_SPEED = 0; - public static double SPIN_OFFSET = 3.5; - } - - public Motor motor; - - public CarouselSubsystem(Motor m){ - motor = m; - } - - public void right(){motor.setSpeed(AUTO_SPEED);} - - public void right(double speed){motor.setSpeed(MAX_SPEED*Math.max(MIN_SPEED, speed));} - - /** - * when called by Carousel Right Command, sets motor to turn carousel right - */ - - public void left(){motor.setSpeed(-AUTO_SPEED);} - public void left(double speed){motor.setSpeed(-MAX_SPEED*Math.max(MIN_SPEED, speed));} - - /** - * when called by Carousel Left Command, sets motor to turn carousel left - */ - - public void stop(){motor.setSpeed(CAROUSEL_STOP_SPEED);} - - /** - * when called by Carousel Stop Command, stops carousel motor - */ - - @Override - public Double get() { - return motor.getSpeed(); - /** - * gets current motor speed - */ - } - -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java new file mode 100644 index 00000000..8ae36651 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java @@ -0,0 +1,201 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.control.PIDFController; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.Range; +import com.technototes.library.hardware.motor.EncodedMotor; +import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.logger.Log; +import com.technototes.library.logger.Loggable; +import com.technototes.library.subsystem.Subsystem; +import org.firstinspires.ftc.ptechnodactyl.Hardware; + +@Config +public class ClawSubsystem implements Subsystem, Loggable { + + private Servo clawServo; + private EncodedMotor arm; + private boolean isHardware; + + @Log(name = "clawPosition") + public double clawPosition = 0; + + public static double CLAW_OPEN_POSITION = 0.3; + public static double CLAW_CLOSE_POSITION = 0.9; + public static double MIN_ARM_MOTOR_SPEED = -1; + public static double MAX_ARM_MOTOR_SPEED = 1; + public static int INCREMENT_DECREMENT = 5; + public static int INCREMENT_DECREMENT_BUTTON = 50; + public static double FEEDFORWARD_COEFFICIENT = 0.25; + public static int ARM_VERTICAL = 165; + public static int ARM_HORIZONTAL = 53; + public static int INITIAL_POSITION = 20; + public static int ARM_MAX = 327; + public static int ARM_MIN = 8; + + @Log(name = "armTarget") + public int armTargetPos; + + @Log(name = "armPos") + public int armPos; + + @Log(name = "armPow") + public double armPow; + + @Log(name = "armFdFwdVal") + public double armFeedFwdValue; + + private PIDFController armPidController; + + private void setArmPos(int e) { + armPidController.setTargetPosition(e); + armTargetPos = e; + } + + public static PIDCoefficients armPID = new PIDCoefficients(0.02, 0.0, 0); + + private void setClawPosition(double d) { + if (isHardware) { + clawServo.setPosition(d); + clawPosition = d; + } + } + + private int getArmUnmodifiedPosition() { + if (isHardware) { + return (int) arm.getSensorValue(); + } else { + return 0; + } + } + + public ClawSubsystem(Hardware hw) { + isHardware = true; + clawServo = hw.clawServo; + arm = hw.arm; + armPidController = new PIDFController( + armPID, + 0, + 0, + 0, + /* + + The function arguments for the Feed Forward function are Position (ticks) and + Velocity (units?). So, for the arm, we want to check to see if which side of + center we're on, and if the velocity is pushing us down, FF should probably be + low (negative?) while if velocity is pushing us *up*, FF should be high (right?) + Someone who's done physics and/or calculus recently should write some real equations + + Braelyn got the math right + + For angle T through this range where we start at zero: + / + / T + 180 _____/_____ 0 + The downward torque due to gravity is cos(T) * Gravity (9.8m/s^2) + + If we're moving from 0 to 180 degrees, then: + While T is less than 90, the "downward torque" is working *against* the motor + When T is greater than 90, the "downward torque" is working *with* the motor + + */ + + (ticks, velocity) -> { + armFeedFwdValue = FEEDFORWARD_COEFFICIENT * Math.cos(getArmAngle(ticks)); + + // if (velocity > MIN_ANGULAR_VELOCITY) { + // //increase armFeedFwdValue to avoid slamming or increase D in PID + // armFeedFwdValue += ARM_SLAM_PREVENTION; + // } + + return armFeedFwdValue; + } + ); + setArmPos(ARM_HORIZONTAL); + } + + public ClawSubsystem() { + isHardware = false; + clawServo = null; + arm = null; + } + + public void increment(double value) { + int newArmPos = (int) (armTargetPos + value * INCREMENT_DECREMENT); + if (newArmPos > ARM_MAX) { + newArmPos = ARM_MAX; + } else if (newArmPos < ARM_MIN) { + newArmPos = ARM_MIN; + } + setArmPos(newArmPos); + } + + public void increment_button(double value) { + int newArmPos = (int) (armTargetPos + value * INCREMENT_DECREMENT_BUTTON); + if (newArmPos > ARM_MAX) { + newArmPos = ARM_MAX; + } else if (newArmPos < ARM_MIN) { + newArmPos = ARM_MIN; + } + setArmPos(newArmPos); + } + + public void incrementn() { + increment_button(1); + } + + public void decrement() { + increment_button(-1); + } + + public void powIncrement() { + setArmPos(armTargetPos + 1); + } + + public void powDecrement() { + setArmPos(armTargetPos - 1); + } + + private static double getArmAngle(double ticks) { + // our horizontal value starts at ARM_HORIZONTAL, so we need to + // subtract it + return ((Math.PI / 2.0) * (ticks - ARM_HORIZONTAL)) / (ARM_VERTICAL - ARM_HORIZONTAL); + } + + public void openClaw() { + setClawPosition(CLAW_OPEN_POSITION); + } + + public void closeClaw() { + setClawPosition(CLAW_CLOSE_POSITION); + } + + public void setArmVertical() { + setArmPos(ARM_VERTICAL); + } + + public void setArmHorizontal() { + setArmPos(ARM_HORIZONTAL); + } + + public void setArmIntake() { + setArmPos(ARM_MAX); + } + + private void setArmMotorPower(double speed) { + if (isHardware) { + double clippedSpeed = Range.clip(speed, MIN_ARM_MOTOR_SPEED, MAX_ARM_MOTOR_SPEED); + arm.setPower(clippedSpeed); + } + armPow = speed; + } + + @Override + public void periodic() { + armPos = getArmUnmodifiedPosition(); + armPow = armPidController.update(armPos); + setArmMotorPower(armPow); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java index b907af21..5ec27188 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java @@ -1,32 +1,19 @@ package org.firstinspires.ftc.ptechnodactyl.subsystems; -import static org.firstinspires.ftc.ptechnodactyl.Hardware.imu; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.TIP_AUTHORITY; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.TIP_TOLERANCE; - -import android.util.Pair; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.control.PIDCoefficients; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import com.qualcomm.robotcore.util.Range; import com.technototes.library.hardware.motor.EncodedMotor; -import com.technototes.library.hardware.sensor.IMU; -import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; +import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; -import com.technototes.library.util.MapUtils; -import com.technototes.path.geometry.ConfigurablePose; -import com.technototes.path.subsystem.DistanceSensorLocalizer; import com.technototes.path.subsystem.MecanumConstants; import com.technototes.path.subsystem.PathingMecanumDrivebaseSubsystem; import java.util.function.Supplier; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.RobotConstants; import org.firstinspires.ftc.ptechnodactyl.Setup; import org.firstinspires.ftc.ptechnodactyl.helpers.HeadingHelper; -import org.firstinspires.ftc.robotcore.external.navigation.Orientation; public class DrivebaseSubsystem extends PathingMecanumDrivebaseSubsystem @@ -45,10 +32,10 @@ public abstract static class DriveConstants implements MecanumConstants { public static double AUTO_MOTOR_SPEED = 0.9; @TicksPerRev - public static final double TICKS_PER_REV = 28; // From GoBilda's website + public static final double TICKS_PER_REV = 537.7; // From GoBilda's website @MaxRPM - public static final double MAX_RPM = 6000; + public static final double MAX_RPM = 312; public static double MAX_TICKS_PER_SEC = (TICKS_PER_REV * MAX_RPM) / 60.0; @@ -57,9 +44,9 @@ public abstract static class DriveConstants implements MecanumConstants { @MotorVeloPID public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients( - 25, + 20, 0, - 4, + 3, MecanumConstants.getMotorVelocityF((MAX_RPM / 60) * TICKS_PER_REV) ); @@ -67,13 +54,13 @@ public abstract static class DriveConstants implements MecanumConstants { public static double WHEEL_RADIUS = 1.88976; // inches "roughly" lol @GearRatio - public static double GEAR_RATIO = 1 / 13.7; // output (wheel) speed / input (motor) speed + public static double GEAR_RATIO = 1.0; // output (wheel) speed / input (motor) speed @TrackWidth - public static double TRACK_WIDTH = 9; // inches + public static double TRACK_WIDTH = 9.25; // inches @WheelBase - public static double WHEEL_BASE = 8.5; // inches + public static double WHEEL_BASE = 9.25; // inches @KV public static double kV = @@ -87,11 +74,11 @@ public abstract static class DriveConstants implements MecanumConstants { // This was 60, which was too fast. Things slid around a lot. @MaxVelo - public static double MAX_VEL = 60; + public static double MAX_VEL = 50; // This was 35, which also felt a bit too fast. The bot controls more smoothly now @MaxAccel - public static double MAX_ACCEL = 50; + public static double MAX_ACCEL = 30; // This was 180 degrees @MaxAngleVelo @@ -105,7 +92,7 @@ public abstract static class DriveConstants implements MecanumConstants { public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0); @HeadPID - public static PIDCoefficients HEADING_PID = new PIDCoefficients(4, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); @LateralMult public static double LATERAL_MULTIPLIER = 1.0; // For Mecanum, this was by 1.14 (14% off) @@ -122,26 +109,6 @@ public abstract static class DriveConstants implements MecanumConstants { @PoseLimit public static int POSE_HISTORY_LIMIT = 100; - public static double TIP_TOLERANCE = Math.toRadians(10); - - public static double TIP_AUTHORITY = 0.9; - - public static ConfigurablePose LEFT_SENSOR_POSE = new ConfigurablePose( - 0.5, - -5.5, - Math.toRadians(-90) - ); - public static ConfigurablePose RIGHT_SENSOR_POSE = new ConfigurablePose( - 0.5, - 5.5, - Math.toRadians(90) - ); - public static ConfigurablePose FRONT_SENSOR_POSE = new ConfigurablePose( - -5.7, - -2.5, - Math.toRadians(180) - ); - // Helps deal with tired motors public static double AFR_SCALE = 0.9; public static double AFL_SCALE = 0.9; @@ -150,12 +117,6 @@ public abstract static class DriveConstants implements MecanumConstants { } private static final boolean ENABLE_POSE_DIAGNOSTICS = true; - public Rev2MDistanceSensor left, right, front; - // protected FtcDashboard dashboard; - - public float xOffset, yOffset; - - public DistanceSensorLocalizer distanceSensorLocalizer; @Log(name = "Pose2d: ") public String poseDisplay = ENABLE_POSE_DIAGNOSTICS ? "" : null; @@ -186,10 +147,7 @@ public DrivebaseSubsystem( EncodedMotor frMotor, EncodedMotor rlMotor, EncodedMotor rrMotor, - IMU imu, - Rev2MDistanceSensor l, - Rev2MDistanceSensor r, - Rev2MDistanceSensor f + IGyro imu ) { super(flMotor, frMotor, rlMotor, rrMotor, imu, () -> DriveConstants.class); fl2 = flMotor; @@ -198,19 +156,6 @@ public DrivebaseSubsystem( rr2 = rrMotor; curHeading = imu.getHeading(); Setup.HardwareNames.COLOR = imu.getClass().toString(); - distanceSensorLocalizer = new DistanceSensorLocalizer( - imu, - MapUtils.of( - new Pair<>(l, DriveConstants.LEFT_SENSOR_POSE.toPose()), - new Pair<>(r, DriveConstants.RIGHT_SENSOR_POSE.toPose()), - new Pair<>(f, DriveConstants.FRONT_SENSOR_POSE.toPose()) - ) - ); - left = l; - right = r; - front = f; - - resetGyro(); } @Override @@ -219,10 +164,9 @@ public void periodic() { updatePoseEstimate(); Pose2d pose = getPoseEstimate(); Pose2d poseVelocity = getPoseVelocity(); - poseDisplay = - pose.toString() + - " : " + - (poseVelocity != null ? poseVelocity.toString() : "nullv"); + poseDisplay = pose.toString() + + " : " + + (poseVelocity != null ? poseVelocity.toString() : "nullv"); curHeading = this.gyro.getHeading(); Setup.HardwareNames.FLYWHEELMOTOR = this.gyro.getClass().toString(); } @@ -236,55 +180,6 @@ public void slow() { speed = DriveConstants.SLOW_MOTOR_SPEED; } - public DrivebaseSubsystem(Hardware hardware) { - this( - hardware.flDriveMotor, - hardware.frDriveMotor, - hardware.rlDriveMotor, - hardware.rrDriveMotor, - imu, - hardware.leftRangeSensor, - hardware.rightRangeSensor, - hardware.frontRangeSensor - ); - } - - public void relocalize() { - distanceSensorLocalizer.update(getPoseEstimate()); - setPoseEstimate(distanceSensorLocalizer.getPoseEstimate()); - } - - public void relocalizeUnsafe() { - distanceSensorLocalizer.update(); - setPoseEstimate(distanceSensorLocalizer.getPoseEstimate()); - } - - public void resetGyro() { - Orientation i = imu.getAngularOrientation(); - xOffset = i.secondAngle; - yOffset = i.thirdAngle; - distanceSensorLocalizer.setGyroOffset( - i.firstAngle - Math.toRadians(RobotConstants.getAlliance().selectOf(-90, 90)) - ); - } - - public void setSafeDrivePower(Pose2d raw) { - Orientation i = imu.getAngularOrientation(); - float x = 0, y = 0, adjX = xOffset - i.secondAngle, adjY = i.thirdAngle - yOffset; - if (Math.abs(adjY) > TIP_TOLERANCE) y = adjY; - if (Math.abs(adjX) > TIP_TOLERANCE) x = adjX; - // setWeightedDrivePower(raw.plus(new Pose2d(x>0 ? Math.max(x-TIP_TOLERANCE, 0) : Math.min(x+TIP_TOLERANCE, 0), y>0 ? Math.max(y-TIP_TOLERANCE, 0) : Math.min(y+TIP_TOLERANCE, 0), 0))); - setWeightedDrivePower( - raw.plus( - new Pose2d( - Range.clip(x * 2, -TIP_AUTHORITY, TIP_AUTHORITY), - Range.clip(y * 2, -TIP_AUTHORITY, TIP_AUTHORITY), - 0 - ) - ) - ); - } - public void auto() { speed = DriveConstants.AUTO_MOTOR_SPEED; } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java deleted file mode 100644 index 6630e709..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java +++ /dev/null @@ -1,82 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.servo.Servo; -import com.technototes.library.subsystem.Subsystem; -import java.util.function.Supplier; - -@SuppressWarnings("unused") -public class ExtensionSubsystem implements Subsystem, Supplier { - - @Config - public static class ExtensionConstants { - - public static double SNAP_1 = 0, SNAP_2 = Math.PI; - public static double IN = 0.46, SHARED = 0.3, TELEOP_ALLIANCE = 0.35, STEAL_SHARED = - 0.2, LOW_GOAL_AUTO = 0, OUT = 0; - public static double LEFT = 0.1, CENTER = 0.5, RIGHT = 0.9, AUTO_LEFT = 0.25, AUTO_RIGHT = - 0.75; - } - - public Servo slideServo; - public Servo turretServo; - - public ExtensionSubsystem(Servo s, Servo t) { - slideServo = s; - turretServo = t; - } - - public void left() { - turretServo.setPosition(ExtensionConstants.LEFT); - } - - public void right() { - turretServo.setPosition(ExtensionConstants.RIGHT); - } - - public void center() { - turretServo.setPosition(ExtensionConstants.CENTER); - } - - public void setTurret(double v) { - turretServo.setPosition(v); - } - - public boolean isSlideOut() { - return slideServo.getPosition() < ExtensionConstants.IN - 0.05; - } - - public boolean isSlideIn() { - return !isSlideOut(); - } - - public void fullyIn() { - slideServo.setPosition(ExtensionConstants.IN); - } - - public void fullyOut() { - slideServo.setPosition(ExtensionConstants.OUT); - } - - public void setSlide(double v) { - slideServo.setPosition(Range.clip(v, ExtensionConstants.OUT, ExtensionConstants.IN)); - } - - public void translateSlide(double v) { - setSlide(slideServo.getPosition() + v); - } - - public void translateTurret(double v) { - setTurret(turretServo.getPosition() + v); - } - - /** - *Method to display the extension and dump value on the telemetry to the driver knows - * how much they are extending the arm and positioning the cup - */ - @Override - public String get() { - return "TURRET: " + turretServo.getPosition() + ", SLIDE: " + slideServo.getPosition(); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/IntakeSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/IntakeSubsystem.java deleted file mode 100644 index a3bd9032..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,94 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem.IntakeConstants.*; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.technototes.library.hardware.motor.Motor; -import com.technototes.library.hardware.sensor.ColorDistanceSensor; -import com.technototes.library.subsystem.Subsystem; -import com.technototes.library.util.Range; -import java.util.function.Supplier; -import org.firstinspires.ftc.ptechnodactyl.RobotState; - -/** - * Subsystem holding methods used for Intake commands. Intake will be responsible for bringing - * freight in and out of our robot - */ -public class IntakeSubsystem implements Subsystem, Supplier { - - @Config - public static class IntakeConstants { - - public static double INTAKE_IN_SPEED = 1; - public static double INTAKE_OUT_SPEED = -1; - public static double INTAKE_STOP_SPEED = 0; - public static double INTAKE_IDLE_SPEED = 0.5; - - public static Range CUBE_RANGE = new Range(400, 2100); - public static Range DUCK_RANGE = new Range(400, 2100); - public static Range BALL_RANGE = new Range(400, 2100); - - public static double SENSOR_REFRESH_RATE = 50; - } - - public Motor motor; - - public ColorDistanceSensor sensor; - - public IntakeSubsystem(Motor m, ColorDistanceSensor s) { - motor = m; - sensor = s; - } - - /** - * Set the intake motor to run in at a constant speed - */ - public void in() { - motor.setSpeed(INTAKE_IN_SPEED); - } - - /** - * Set the intake motor to run out at a constant speed - */ - public void out() { - motor.setSpeed(INTAKE_OUT_SPEED); - } - - /** - * Set the intake motor to stop running - */ - public void stop() { - motor.setSpeed(INTAKE_STOP_SPEED); - } - - public void idle() { - motor.setSpeed(INTAKE_IDLE_SPEED); - } - - private double light; - - @Override - public String get() { - return RobotState.getFreight().toString(); - } - - ElapsedTime t = new ElapsedTime(); - - @Override - public void periodic() { - if (t.seconds() > 1 / SENSOR_REFRESH_RATE && motor.getSpeed() > 0) { - t.reset(); - light = sensor.getLight(); - RobotState.setFreight(parseFreight()); - } - } - - public RobotState.Freight parseFreight() { - if (CUBE_RANGE.inRange(light)) return RobotState.Freight.CUBE; - if (DUCK_RANGE.inRange(light)) return RobotState.Freight.DUCK; - if (BALL_RANGE.inRange(light)) return RobotState.Freight.BALL; - return RobotState.Freight.NONE; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/LiftSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/LiftSubsystem.java deleted file mode 100644 index 4979f8dd..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/LiftSubsystem.java +++ /dev/null @@ -1,104 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import static org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem.LiftConstants.DEADZONE; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem.LiftConstants.LIFT_LOWER_LIMIT; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem.LiftConstants.LIFT_UPPER_LIMIT; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem.LiftConstants.PID; - -import com.acmerobotics.roadrunner.control.PIDCoefficients; -import com.acmerobotics.roadrunner.control.PIDFController; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.motor.EncodedMotor; -import com.technototes.library.subsystem.Subsystem; -import java.util.function.Supplier; - -@SuppressWarnings("unused") -public class LiftSubsystem implements Subsystem, Supplier { - - //cringe but otherwise code borks for some reason i am confused plz help - @com.acmerobotics.dashboard.config.Config - public static class LiftConstants { - - public static double LIFT_UPPER_LIMIT = 600.0; - public static double LIFT_LOWER_LIMIT = 0.0; - //300 for single slide - public static double COLLECT = 0, NEUTRAL = 150, LEVEL_1 = 100, LEVEL_2 = - 200, TELEOP_LEVEL_2 = 150, LEVEL_3 = 550; - - public static double DEADZONE = 10; - - public static PIDCoefficients PID = new PIDCoefficients(0.008, 0, 0.0005); - } - - public EncodedMotor liftMotor; - - public PIDFController pidController; - - public LiftSubsystem(EncodedMotor l) { - liftMotor = l; - pidController = new PIDFController(PID, 0, 0, 0, (x, y) -> 0.1); - } - - public void setLiftPosition(double pos) { - pidController.setTargetPosition(Range.clip(pos, LIFT_LOWER_LIMIT, LIFT_UPPER_LIMIT)); - } - - /** - * set the lift to top, with the float constant LIFT_UPPER_LIMIT - */ - public void liftToTop() { - setLiftPosition(LIFT_UPPER_LIMIT); - } - - /** - * set the lift to bottom, with the float constant LIFT_LOWER_LIMIT, which is 0 - */ - public void liftToBottom() { - setLiftPosition(LIFT_LOWER_LIMIT); - } - - /** - * basically update something every loop - * which is let the motor running until receive signal about stopping - */ - @Override - public void periodic() { - liftMotor.setSpeed(Range.clip(-pidController.update(-liftMotor.get()), -0.9, 0.2)); - } - - /** - * @return true when motor position reached around target position - * using something called dead-zone, so when the motor moved slightly over the target don't necessary go-back - */ - public boolean isAtTarget() { - return ( - Math.abs( - pidController.getTargetPosition() + - /*+ because lift is inverted*/liftMotor.getSensorValue() - ) < - DEADZONE - ); - } - - /** - * stop the pid controller - * indirectly tells the periodic() method to stop update, which make the boolean isFollowing false - */ - public void stop() { - pidController.reset(); - } - - /** - * @return motor position in double - */ - @Override - public Double get() { - return pidController.getTargetPosition(); - } - - public boolean isLifted() { - return pidController.getTargetPosition() > 100; - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java deleted file mode 100644 index b49cb368..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java +++ /dev/null @@ -1,56 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import static com.technototes.library.hardware.HardwareDevice.hardwareMap; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem.VisionConstants.HEIGHT; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem.VisionConstants.ROTATION; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem.VisionConstants.WIDTH; - -import com.acmerobotics.dashboard.config.Config; -import com.technototes.library.logger.Log; -import com.technototes.library.logger.LogConfig; -import com.technototes.library.logger.Loggable; -import com.technototes.library.subsystem.Subsystem; -import com.technototes.library.util.Color; -import com.technototes.vision.hardware.Webcam; -import org.firstinspires.ftc.ptechnodactyl.opmodes.tele.TeleOpBase; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvWebcam; - -public class VisionSubsystem implements Subsystem, Loggable { - - @Config - public static class VisionConstants { - - public static int WIDTH = 320; - public static int HEIGHT = 240; - public static OpenCvCameraRotation ROTATION = OpenCvCameraRotation.UPSIDE_DOWN; - } - - @LogConfig.DenyList({ TeleOpBase.RedTeleOp.class, TeleOpBase.BlueTeleOp.class }) - @LogConfig.Run(duringRun = false, duringInit = true) - @Log.Number(name = "Barcode") - public BarcodePipeline barcodePipeline = new BarcodePipeline(); - - public Webcam camera; - - public VisionSubsystem(Webcam c) { - camera = c; - } - - public void startStreaming() { - camera.startStreaming(WIDTH, HEIGHT, ROTATION); - } - - public void startBarcodePipeline() { - camera.setPipeline(barcodePipeline); - camera.openCameraDeviceAsync(this::startStreaming, i -> startBarcodePipeline()); - } - - public void stopBarcodePipeline() { - camera.setPipeline(null); - camera.closeCameraDeviceAsync(() -> {}); - } -} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/HangSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/HangSubsystem.java index 277be804..cccdc0dd 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/HangSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/HangSubsystem.java @@ -2,8 +2,11 @@ import com.acmerobotics.dashboard.config.Config; import com.technototes.library.hardware.motor.Motor; +import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.twenty403.Hardware; @Config diff --git a/bun.lockb b/bun.lockb index d520567b..a12a9672 100755 Binary files a/bun.lockb and b/bun.lockb differ