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 6434cd9e..1818e6e3 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.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import org.firstinspires.ftc.hoops.subsystems.ClawAndWristSubsystem; //@Config @@ -13,6 +13,10 @@ public class ClawAndWristBot { public ClawAndWristSubsystem caw; public ClawAndWristBot() { - caw = new ClawAndWristSubsystem(new Servo(CLAW1), new Servo(CLAW2), new Servo(WRIST)); + caw = new ClawAndWristSubsystem( + new ServoGroup(CLAW1), + new ServoGroup(CLAW2), + new ServoGroup(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 82b45b1d..6483ba2e 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.Servo; +import com.technototes.library.hardware.servo.ServoGroup; 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 Servo placeholder2; - public Servo servo; + public ServoGroup placeholder2; + public ServoGroup 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 Servo(Setup.HardwareNames.SERVO); + this.servo = new ServoGroup(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 17fb2d71..bf36ee12 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.Servo; +import com.technototes.library.hardware.servo.ServoGroup; 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 Servo claw1; - private Servo claw2; - private Servo wrist; + private ServoGroup claw1; + private ServoGroup claw2; + private ServoGroup wrist; - public ClawAndWristSubsystem(Servo c1, Servo c2, Servo w) { + public ClawAndWristSubsystem(ServoGroup c1, ServoGroup c2, ServoGroup 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 89598a11..6370224a 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.Servo; +import com.technototes.library.hardware.servo.ServoGroup; 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 Servo armServo; + public ServoGroup armServo; public static double ScoreServoInput = 0.5; @@ -27,7 +27,7 @@ public class PlacementSubsystem implements Subsystem, Loggable { public static double ArmServoOutput = 0.5; - public Servo scoreServo; + public ServoGroup 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 54dd2b82..4cc5857a 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.Servo; +import com.technototes.library.hardware.servo.ServoGroup; 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 Servo servo; + private ServoGroup 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 new file mode 100644 index 00000000..95e91267 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Controls.java @@ -0,0 +1,291 @@ +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 930868d6..d76edd63 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java @@ -1,91 +1,147 @@ package org.firstinspires.ftc.ptechnodactyl; -import com.qualcomm.hardware.lynx.LynxModule; +import static org.firstinspires.ftc.ptechnodactyl.Hardware.HardwareConstants.*; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.*; + 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 java.util.List; -import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; +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; public class Hardware implements Loggable { - public List hubs; + @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 EncodedMotor theMotor, flMotor, frMotor, rlMotor, rrMotor, arm; - public Motor placeholder1; - public DcMotorEx liftMotor; + public Servo turretServo; + public Servo slideServo; - public Servo placeholder2; - public Servo servo, clawServo; + 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 IGyro imu; public Webcam camera; - 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"; + + 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(); } - 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 (ARM_ENABLED) { + dumpServo = new Servo(DUMP).invert().startAt(ArmSubsystem.ArmConstants.CARRY); + armServo = new Servo(ARM).startAt(ArmSubsystem.ArmConstants.UP); } - if (Setup.Connected.MOTOR) { - this.theMotor = new EncodedMotor<>(Setup.HardwareNames.MOTOR); + if (EXTENSION_ENABLED) { + slideServo = new Servo(SLIDE).startAt(ExtensionSubsystem.ExtensionConstants.IN); + turretServo = new Servo(TURRET) + .startAt(ExtensionSubsystem.ExtensionConstants.CENTER) + .expandedRange(); } - if (Setup.Connected.FLYWHEEL) { - this.placeholder1 = new Motor<>(Setup.HardwareNames.FLYWHEELMOTOR); + 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.WEBCAM) { - camera = new Webcam(Setup.HardwareNames.CAMERA); + if (CAROUSEL_ENABLED) { + carouselMotor = new Motor(CAROUSEL).brake(); } - 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 (VISION_ENABLED) { + camera = new Webcam(CAMERA); } - if (Setup.Connected.CLAWSUBSYSTEM) { - this.clawServo = new Servo(Setup.HardwareNames.CLAWSERVO); - this.arm = new EncodedMotor<>(Setup.HardwareNames.ARM); + if (INTAKE_ENABLED) { + intakeMotor = new Motor<>(INTAKE); + intakeSensor = new ColorDistanceSensor(INTAKE_COLOR).onUnit(DistanceUnit.INCH); } - } + 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); - // 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); + capClawServo = new Servo("capclaw").startAt(CapSubsystem.CapConstants.CLAW_CLOSE); + capTurretServo = new Servo("capturr").startAt(CapSubsystem.CapConstants.TURRET_INIT); } - 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 0a34a722..fb1fc8e0 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java @@ -1,33 +1,103 @@ 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.Alliance; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; +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 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 { - public StartingPosition position; - public Alliance alliance; - public double initialVoltage; + @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 DrivebaseSubsystem drivebaseSubsystem; - 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); - } + + @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 + ); } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java new file mode 100644 index 00000000..7acfe50c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java @@ -0,0 +1,271 @@ +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 new file mode 100644 index 00000000..97e1120f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotState.java @@ -0,0 +1,78 @@ +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 deleted file mode 100644 index 6d90a265..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java +++ /dev/null @@ -1,42 +0,0 @@ -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 new file mode 100644 index 00000000..ac50008e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmAllianceCommand.java @@ -0,0 +1,17 @@ +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 new file mode 100644 index 00000000..96dcf7ae --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmBarcodeSelectCommand.java @@ -0,0 +1,21 @@ +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 new file mode 100644 index 00000000..9741f8f1 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmCommand.java @@ -0,0 +1,19 @@ +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 new file mode 100644 index 00000000..31e630d0 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmInCommand.java @@ -0,0 +1,22 @@ +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 new file mode 100644 index 00000000..195c8949 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseCommand.java @@ -0,0 +1,16 @@ +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 new file mode 100644 index 00000000..4df1dace --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseInCommand.java @@ -0,0 +1,21 @@ +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 new file mode 100644 index 00000000..c6f19213 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmSharedCommand.java @@ -0,0 +1,26 @@ +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 new file mode 100644 index 00000000..6b435d2f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmTranslateCommand.java @@ -0,0 +1,29 @@ +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 new file mode 100644 index 00000000..c00d95f5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCarryCommand.java @@ -0,0 +1,19 @@ +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 new file mode 100644 index 00000000..8c5b1c55 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCollectCommand.java @@ -0,0 +1,24 @@ +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 new file mode 100644 index 00000000..69e7714a --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpCommand.java @@ -0,0 +1,24 @@ +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 new file mode 100644 index 00000000..2a52a9e8 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpVariableCommand.java @@ -0,0 +1,38 @@ +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 new file mode 100644 index 00000000..eab077ac --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCarouselCommand.java @@ -0,0 +1,30 @@ +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 new file mode 100644 index 00000000..faa599ed --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCycleCommand.java @@ -0,0 +1,50 @@ +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 new file mode 100644 index 00000000..a2d35cb6 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCyclePreloadCommand.java @@ -0,0 +1,32 @@ +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 new file mode 100644 index 00000000..edbb925c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositAllianceCommand.java @@ -0,0 +1,44 @@ +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 new file mode 100644 index 00000000..90c706d5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositDuckCommand.java @@ -0,0 +1,35 @@ +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 new file mode 100644 index 00000000..7ae11190 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckCommand.java @@ -0,0 +1,40 @@ +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 new file mode 100644 index 00000000..bcf61202 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckPreloadCommand.java @@ -0,0 +1,30 @@ +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 new file mode 100644 index 00000000..3c3ae489 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeDuckCommand.java @@ -0,0 +1,22 @@ +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 new file mode 100644 index 00000000..23cd5757 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeWarehouseCommand.java @@ -0,0 +1,32 @@ +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 new file mode 100644 index 00000000..ed05c062 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkBarrierCommand.java @@ -0,0 +1,28 @@ +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 new file mode 100644 index 00000000..e84e511e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkSquareCommand.java @@ -0,0 +1,26 @@ +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 new file mode 100644 index 00000000..956cccad --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkWarehouseCommand.java @@ -0,0 +1,26 @@ +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 new file mode 100644 index 00000000..92204213 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositAllianceCommand.java @@ -0,0 +1,57 @@ +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 new file mode 100644 index 00000000..d2f2b462 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositSharedCommand.java @@ -0,0 +1,58 @@ +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 new file mode 100644 index 00000000..ebe0d33a --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeAllianceWarehouseCommand.java @@ -0,0 +1,36 @@ +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 new file mode 100644 index 00000000..3963e2bb --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeSharedWarehouseCommand.java @@ -0,0 +1,36 @@ +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 new file mode 100644 index 00000000..612c0374 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapArmTranslateCommand.java @@ -0,0 +1,29 @@ +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 new file mode 100644 index 00000000..14a582fd --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapCloseCommand.java @@ -0,0 +1,19 @@ +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 new file mode 100644 index 00000000..72591b4c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapDownCommand.java @@ -0,0 +1,25 @@ +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 new file mode 100644 index 00000000..cc7c27b9 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOpenCommand.java @@ -0,0 +1,19 @@ +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 new file mode 100644 index 00000000..dc05e5f5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOutCommand.java @@ -0,0 +1,25 @@ +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 new file mode 100644 index 00000000..fad63c65 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapStoreCommand.java @@ -0,0 +1,25 @@ +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 new file mode 100644 index 00000000..ec318fdf --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapTurretTranslateCommand.java @@ -0,0 +1,29 @@ +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 new file mode 100644 index 00000000..28f322e8 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselLeftCommand.java @@ -0,0 +1,31 @@ +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 new file mode 100644 index 00000000..1c0bdf1d --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselRightCommand.java @@ -0,0 +1,31 @@ +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 new file mode 100644 index 00000000..8b71c4a6 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselSpinCommand.java @@ -0,0 +1,32 @@ +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 new file mode 100644 index 00000000..1415825b --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselStopCommand.java @@ -0,0 +1,19 @@ +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 new file mode 100644 index 00000000..db828cc5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositAllianceCommand.java @@ -0,0 +1,26 @@ +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 new file mode 100644 index 00000000..6104c928 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCollectCommand.java @@ -0,0 +1,32 @@ +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 new file mode 100644 index 00000000..e579e161 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCycleAllianceCommand.java @@ -0,0 +1,26 @@ +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 new file mode 100644 index 00000000..11fed43e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositOppositeSharedCommand.java @@ -0,0 +1,33 @@ +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 new file mode 100644 index 00000000..e7676788 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositPreloadCommand.java @@ -0,0 +1,26 @@ +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 new file mode 100644 index 00000000..2c692e90 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositSharedCommand.java @@ -0,0 +1,24 @@ +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 new file mode 100644 index 00000000..13471553 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionBarcodeSelectCommand.java @@ -0,0 +1,24 @@ +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 new file mode 100644 index 00000000..8601576f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectCommand.java @@ -0,0 +1,17 @@ +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 new file mode 100644 index 00000000..882c3f48 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectSafeCommand.java @@ -0,0 +1,23 @@ +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 new file mode 100644 index 00000000..fdbabd9e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCommand.java @@ -0,0 +1,23 @@ +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 new file mode 100644 index 00000000..74b9e321 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftOutCommand.java @@ -0,0 +1,10 @@ +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 new file mode 100644 index 00000000..a7bd0acc --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftSideCommand.java @@ -0,0 +1,25 @@ +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 new file mode 100644 index 00000000..9667bea3 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionOutCommand.java @@ -0,0 +1,31 @@ +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 new file mode 100644 index 00000000..c20bea4d --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightOutCommand.java @@ -0,0 +1,10 @@ +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 new file mode 100644 index 00000000..0c422a1c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightSideCommand.java @@ -0,0 +1,25 @@ +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 new file mode 100644 index 00000000..af4f2db5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionSideCommand.java @@ -0,0 +1,34 @@ +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 new file mode 100644 index 00000000..11605473 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionTranslateCommand.java @@ -0,0 +1,23 @@ +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 new file mode 100644 index 00000000..c7ced9e3 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/TurretTranslateCommand.java @@ -0,0 +1,33 @@ +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 new file mode 100644 index 00000000..1fe462c7 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeInCommand.java @@ -0,0 +1,19 @@ +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 new file mode 100644 index 00000000..2f05c979 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeOutCommand.java @@ -0,0 +1,30 @@ +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 new file mode 100644 index 00000000..91562182 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeSafeCommand.java @@ -0,0 +1,21 @@ +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 new file mode 100644 index 00000000..d1c4d1ef --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeStopCommand.java @@ -0,0 +1,19 @@ +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 new file mode 100644 index 00000000..f4842384 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftBarcodeSelectCommand.java @@ -0,0 +1,17 @@ +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 new file mode 100644 index 00000000..fda5a0ae --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCollectCommand.java @@ -0,0 +1,10 @@ +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 new file mode 100644 index 00000000..a0ca79e7 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCommand.java @@ -0,0 +1,34 @@ +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 new file mode 100644 index 00000000..8820e0df --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel1Command.java @@ -0,0 +1,10 @@ +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 new file mode 100644 index 00000000..3df1fb24 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2Command.java @@ -0,0 +1,10 @@ +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 new file mode 100644 index 00000000..42afc7e6 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2TeleCommand.java @@ -0,0 +1,10 @@ +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 new file mode 100644 index 00000000..48c5524b --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel3Command.java @@ -0,0 +1,10 @@ +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 new file mode 100644 index 00000000..6bd57dab --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevelCommand.java @@ -0,0 +1,19 @@ +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 new file mode 100644 index 00000000..d4c9505e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftSharedCommand.java @@ -0,0 +1,10 @@ +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 new file mode 100644 index 00000000..dfda6a2c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftTranslateCommand.java @@ -0,0 +1,23 @@ +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 new file mode 100644 index 00000000..0734241f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/vision/VisionBarcodeCommand.java @@ -0,0 +1,32 @@ +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 deleted file mode 100644 index 2727dddc..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java +++ /dev/null @@ -1,52 +0,0 @@ -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 deleted file mode 100644 index 20985224..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java +++ /dev/null @@ -1,77 +0,0 @@ -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 deleted file mode 100644 index 0cd94954..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java +++ /dev/null @@ -1,52 +0,0 @@ -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 deleted file mode 100644 index e9ddeaf9..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java +++ /dev/null @@ -1,42 +0,0 @@ -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 new file mode 100644 index 00000000..f3c8bd1e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoCycleBase.java @@ -0,0 +1,56 @@ +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 new file mode 100644 index 00000000..a383a111 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoDuckBase.java @@ -0,0 +1,56 @@ +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 deleted file mode 100644 index a77ee77b..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java +++ /dev/null @@ -1,62 +0,0 @@ -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 new file mode 100644 index 00000000..3e988799 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/LoopTimeTest.java @@ -0,0 +1,52 @@ +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 new file mode 100644 index 00000000..4d47a8fa --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/BackAndForth.java @@ -0,0 +1,59 @@ +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 new file mode 100644 index 00000000..352cdf17 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/DriveVelocityPIDTuner.java @@ -0,0 +1,174 @@ +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 new file mode 100644 index 00000000..920ed20d --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/FollowerPIDTuner.java @@ -0,0 +1,55 @@ +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 new file mode 100644 index 00000000..bab57fac --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/LocalizationTest.java @@ -0,0 +1,55 @@ +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 new file mode 100644 index 00000000..294e253f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/ManualFeedforwardTuner.java @@ -0,0 +1,156 @@ +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 new file mode 100644 index 00000000..28a7b351 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxAngularVeloTuner.java @@ -0,0 +1,74 @@ +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 new file mode 100644 index 00000000..8f7d6fbf --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxVelocityTuner.java @@ -0,0 +1,101 @@ +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 new file mode 100644 index 00000000..8d0dbdb8 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MotorDirectionDebugger.java @@ -0,0 +1,105 @@ +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 new file mode 100644 index 00000000..15335649 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/OpenCVTest.java @@ -0,0 +1,373 @@ +/* + * 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 new file mode 100644 index 00000000..e2bf55f1 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SampleMecanumDrive.java @@ -0,0 +1,496 @@ +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 new file mode 100644 index 00000000..11b28d5d --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SplineTest.java @@ -0,0 +1,46 @@ +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 new file mode 100644 index 00000000..06974ced --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StrafeTest.java @@ -0,0 +1,50 @@ +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 new file mode 100644 index 00000000..bd1cb059 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StraightTest.java @@ -0,0 +1,50 @@ +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 new file mode 100644 index 00000000..051bc940 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/TurnTest.java @@ -0,0 +1,34 @@ +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 deleted file mode 100644 index 6302c59d..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java +++ /dev/null @@ -1,42 +0,0 @@ -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 deleted file mode 100644 index d65a0b52..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java +++ /dev/null @@ -1,29 +0,0 @@ -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 deleted file mode 100644 index a6501b33..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java +++ /dev/null @@ -1,28 +0,0 @@ -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 new file mode 100644 index 00000000..122fe08c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/TeleOpBase.java @@ -0,0 +1,46 @@ +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 deleted file mode 100644 index 69cd1dc5..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java +++ /dev/null @@ -1,31 +0,0 @@ -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 new file mode 100644 index 00000000..5da6730a --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java @@ -0,0 +1,155 @@ +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 new file mode 100644 index 00000000..50067076 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BarcodePipeline.java @@ -0,0 +1,192 @@ +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 new file mode 100644 index 00000000..cad54e6c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java @@ -0,0 +1,37 @@ +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 new file mode 100644 index 00000000..d20e9640 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java @@ -0,0 +1,107 @@ +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 new file mode 100644 index 00000000..1f3c907f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CarouselSubsystem.java @@ -0,0 +1,70 @@ +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 deleted file mode 100644 index 8ae36651..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java +++ /dev/null @@ -1,201 +0,0 @@ -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 5ec27188..b907af21 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,19 +1,32 @@ 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.IGyro; +import com.technototes.library.hardware.sensor.IMU; +import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; 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 @@ -32,10 +45,10 @@ public abstract static class DriveConstants implements MecanumConstants { public static double AUTO_MOTOR_SPEED = 0.9; @TicksPerRev - public static final double TICKS_PER_REV = 537.7; // From GoBilda's website + public static final double TICKS_PER_REV = 28; // From GoBilda's website @MaxRPM - public static final double MAX_RPM = 312; + public static final double MAX_RPM = 6000; public static double MAX_TICKS_PER_SEC = (TICKS_PER_REV * MAX_RPM) / 60.0; @@ -44,9 +57,9 @@ public abstract static class DriveConstants implements MecanumConstants { @MotorVeloPID public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients( - 20, + 25, 0, - 3, + 4, MecanumConstants.getMotorVelocityF((MAX_RPM / 60) * TICKS_PER_REV) ); @@ -54,13 +67,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.0; // output (wheel) speed / input (motor) speed + public static double GEAR_RATIO = 1 / 13.7; // output (wheel) speed / input (motor) speed @TrackWidth - public static double TRACK_WIDTH = 9.25; // inches + public static double TRACK_WIDTH = 9; // inches @WheelBase - public static double WHEEL_BASE = 9.25; // inches + public static double WHEEL_BASE = 8.5; // inches @KV public static double kV = @@ -74,11 +87,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 = 50; + public static double MAX_VEL = 60; // This was 35, which also felt a bit too fast. The bot controls more smoothly now @MaxAccel - public static double MAX_ACCEL = 30; + public static double MAX_ACCEL = 50; // This was 180 degrees @MaxAngleVelo @@ -92,7 +105,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(8, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(4, 0, 0); @LateralMult public static double LATERAL_MULTIPLIER = 1.0; // For Mecanum, this was by 1.14 (14% off) @@ -109,6 +122,26 @@ 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; @@ -117,6 +150,12 @@ 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; @@ -147,7 +186,10 @@ public DrivebaseSubsystem( EncodedMotor frMotor, EncodedMotor rlMotor, EncodedMotor rrMotor, - IGyro imu + IMU imu, + Rev2MDistanceSensor l, + Rev2MDistanceSensor r, + Rev2MDistanceSensor f ) { super(flMotor, frMotor, rlMotor, rrMotor, imu, () -> DriveConstants.class); fl2 = flMotor; @@ -156,6 +198,19 @@ 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 @@ -164,9 +219,10 @@ 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(); } @@ -180,6 +236,55 @@ 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 new file mode 100644 index 00000000..6630e709 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java @@ -0,0 +1,82 @@ +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 new file mode 100644 index 00000000..a3bd9032 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/IntakeSubsystem.java @@ -0,0 +1,94 @@ +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 new file mode 100644 index 00000000..4979f8dd --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/LiftSubsystem.java @@ -0,0 +1,104 @@ +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 new file mode 100644 index 00000000..b49cb368 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java @@ -0,0 +1,56 @@ +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 cccdc0dd..277be804 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,11 +2,8 @@ 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 a12a9672..d520567b 100755 Binary files a/bun.lockb and b/bun.lockb differ