diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java index 2579cd0..287e7bf 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java @@ -31,11 +31,11 @@ are permitted (subject to the limitations in the disclaimer below) provided that THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ package org.firstinspires.ftc.robotcontroller.external.samples; - -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; // Try this specific path import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -70,7 +70,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that @Disabled public class SensorLimelight3A extends LinearOpMode { - private Limelight3A limelight; + public Limelight3A limelight; @Override public void runOpMode() throws InterruptedException @@ -145,6 +145,7 @@ public void runOpMode() throws InterruptedException List colorResults = result.getColorResults(); for (LLResultTypes.ColorResult cr : colorResults) { telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees()); + } } else { telemetry.addData("Limelight", "No data available"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java index 1be2124..d8be7e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java @@ -10,182 +10,13 @@ * Class the holds common functions and variable for the 13233 DECODE robot */ public class CommonControls { - /** - * Front left drive motor - */ - public DcMotor leftFront; - /** - * Front right drive motor - */ - public DcMotor rightFront; - /** - * Back left drive motor - */ - public DcMotor leftBack; - /** - * Right back drive motor - */ - public DcMotor rightBack; - public float launchPower = 1.0f; - /** - * The motor for that controls the ball intake - */ - private DcMotor intake; - /** - * One of the motors used to shoot the balls - */ - private DcMotor Launcher; - /** - * One of the motors used to shoot the balls - */ - private DcMotor Launcher2; - /** - * One of the servos used to carry the balls up the ramp - */ - private CRServo rampServo1; - /** - * One of the servos used to carry the balls up the ramp - */ - private CRServo rampServo2; - private CRServo rampServo3; - private CRServo rampServo4; - /** * Constructor for the CommonControls class * * @param hardwareMap The hardware map for the robot */ public CommonControls(HardwareMap hardwareMap) { - // Map main Drive Motors - leftFront = hardwareMap.get(DcMotor.class, "leftFront"); - rightFront = hardwareMap.get(DcMotor.class, "rightFront"); - leftBack = hardwareMap.get(DcMotor.class, "leftBack"); - rightBack = hardwareMap.get(DcMotor.class, "rightBack"); - - // Map Intake Motor - intake = hardwareMap.get(DcMotor.class, "intake"); - - // Map the launcher Prototype - Launcher = hardwareMap.dcMotor.get("Launcher"); - Launcher2 = hardwareMap.dcMotor.get("Launcher2"); - rampServo1 = hardwareMap.crservo.get("rampServo1"); - rampServo2 = hardwareMap.crservo.get("rampServo2"); - rampServo3 = hardwareMap.crservo.get("rampServo3"); - rampServo4 = hardwareMap.crservo.get("rampServo4"); - // Set direction of the main drive motors - leftFront.setDirection(DcMotorSimple.Direction.FORWARD); - rightFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftBack.setDirection(DcMotorSimple.Direction.FORWARD); - rightBack.setDirection(DcMotorSimple.Direction.REVERSE); - - // Set the direction of the intake motor - intake.setDirection(DcMotorSimple.Direction.REVERSE); - } - - - // TODO: - // Implement field oriented drive - - /** - * Updates all the drive controls based on the current gamepad stick positions - * - * @param controlLeftStickY Left stick Y setting, up/down - * @param controlLeftStickX Left stick X setting, left/right - * @param controlRightStick Right Stick setting, controls left right turn - * @param speedLimiter Trigger setting, controls speed of the robot - */ - public void setDrivePower(float controlLeftStickY, float controlLeftStickX, - float controlRightStick, float speedLimiter) { - // For readability and flexibility for future control input changes - //noinspection UnnecessaryLocalVariable - float forward = controlLeftStickY; - //noinspection UnnecessaryLocalVariable - float strafe = controlLeftStickX; - //noinspection UnnecessaryLocalVariable - float rotate = controlRightStick; - float speed = Math.max(1.0f - speedLimiter, 0.5f); - - - // Calculate the power for each motor - // Remember the direction of the stick is the opposite of what you would expect - // up is in the negatives and down is in the positives - float frontRightPower = (forward + strafe + rotate) * (speed); - float frontLeftPower = (forward - strafe - rotate) * (speed); - float backRightPower = (forward - strafe + rotate) * (speed); - float backLeftPower = (forward + strafe - rotate) * (speed); - - // Set the power for each of the motors - rightFront.setPower(frontRightPower); - leftFront.setPower(frontLeftPower); - rightBack.setPower(backRightPower); - leftBack.setPower(backLeftPower); - } - - /** - * Updates just the forward and back movement based on controller stick position - * - * @param controlLeftStickY Left stick Y setting, up/down - */ - public void setDrivePower(float controlLeftStickY) { - setDrivePower(controlLeftStickY, 0.0f, 0.0f, 0.0f); - } - - - /** - * Sets the power of all of the launch motors depending on the value of launchInput - * - * @param launchInput Button mapped to launch input - */ - void setLaunchPower(boolean launchInput) { - double power = launchInput ? launchPower : 0.0; - - // Set the the power value to the motors - Launcher.setPower(power); - Launcher2.setPower(-power); - } - - /** - * Controls the spin direction of the intake wheel based on controller buttons - * - * @param intakeForwardInput Button mapped to forward input - * @param intakeReverseInput Button mapped to reverse input - */ - void setIntakeDirection(boolean intakeForwardInput, boolean intakeReverseInput) { - double intakePower = 0; - // ^ is the XOR operator, will return true if only one variable is true - // If intakeForwardInput OR intakeReverseInput is true then this is true, but not - // if both are true, which is why this differs from the || logical or operator - if (intakeForwardInput ^ intakeReverseInput) { - // This uses an inline-if statement which is useful when assigning values to variables - // This says set power = 1 if intakeForwardInput is true, else set it to -1 - intakePower = intakeForwardInput ? 1 : -1; - } - rampServo1.setPower(-intakePower); - rampServo2.setPower(intakePower); - rampServo3.setPower(intakePower); - rampServo4.setPower(-intakePower); - intake.setPower(intakePower); - } - - void setDriveBrake() { - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } - - void setDriveFloat() { - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - } - void setDriveMotorZeroPowerBehavior(boolean input) { - if (input) { - setDriveBrake(); - } else { - setDriveFloat(); - } } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java new file mode 100644 index 0000000..aa66945 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java @@ -0,0 +1,259 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LimelightControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; + +import java.util.List; + +@Autonomous(name = "LimeLightAutoBlue", group = "Auto") + +public class LimeLightAutoBlue extends LinearOpMode { + + // Constructors for the utils classes + private DriveControls drive; + private LaunchControls launch; + private SorterControls sorter; + private MotorConstructor motors; + private LimelightControls limelightCont; + + + //This function is executed when this Op Mode is selected from the Driver Station + @Override + public void runOpMode() { + // Create the utils classes + drive = new DriveControls(hardwareMap); + launch = new LaunchControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + limelightCont = new LimelightControls(hardwareMap); + sorter = new SorterControls(motors); + waitForStart(); + + + while (opModeIsActive()) { + drive.setDrivePower(-1.0f); + sleep(1600); + // turns to view april tag tower + drive.setDrivePower(0.0f, 0.0f, 1.0f, + 0.0f); + sleep(550);//turns + drive.setDrivePower(0.0f); + + telemetry.addData("ID", limelightCont.getTagID()); + telemetry.update(); + + sleep(1000); + // turns back viewing goal + drive.setDrivePower(0.0f, 0.0f, -1.0f, + 0.0f); + sleep(550); + drive.setDrivePower(0.0f); + + switch (limelightCont.getTagID()) { + case 21: + telemetry.addLine("AUTO: Green Purple Purple"); + telemetry.update(); + //runs GPP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + //id 21 + break; + + case 22: + telemetry.addLine("AUTO: Purple Green Purple"); + telemetry.update(); + //runs PGP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + break; + + + case 23: + //runs PPG + telemetry.addLine("AUTO: Purple Purple Green"); + telemetry.update(); + //runs GPP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + break; + + } + }switch (limelightCont.getTagID()) { + case 23: + //runs PPG + telemetry.addLine("AUTO: Purple Purple Green"); + telemetry.update(); + //runs GPP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + } +} +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java new file mode 100644 index 0000000..0b48764 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java @@ -0,0 +1,317 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + + +import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LimelightControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; + +@Autonomous(name = "LimeLightBlueTEST ", group = "Auto") +public class LimeLightAutoBlueTEST extends LinearOpMode { + + // ================= HARDWARE ================= + + private DriveControls drive; + private LaunchControls launch; + private SorterControls sorter; + private MotorConstructor motors; + private LimelightControls limelightCont; + + @Override + public void runOpMode() { + + //INIT + drive = new DriveControls(hardwareMap); + launch = new LaunchControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + limelightCont = new LimelightControls(hardwareMap); + sorter = new SorterControls(motors); + telemetry.addLine("Initialized. Waiting for start..."); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + // - PHASE 1: MOVE TO SEE TAG +// preMove(); + drive.setDrivePower(0, -1.0f, 0, 0);//strafes over to launch + sleep(1400); + + int tagID = scanTag(); + + drive.setDrivePower(0.0f, 0.0f, -1.0f, 0.0f); + sleep(550); + + drive.setDrivePower(0.0f); + + // - PHASE 2: DETECT TAG + + telemetry.addData("ID", limelightCont.getTagID()); + telemetry.update(); + sleep(500); + + // PHASE 3: RUN AUTO + runAuto(tagID); + + // END + requestOpModeStop(); + } + + // ========================================================== + // ===================== PHASE METHODS ====================== + // ========================================================== + +// private void preMove() { +// } + + private int scanTag() { + int tagID = -1; + long startTime = System.currentTimeMillis(); + + while (opModeIsActive() + && tagID == -1 + && System.currentTimeMillis() - startTime < 2000) { + + tagID = limelightCont.getTagID(); + + telemetry.addData("Scanning Tag", tagID); + telemetry.update(); + } + + return tagID; + } + + private void runAuto(int tagID) { + + switch (tagID) { + + case 21: + telemetry.addLine("AUTO: Green Purple Purple"); + telemetry.update(); + auto21(); + break; + + case 22: + telemetry.addLine("AUTO: Purple Green Purple"); + telemetry.update(); + auto22(); + break; + + case 23: + telemetry.addLine("AUTO: Purple Purple Green"); + telemetry.update(); + auto23(); + break; + + default: + telemetry.addLine("AUTO: DEFAULT (NO TAG)"); + telemetry.update(); + auto24(); + break; + } + } + + // ========================================================== + // ===================== AUTO PATHS ========================= + // ========================================================== + + private void auto21() { + //runs GPP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.8f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + //id 21 + } + + private void auto22() { + //runs PGP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + } + + private void auto23() { + //runs PPG + + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + + } + + + private void auto24() { + //runs default (PPG) + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + } + + +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java new file mode 100644 index 0000000..afed258 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java @@ -0,0 +1,92 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Utils_13233.AutoDrive; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoTurn; +import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; + +@Autonomous(name = "QuickAutoBlue", group = "Autonomous") +public class QuickAutoBlue extends LinearOpMode { + // Constructors for the utils classes + private DriveControls drive; + private LaunchControls launch; + private SorterControls sorter; + private MotorConstructor motors; + + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + if (opModeIsActive()) { + // Create the utils classes + drive = new DriveControls(hardwareMap); + launch = new LaunchControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + sorter = new SorterControls(motors); + + + //autoDrive.driveForward(10.0, AutoConstants.quarterPower); + //autoTurn.rotate(90, AutoConstants.quarterPower); + + + //Jacob's not so useful code + + drive.setDrivePower(-1.0f); + sleep(1600); + drive.setDriveMotorZeroPowerBehavior(true); + drive.setDrivePower(0);//brakes the reverse to ensure no major fouls are incurred + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + + + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java similarity index 81% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java index 335fa71..97519c9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java @@ -4,13 +4,12 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; -import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; @Autonomous(name = "QuickAuto") -public class QuickAuto extends LinearOpMode { +public class QuickAutoForward extends LinearOpMode { private final DriveControls drive; - public QuickAuto(HardwareMap hardwareMap) { + public QuickAutoForward(HardwareMap hardwareMap) { this.drive = new DriveControls(hardwareMap); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java new file mode 100644 index 0000000..f1965d6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java @@ -0,0 +1,97 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Utils_13233.AutoDrive; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoTurn; +import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; + +@Autonomous(name = "QuickAutoRed", group = "Autonomous") +public class QuickAutoRed extends LinearOpMode { + + // Constructors for the utils classes + private DriveControls drive; + private LaunchControls launch; + private SorterControls sorter; + private MotorConstructor motors; + + @Override + public void runOpMode() { + + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + if (opModeIsActive()) { + // Create the utils classes + drive = new DriveControls(hardwareMap); + launch = new LaunchControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + sorter = new SorterControls(motors); + + + //autoDrive.driveForward(10.0, AutoConstants.quarterPower); + //autoTurn.rotate(90, AutoConstants.quarterPower); + + drive.setDrivePower(-1.0f); + sleep(1200); + drive.setDriveMotorZeroPowerBehavior(true); + drive.setDrivePower(0);//brakes the reverse to ensure no major fouls are incurred + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(1.0f, 0, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + + + } + } +} +// 傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡,傑弗裡，你是說要運行它嗎？ +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡， +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡， +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡， +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡 +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡 +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index e995556..78ea378 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -3,10 +3,36 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; -import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; + +/* + Current Controls: + Gamepad1: + Left Stick Y: Forward backward movement + Left Stick X: Left right strafe movement + Right Stick X: Turn the entier robot left and right + + A: Move the flipper to push the artifact + B: Set the drive motors to brake + X: Set launch wheels to 0.7 power + Left Bumper: Set launch wheels to 1.0 power + + Gamepad2: + D-Pad: + Left: Move sorter to launch position 1 + Up: Move sorter to launch position 2 + Right: Move sorter to launch position 3 + + X: Move sorter to intake position 1 + Y: Move sorter to intake position 2 + B: Move sorter to intake position 3 + + */ @TeleOp(name = "TeleOpMain", group = "Drive") @@ -15,7 +41,8 @@ public class TeleOpMain extends LinearOpMode { // Constructors for the utils classes private DriveControls drive; private LaunchControls launch; - private RampControls ramp; + private SorterControls sorter; + private MotorConstructor motors; //This function is executed when this Op Mode is selected from the Driver Station @@ -24,8 +51,13 @@ public void runOpMode() { // Create the utils classes drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); - ramp = new RampControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + sorter = new SorterControls(motors); + + sorter.currentSorterPosition = SorterControls.sorterPositions.LAUNCH_POS_1; + + // Wait for the game to start (driver presses PLAY) // The waitForStart() function will wait for the start button to begin // DON'T WRITE ANY CODE AFTER THE WAIT FOR START UNTIL THE "while (opModIsActive())" // THIS WILL CAUSE PROBLEMS WHEN GOING THROUGH INSPECTION @@ -34,28 +66,56 @@ public void runOpMode() { while (opModeIsActive()) { // Add status data to driver hub display telemetry.addData("Status", "opModeIsActive"); + telemetry.addLine(); + telemetry.addData("Sorter ", "Stats"); + telemetry.addData("Target", motors.Sorter.getTargetPosition()); + telemetry.addData("Current", motors.Sorter.getCurrentPosition()); + telemetry.addData("Busy", motors.Sorter.isBusy()); + telemetry.addData("Current Position", sorter.currentSorterPosition.toString()); + telemetry.addLine(); + telemetry.addData("Current Sorter Positions", ":"); + telemetry.addData("Sorter Slot 1", sorter.currentSorterStates[0].toString()); + telemetry.addData("Sorter Slot 2", sorter.currentSorterStates[1].toString()); + telemetry.addData("Sorter Slot 3", sorter.currentSorterStates[2].toString()); telemetry.update(); // Set the power to the launch motors based while the x button is being pressed + // and rumble to let the driver know that the launch motors are being controlled + launch.setLaunchPower(gamepad1.x, 0.7f); + launch.setLaunchPower(gamepad1.left_bumper, 1.0f); - launch.setLaunchPower(gamepad1.x); + // Allows for driver Control of the sorter + // Intake positions for the sorter + sorter.simpleSorterPosition(gamepad2.x, gamepad2.y, gamepad2.b, + SorterControls.sorterModes.INTAKE); - //Add option to enable brakes when sharbell holds a - drive.setDriveMotorZeroPowerBehavior(gamepad1.a); - //Add option to enable brakes when driver 1 holds the "a" button - drive.setDriveMotorZeroPowerBehavior(gamepad1.a); + // Launch positons for the sorter + sorter.simpleSorterPosition(gamepad2.dpad_left, gamepad2.dpad_up, gamepad2.dpad_right, + SorterControls.sorterModes.LAUNCH); - // Set all of the intake motors and servos to go forward when dpad up is pressed - // and reverse when dpad down is pressed - ramp.setIntakeDirection(gamepad2.dpad_up, gamepad2.dpad_down); - // Wait for motors to speed up before changing value - // While it is bad practice to put a sleep in a loop it is the only way to (that I know) - // to make the intake not jitter I also + // Control the flipper and make sure that when the sorter is moving the flipper is set + // to the not active positon + if (gamepad1.a && !motors.Sorter.isBusy()) { + motors.Flipper.setDirection(Servo.Direction.FORWARD); + motors.Flipper.setPosition(0.0); + } else { + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + } +// if (gamepad1.left_bumper) { +// sorter.moveGreenToLaunchPos(); +// } + + //Add option to enable brakes when sharbell holds a + drive.setDriveMotorZeroPowerBehavior(gamepad1.b); // Sets the power to the drive motors based on current gamepad inputs drive.setDrivePower(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x, gamepad1.left_trigger); + + // Slow down the loop because for some reason it goes to fast and causes the motors to + // not work properly sleep(50); } // Do not place any drive code here outside of the while loop (will fail inspection) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/IntakeControls.java similarity index 81% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/IntakeControls.java index 4154f50..64b9794 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/IntakeControls.java @@ -2,10 +2,10 @@ import com.qualcomm.robotcore.hardware.HardwareMap; -public class RampControls { +public class IntakeControls { private final MotorConstructor motors; - public RampControls(HardwareMap hardwareMap) { + public IntakeControls(HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); } @@ -25,10 +25,6 @@ public void setIntakeDirection(boolean intakeForwardInput, boolean intakeReverse // This says set power = 1 if intakeForwardInput is true, else set it to -1 intakePower = intakeForwardInput ? 1 : -1; } - motors.rampServo1.setPower(-intakePower); - motors.rampServo2.setPower(intakePower); - motors.rampServo3.setPower(intakePower); - motors.rampServo4.setPower(-intakePower); motors.intake.setPower(intakePower); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java index b5a5533..26fdf19 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java @@ -1,9 +1,15 @@ package org.firstinspires.ftc.teamcode.Utils_13233; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; public class LaunchControls { + private static ElapsedTime rpmTimer = new ElapsedTime(); private final MotorConstructor motors; + double launch1Rpm; + double launch2Rpm; + double avgLaunchRpm; public LaunchControls(HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); @@ -14,12 +20,30 @@ public LaunchControls(HardwareMap hardwareMap) { * * @param launchInput Button mapped to launch input */ + + public void setLaunchPower(boolean launchInput) { - double power = launchInput ? 1.0 : 0.0; + double power = launchInput ? 1.0f : 0.0f; // Set the the power value to the motors motors.Launcher.setPower(-power); motors.Launcher2.setPower(power); } + public void setLaunchPower(boolean launchInput, float launchPower) { + double power = launchInput ? launchPower : 0.0f; + + // Set the the power value to the motors + motors.Launcher.setPower(-power); + motors.Launcher2.setPower(power); + } + + public double launch1Speed() { + if (rpmTimer.milliseconds() >= 50) { + launch1Rpm = ((double) motors.Launcher.getCurrentPosition() / 28) * 1200; + motors.Launcher.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rpmTimer.reset(); + } + return (launch1Rpm); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java new file mode 100644 index 0000000..98d2f07 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.robotcore.hardware.HardwareMap; + + +public class LimelightControls { + private final MotorConstructor motors; + private final int aprilTagPipeline = 0; + private int lastTagScanned; + + /** + * Constructor for the LimelightControls class + * + * @param hardwareMap Hardware map for the robot + */ + public LimelightControls(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + } + + /** + * Get the ID of the tag that is being seen by the limelight + * + * @return Returns the ID of the tag that is being seen by the limelight + */ + public int getTagID() { + motors.limelight.pipelineSwitch(aprilTagPipeline); + motors.limelight.start(); + + LLResult result = motors.limelight.getLatestResult(); + + if (result != null && result.isValid()) { + for (LLResultTypes.FiducialResult tag : result.getFiducialResults()) { + lastTagScanned = tag.getFiducialId(); + return (tag.getFiducialId()); + } + motors.limelight.stop(); + } + return lastTagScanned; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index d4796ea..2dc82a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -1,63 +1,48 @@ package org.firstinspires.ftc.teamcode.Utils_13233; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.VoltageSensor; /** * Class holds motors and servos that are used throughout the code */ public class MotorConstructor { - /** - * Front left drive motor - */ + + // Drive Motor public DcMotor leftFront; - /** - * Front right drive motor - */ public DcMotor rightFront; - /** - * Back left drive motor - */ public DcMotor leftBack; - /** - * Right back drive motor - */ public DcMotor rightBack; - /** - * The motor for that controls the ball intake - */ + // Intake public DcMotor intake; - /** - * One of the motors used to shoot the balls - */ + // Launch motors to shoot the balls out of the robot public DcMotor Launcher; - /** - * One of the motors used to shoot the balls - */ public DcMotor Launcher2; + // The motor that controls the sorter position + public DcMotor Sorter; - /** - * One of the servos used to carry the balls up the ramp - */ - public CRServo rampServo1; - /** - * One of the servos used to carry the balls up the ramp - */ - public CRServo rampServo2; - public CRServo rampServo3; - public CRServo rampServo4; + // The servo the pushes the balls into the launcher flywheels + public Servo Flipper; + // The battery voltage sensor public VoltageSensor VoltSens; + + //Rotation Sensor on the Control hub public IMU imu; + // The limelight 3a used for scanning April Tags + public Limelight3A limelight; + public MotorConstructor(HardwareMap hardwareMap) { // Map main Drive Motors leftFront = hardwareMap.get(DcMotor.class, "leftFront"); @@ -72,11 +57,8 @@ public MotorConstructor(HardwareMap hardwareMap) { Launcher = hardwareMap.get(DcMotor.class, "Launcher"); Launcher2 = hardwareMap.get(DcMotor.class, "Launcher2"); - // Map the ramp servos - rampServo1 = hardwareMap.get(CRServo.class, "rampServo1"); - rampServo2 = hardwareMap.get(CRServo.class, "rampServo2"); - rampServo3 = hardwareMap.get(CRServo.class, "rampServo3"); - rampServo4 = hardwareMap.get(CRServo.class, "rampServo4"); + Sorter = hardwareMap.get(DcMotor.class, "sorter"); + Flipper = hardwareMap.get(Servo.class, "flipper"); // Control hub voltage sensor // Used to move at a constant speed in auto regardless of battery voltage @@ -96,12 +78,28 @@ public MotorConstructor(HardwareMap hardwareMap) { Launcher2.setDirection(DcMotorSimple.Direction.REVERSE); // Set the launch motors to run using encoders - Launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - Launcher2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Launcher.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + Launcher2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + Launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + Launcher2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + Sorter.setDirection(DcMotorSimple.Direction.REVERSE); + Sorter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + Sorter.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + Sorter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + limelight.pipelineSwitch(0); + limelight.start(); + + + //Flipper.setDirection(Servo.Direction.REVERSE); // Set the direction of the intake motor intake.setDirection(DcMotorSimple.Direction.REVERSE); } + public void leftFront(float v) { + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java new file mode 100644 index 0000000..0675c6c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -0,0 +1,215 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.hardware.DcMotor; + + +public class SorterControls { + private final MotorConstructor motors; + + // Sorter intake positions + public int intakePos1 = 263; + public int intakePos2 = -87; + public int intakePos3 = 84; + + // Sorter launch positions + public int LaunchPos1 = 0; + public int LaunchPos2 = 180; + public int LaunchPos3 = 356; + + public sorterPositions currentSorterPosition; + + + public ballColors[] currentSorterStates = {ballColors.NULL, ballColors.NULL, ballColors.GREEN}; + + // Motor Speed + float motorSpeed = 1.0f; + + public SorterControls(MotorConstructor motors) { + this.motors = motors; + } + + /** + * Move the sorter to a position to intake from + * + * @param pos Enum to determine what positon to move to + */ + private void moveToIntakePos(intakePos pos) { + switch (pos) { + case INTAKE_POS_1: + motors.Sorter.setTargetPosition(intakePos1); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(motorSpeed); + break; + case INTAKE_POS_2: + motors.Sorter.setTargetPosition(intakePos2); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(motorSpeed); + break; + case INTAKE_POS_3: + motors.Sorter.setTargetPosition(intakePos3); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(motorSpeed); + break; + } + } + + + /** + * Move the sorter to a position to launch from + * + * @param pos Enum to determine what positon to move to + */ + private void moveToLaunchPos(launchPos pos) { + switch (pos) { + case LAUNCH_POS_1: + motors.Sorter.setTargetPosition(LaunchPos1); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(motorSpeed); + break; + case LAUNCH_POS_2: + motors.Sorter.setTargetPosition(LaunchPos2); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(motorSpeed); + break; + case LAUNCH_POS_3: + motors.Sorter.setTargetPosition(LaunchPos3); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(motorSpeed); + break; + } + } + + + /** + * Moves the sorter to a position defined in mode and pos allows for both intake and launching + * + * @param mode Enum to determine weather to launch or intake + * @param pos Enum to determine what sorter position to move to can be 1, 2 or 3 + * @throws RuntimeException Throws a runtime error if pos is not 1, 2 or 3 + */ + public void moveSorterToPos(sorterModes mode, int pos) { + if (mode == sorterModes.INTAKE) { + switch (pos) { + case 1: + moveToIntakePos(intakePos.INTAKE_POS_1); + currentSorterPosition = sorterPositions.INTAKE_POS_1; + break; + case 2: + moveToIntakePos(intakePos.INTAKE_POS_2); + currentSorterPosition = sorterPositions.INTAKE_POS_2; + break; + case 3: + moveToIntakePos(intakePos.INTAKE_POS_3); + currentSorterPosition = sorterPositions.INTAKE_POS_3; + break; + default: + // throw error when pos is not 1, 2 or 3 + throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + + "say run it?"); + } + } else if (mode == sorterModes.LAUNCH) { + switch (pos) { + case 1: + moveToLaunchPos(launchPos.LAUNCH_POS_1); + currentSorterPosition = sorterPositions.LAUNCH_POS_1; + break; + case 2: + moveToLaunchPos(launchPos.LAUNCH_POS_2); + currentSorterPosition = sorterPositions.LAUNCH_POS_2; + break; + case 3: + moveToLaunchPos(launchPos.LAUNCH_POS_3); + currentSorterPosition = sorterPositions.LAUNCH_POS_3; + break; + default: + // throw error when pos is not 1, 2 or 3 + throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + + "say run it?"); + } + } + } + + public void simpleSorterPosition(boolean position1Button, boolean position2Button, + boolean position3Button, sorterModes mode) { + if (position1Button) { + moveSorterToPos(mode, 1); + } else if (position2Button) { + moveSorterToPos(mode, 2); + } else if (position3Button) { + moveSorterToPos(mode, 3); + } + } + + /** + * Sorts through the balls in the sorter and moves a green ball to the launch position + * automatically + */ + public void moveGreenToLaunchPos() { + for (int i = 0; i < currentSorterStates.length; i++) { + if (currentSorterStates[i] == ballColors.GREEN) { + moveSorterToPos(sorterModes.LAUNCH, i + 1); + break; + } + } + } + + /** + * Sorts through the balls in the sorter and moves a purple ball to the launch position + * automatically + */ + public void moveToPurpleLaunchPos() { + for (int i = 0; i < currentSorterStates.length; i++) { + if (currentSorterStates[i] == ballColors.PURPLE) { + moveSorterToPos(sorterModes.LAUNCH, i + 1); + break; + } + } + } + + /** + * Intake positions + */ + public enum intakePos { + INTAKE_POS_1, + INTAKE_POS_2, + INTAKE_POS_3 + } + + /** + * Launch positions + */ + public enum launchPos { + LAUNCH_POS_1, + LAUNCH_POS_2, + LAUNCH_POS_3 + } + + + /** + * Different modes that the sorter can be moved to + */ + public enum sorterModes { + INTAKE, + LAUNCH + } + + /** + * Possible colors that the balls can be + */ + public enum ballColors { + PURPLE, + GREEN, + NULL + } + + public enum sorterPositions { + INTAKE_POS_1, + INTAKE_POS_2, + INTAKE_POS_3, + LAUNCH_POS_1, + LAUNCH_POS_2, + LAUNCH_POS_3 + } + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java new file mode 100644 index 0000000..002c14a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +@Autonomous(name = "Limelight AprilTag Test") +public class limeLightTest extends LinearOpMode { + + private Limelight3A limelight; + + + @Override + public void runOpMode() { + + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + // Set AprilTag pipeline (must match Limelight web config) + limelight.pipelineSwitch(0); + limelight.start(); + + telemetry.addLine("Waiting for start..."); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + LLResult result = limelight.getLatestResult(); + + if (result != null && result.isValid()) { + for (FiducialResult tag : result.getFiducialResults()) { + telemetry.addData("AprilTag ID", tag.getFiducialId()); + + } + } else { + telemetry.addLine("No tags detected"); + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java index 6c3c416..d7ef148 100644 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java @@ -3,11 +3,13 @@ import static org.mockito.Mockito.mock; import static org.mockito.Mockito.when; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; @@ -16,10 +18,11 @@ import org.mockito.MockitoAnnotations; public class MockMotorUtil { - public DcMotor launcher, launcher2, intake, rightFront, leftFront, leftBack, rightBack; - public CRServo rampServo1, rampServo2, rampServo3, rampServo4; + public DcMotor launcher, launcher2, intake, rightFront, leftFront, leftBack, rightBack, Sorter; + public Servo Flipper; public HardwareMap hardwareMap; public IMU imu; + public Limelight3A limelight; @Mock HardwareMap.DeviceMapping voltageSensorMap; @@ -44,14 +47,14 @@ public void setUp() { launcher = mock(DcMotor.class); launcher2 = mock(DcMotor.class); - rampServo1 = mock(CRServo.class); - rampServo2 = mock(CRServo.class); - rampServo3 = mock(CRServo.class); - rampServo4 = mock(CRServo.class); + Sorter = mock(DcMotor.class); + Flipper = mock(Servo.class); VoltSens = mock(VoltageSensor.class); imu = mock(IMU.class); + limelight = mock(Limelight3A.class); + // Mock the motors when(hardwareMap.get(DcMotor.class, "intake")).thenReturn(intake); @@ -64,10 +67,9 @@ public void setUp() { when(hardwareMap.get(DcMotor.class, "Launcher")).thenReturn(launcher); when(hardwareMap.get(DcMotor.class, "Launcher2")).thenReturn(launcher2); - when(hardwareMap.get(CRServo.class, "rampServo1")).thenReturn(rampServo1); - when(hardwareMap.get(CRServo.class, "rampServo2")).thenReturn(rampServo2); - when(hardwareMap.get(CRServo.class, "rampServo3")).thenReturn(rampServo3); - when(hardwareMap.get(CRServo.class, "rampServo4")).thenReturn(rampServo4); + when(hardwareMap.get(DcMotor.class, "sorter")).thenReturn(Sorter); + when(hardwareMap.get(Servo.class, "flipper")).thenReturn(Flipper); + when(hardwareMap.get(VoltageSensor.class, "Control Hub")).thenReturn(VoltSens); hardwareMap.voltageSensor = voltageSensorMap; @@ -75,6 +77,7 @@ public void setUp() { // Mock the IMU (gyroscope) when(hardwareMap.get(IMU.class, "imu")).thenReturn(imu); + when(hardwareMap.get(Limelight3A.class, "limelight")).thenReturn(limelight); // Set direction of the main drive motors leftFront.setDirection(DcMotorSimple.Direction.FORWARD); diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java deleted file mode 100644 index 044bc3f..0000000 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java +++ /dev/null @@ -1,66 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; -import org.junit.jupiter.api.BeforeEach; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.mockito.Mockito.mock; -import static org.mockito.Mockito.verify; -import static org.mockito.Mockito.when; - -import com.qualcomm.robotcore.hardware.CRServo; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.junit.jupiter.api.DisplayName; -import org.junit.jupiter.api.Test; - - -public class RampControlsTest { - public RampControls rampControls; - - MockMotorUtil mockMotor = new MockMotorUtil(); - - @BeforeEach - void motorSetup(){ - mockMotor.setUp(); - rampControls = new RampControls(mockMotor.hardwareMap); - } - - @DisplayName("Intake should spin forward when forward button is pressed") - @Test - void testIntakeForward() { - rampControls.setIntakeDirection(true, false); - verify(mockMotor.intake).setPower(1.0); - verify(mockMotor.rampServo1).setPower(-1.0); - verify(mockMotor.rampServo2).setPower(1.0); - } - - @DisplayName("Intake should spin backward when reverse button is pressed") - @Test - void testIntakeReverse() { - rampControls.setIntakeDirection(false, true); - verify(mockMotor.intake).setPower(-1.0); - verify(mockMotor.rampServo1).setPower(1.0); - verify(mockMotor.rampServo2).setPower(-1.0); - } - - @DisplayName("Intake should not spin when direction buttons are not depressed") - @Test - void testIntakeNeutral() { - rampControls.setIntakeDirection(false, false); - verify(mockMotor.intake).setPower(0.0); - verify(mockMotor.rampServo1).setPower(-0.0); - verify(mockMotor.rampServo2).setPower(0.0); - } - - @DisplayName("Intake should stop when both direction buttons pressed at the same time") - @Test - void testIntakeConflict() { - rampControls.setIntakeDirection(true, true); - verify(mockMotor.intake).setPower(0.0); - verify(mockMotor.rampServo1).setPower(-0.0); - verify(mockMotor.rampServo2).setPower(0.0); - } -} diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 1c07e8c..241cbff 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -13,5 +13,6 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:11.0.0' implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' + //implementation 'org.firstinspires.ftc:Limelight:1.0.2'//added by jacob 1/17/26 update jeffery 1/20/26 jacob is stupid }