diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java index 3717861938b8..50c8e94eab68 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java @@ -1,3 +1,4 @@ + package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -6,42 +7,70 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import org.firstinspires.ftc.teamcode.util.TeamConstants; + @Autonomous public class AutoBasic extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { // Declare motors // Make sure your ID's match your configuration - DcMotorEx frontLeftMotor = hardwareMap.get(DcMotorEx.class, "frontLeftMotor"); - DcMotorEx backLeftMotor = hardwareMap.get(DcMotorEx.class, "backLeftMotor"); - DcMotorEx frontRightMotor = hardwareMap.get(DcMotorEx.class, "frontRightMotor"); - DcMotorEx backRightMotor = hardwareMap.get(DcMotorEx.class, "backRightMotor"); - DcMotorEx bigHooperMotor = hardwareMap.get(DcMotorEx.class, "bigHooperMotor"); - DcMotorEx smallHooperMotor = hardwareMap.get(DcMotorEx.class, "smallHooperMotor"); - - backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); waitForStart(); if(isStopRequested()) return; - frontLeftMotor.setPower(0.5); - frontRightMotor.setPower(0.5); - backLeftMotor.setPower(0.5); - backRightMotor.setPower(0.5); - sleep(2200); - frontLeftMotor.setPower(0.25); - frontRightMotor.setPower(0.25); - backLeftMotor.setPower(-0.25); - backRightMotor.setPower(-0.25); - sleep(750); - frontLeftMotor.setPower(0); - frontRightMotor.setPower(0); - backLeftMotor.setPower(0); - backRightMotor.setPower(0); - bigHooperMotor.setPower(-0.625); - sleep(2000); - bigHooperMotor.setPower(0); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(2700); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(0.25); + base[1].setPower(0.25); + base[2].setPower(-0.25); + base[3].setPower(-0.25); + sleep(950); + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1825); + sleep(5000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(250); + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(5500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(800); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(0.25); + base[1].setPower(0.25); + base[2].setPower(0.25); + base[3].setPower(0.25); + sleep(1250); + base[0].setPower(-0.25); + base[1].setPower(-0.25); + base[2].setPower(0.25); + base[3].setPower(0.25); + sleep(3400); + base[0].setPower(0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(-0.5); + sleep(2300); + + } -}; +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java new file mode 100644 index 000000000000..ad642b94fa52 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java @@ -0,0 +1,74 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoBasicbleu extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(2700); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(-0.25); + base[1].setPower(-0.25); + base[2].setPower(0.25); + base[3].setPower(0.25); + sleep(993); + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1920); + sleep(5000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(250); + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(5500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(1500); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(0.25); + base[1].setPower(0.25); + base[2].setPower(0.25); + base[3].setPower(0.25); + sleep(1250); + base[0].setPower(0.25); + base[1].setPower(0.25); + base[2].setPower(-0.25); + base[3].setPower(-0.25); + sleep(3150); + base[0].setPower(-0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(0.5); + sleep(1250); + + + } +}; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueBack.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueBack.java new file mode 100644 index 000000000000..a7103d127a38 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueBack.java @@ -0,0 +1,33 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoBlueBack extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + + base[0].setPower(-1); + base[1].setPower(1); + base[2].setPower(-1); + base[3].setPower(1); + sleep(430); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBluefromGoal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBluefromGoal.java new file mode 100644 index 000000000000..5f6c5ce01b54 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBluefromGoal.java @@ -0,0 +1,127 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoBluefromGoal extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1550); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(40); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1855); + sleep(4000); + scoring[1].setPower(0); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(2500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(650); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(327); + base[0].setPower(-0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(0.5); + sleep(638); + scoring[1].setPower(1); + scoring[2].setPower(-.70); + scoring[0].setPower(-1); + base[0].setPower(0.22); + base[1].setPower(0.22); + base[2].setPower(0.22); + base[3].setPower(0.22); + sleep(5950); + scoring[1].setPower(-.35); + scoring[2].setPower(-.35); + sleep(100); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1000); + scoring[1].setPower(0); + base[0].setPower(1); + base[1].setPower(-1); + base[2].setPower(1); + base[3].setPower(-1); + sleep(580); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(310); + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1745); + sleep(4000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(3300); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(800); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(230); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(70); + base[0].setPower(1); + base[1].setPower(-1); + base[2].setPower(1); + base[3].setPower(-1); + sleep(1030); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoForward.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoForward.java new file mode 100644 index 000000000000..756cf0f60a3f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoForward.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoForward extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(750); + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedBack.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedBack.java new file mode 100644 index 000000000000..1ae938d4eae1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedBack.java @@ -0,0 +1,33 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoRedBack extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + + base[0].setPower(1); + base[1].setPower(-1); + base[2].setPower(1); + base[3].setPower(-1); + sleep(430); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedfromGoal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedfromGoal.java new file mode 100644 index 000000000000..3b0603e020b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedfromGoal.java @@ -0,0 +1,125 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoRedfromGoal extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1555); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(40); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1875); + sleep(4000); + scoring[1].setPower(0); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(2500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(650); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(260); + base[0].setPower(0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(-0.5); + sleep(520); + scoring[1].setPower(1); + scoring[2].setPower(-.65); + scoring[0].setPower(-1); + base[0].setPower(0.22); + base[1].setPower(0.22); + base[2].setPower(0.22); + base[3].setPower(0.22); + sleep(8000); + scoring[1].setPower(-.3); + scoring[2].setPower(-.3); + sleep(150); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1000); + scoring[1].setPower(0); + base[0].setPower(-1); + base[1].setPower(1); + base[2].setPower(-1); + base[3].setPower(1); + sleep(580); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(235); + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1660); + sleep(4000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(3000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(800); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(230); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(80); + base[0].setPower(-1); + base[1].setPower(1); + base[2].setPower(-1); + base[3].setPower(1); + sleep(1030); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoPsuedoEncoderTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoPsuedoEncoderTest.java new file mode 100644 index 000000000000..51b626a01866 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoPsuedoEncoderTest.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; + +@TeleOp +public class ServoPsuedoEncoderTest extends LinearOpMode { + + private CRServo crServo; + + @Override + public void runOpMode() { + + // Get the CR servo from the hardware map + crServo = hardwareMap.get(CRServo.class, "cr_servo"); + + // Optionally set the direction + //crServo.setDirection(CRServo.Direction.FORWARD); + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + + // Drive with right stick Y (gamepad1) + int Degrees = 1; + double Servo_degree_time = 1.13666666; + double Target_time = Degrees*Servo_degree_time; + crServo.setPower(0.9); + try { + Thread.sleep((long) Target_time); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + crServo.setpower(0); + + telemetry.addData("CR Servo Power", power); + telemetry.update(); + } + + // Stop the servo when the op mode ends + crServo.setPower(0); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpBasic.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Basic.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpBasic.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Basic.java index 76a8ac3962da..2a999e2f9dd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpBasic.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Basic.java @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; @TeleOp -public class TeleOpBasic extends LinearOpMode { +public class Basic extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { // Declare our motors diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java new file mode 100644 index 000000000000..04869c2d1fab --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java @@ -0,0 +1,356 @@ +package org.firstinspires.ftc.teamcode.TeleOp; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; +import org.firstinspires.ftc.teamcode.util.DiagnosticLogger; + +import java.io.IOException; +import java.util.Locale; + +@TeleOp(group = "Field Centric") +public class Field2Drivers extends LinearOpMode +{ + // Required for the DiagnosticLogger at the bottom of this file: + private final static IMU.Parameters parameters = TeamConstants.getIMUParms(); + + // --- One-time-only constants (non-@Config) --- + // These only update after a restart of the bot. + private final static FtcDashboard DASH = FtcDashboard.getInstance(); + private final static TelemetryPacket packet = new TelemetryPacket(); + + Telemetry telemetryD = DASH.getTelemetry(); + Telemetry telemetryM = new MultipleTelemetry(telemetry, DASH.getTelemetry()); + + @Override + public void runOpMode() + { + // These methods are only initialized when inside of runOpMode: + telemetryD.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + // --- @Config constants --- + final int CPR = TeamConstants.CPR; + final double LOCK_ON_DENOMINATOR = TeamConstants.LOCK_ON_DENOMINATOR; + final double LOCK_ON_OFFSET = TeamConstants.LOCK_ON_OFFSET; + final double LL_MOUNT_ANGLE = TeamConstants.LL_MOUNT_ANGLE; + final double LL_LENS_HEIGHT_INCHES = TeamConstants.LL_LENS_HEIGHT_INCHES; + final double GOAL_HEIGHT_INCHES = TeamConstants.GOAL_HEIGHT_INCHES; + + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + final IMU imu = TeamConstants.getIMU(hardwareMap); + final Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); + + // Final setup before control starts: + imu.initialize(parameters); + limelight.start(); + + telemetryM.addLine("> Ready."); + telemetryM.update(); + + waitForStart(); + + // Ensure telemetry is clear on start. + // FtcDashboard tends to keep telemetry in the buffer even after + // update()-ing. + // (We abuse this a bit later to have nicely formatted text, since + // FtcDashboard also likes alphabetizing data.) + telemetryM.clear(); + + DiagnosticLogger logger = getLogger(); + Thread loggerRuntimeThread = new Thread(logger); + logger.resumeRun(); + loggerRuntimeThread.start(); + + boolean lockOn = false; + boolean autoShot = true; + boolean pastLeftBumper1 = false; + boolean pastB2 = false; + int meanCounter = 0; + double aggregateDistance = 0; + double meanDistance = 0; + double tempDistance = 0; + double pastDistance = 0; + + while(opModeIsActive()) + { + LLResult rawResult = limelight.getLatestResult(); + boolean resultIsValid = rawResult.isValid(); + + // LockOn logic + double rx; + if(lockOn && resultIsValid) + { + double targetX = rawResult.getTx(); + // A simple proportional input; May use PI (proportional-integral) + // control in the future. + // getTx returns a value in the range of -27.25 to 27.25. + // We want to get a value from -1 to 1, so we can just divide + // by 27.25. (27.25/27.25 = 1) + rx = (targetX+LOCK_ON_OFFSET)/LOCK_ON_DENOMINATOR; + } else + { + rx = gamepad1.right_stick_x; + } + + // "Artemis Take the Flywheel" logic + double hooperPower, rawDistance = 0; + if(autoShot) + { + double targetY = rawResult.getTy(); + double distanceCalc = + (GOAL_HEIGHT_INCHES - LL_LENS_HEIGHT_INCHES) / Math.tan( + ((LL_MOUNT_ANGLE + targetY) * (Math.PI/180.0)) + ) + ; + + rawDistance = rawResult.isValid() ? distanceCalc : pastDistance; + + //if (meanCounter < 10) { + // aggregateDistance += rawDistance; + // meanCounter++; + //} + //else + //{ + // oldMean = aggregateDistance / 10; + // + // aggregateDistance -= oldMean; + // + // aggregateDistance += rawDistance; + // + // meanDistance = aggregateDistance / 10; + //} + + aggregateDistance += rawDistance; + meanCounter++; + + if(meanCounter > 10) + { + meanDistance = aggregateDistance / 10; + + meanCounter = 0; + aggregateDistance = 0; + + } + + tempDistance = rawDistance; + pastDistance = tempDistance; + + if(tempDistance >= 81.5) + { + hooperPower = (6.25*tempDistance)-644; + } else if(tempDistance >= 76.2) + { + hooperPower = (18.9*tempDistance)+2912; + } else if(tempDistance >= 65.7) + { + hooperPower = (33.3*tempDistance)+1810; + } else if(tempDistance >= 59.1) + { + hooperPower = (22.7*tempDistance)+2507; + } else if(tempDistance >= 54.6) + { + hooperPower = (-22.2*tempDistance)+5163; + } else if(tempDistance >= 47.5) + { + hooperPower = 3950; + } else if(tempDistance >= 40.4) + { + hooperPower = (-14.1*tempDistance)+4619; + } else + { + hooperPower = 3000; + } + meanDistance = 0; + } else + { + // Max target RPM: 4900 + hooperPower = 3000 + (gamepad2.left_trigger * 1900); + } + double driveBreakPower = 1-gamepad1.right_trigger; + + // Gamepad variables: + boolean a1 = gamepad1.a; + boolean a2 = gamepad2.a; + boolean b1 = gamepad1.b; + boolean b2 = gamepad2.b; + boolean x1 = gamepad1.x; + boolean x2 = gamepad2.x; + boolean y1 = gamepad1.y; + boolean y2 = gamepad2.y; + boolean leftBumper1 = gamepad1.left_bumper; + boolean rightBumper1 = gamepad1.right_bumper; + double rightTrigger1 = gamepad1.right_trigger; + boolean start1 = gamepad1.start; + + double lsY1 = -gamepad1.left_stick_y; + double lsX1 = gamepad1.left_stick_x*1.1; + + + double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double botHeadingDegrees = botHeading * (180.0/Math.PI); + // Rotate the movement direction counter to the bot's rotation + double rotX = 1.1*( + lsX1 * Math.cos(-botHeading) - lsY1 * Math.sin(-botHeading) + ); + double rotY = + lsX1 * Math.sin(-botHeading) + lsY1 * Math.cos(-botHeading) + ; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max( + Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1 + ); + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY - rotX - rx) / denominator; + double backRightPower = (rotY + rotX - rx) / denominator; + + // -- Runtime Telemetry -- + double hooperSpeed = (scoring[0].getVelocity()*60)/CPR; + String runtimeSeconds = String.format( + Locale.ENGLISH, "%.4f", getRuntime() + ); + + packet.clearLines(); + packet.put("", + "" + + "Runtime (Seconds): " + runtimeSeconds + "
" + + "
" + + "== Shooting Motor (scoring[0]) ==" + ); + + // runtimeSeconds shouldn't be graphable,so we make it a line + // instead of data. (FtcDashboard graph already adds time as X) + DASH.sendTelemetryPacket(packet); + telemetryD.addData("shooting-input-rpm", hooperPower); + telemetryD.addData("shooting-speed-rpm", hooperSpeed); + telemetryD.update(); + telemetryD.addData("
", ""); + telemetryD.update(); + telemetryD.addData("right-stick-x", rx); + telemetryD.addData("bot-heading", botHeadingDegrees); + telemetryD.addData("auto-shot", autoShot); + telemetryD.addData("ll-distance", rawDistance); + telemetryD.addData("lock-on", lockOn); + telemetryD.addData("valid-target", resultIsValid); + telemetryD.update(); + + telemetry.addData("Runtime (Seconds)", runtimeSeconds); + telemetry.addLine(); + telemetry.addLine("== Shooting Motor (scoring[0]) =="); + telemetry.addData("shooting-input-rpm", hooperPower); + telemetry.addData("shooting-speed-rpm", hooperSpeed); + telemetry.addLine(); + telemetry.addData("right-stick-x", rx); + telemetry.addData("bot-heading", botHeadingDegrees); + telemetry.addData("auto-shot", autoShot); + telemetry.addData("ll-distance", rawDistance); + telemetry.addData("lock-on", lockOn); + telemetry.addData("valid-target", resultIsValid); + telemetry.update(); + + if (a1 && b1 && x1 && y1) + { + logger.stopRun(); + } + + if (start1) + { + imu.resetYaw(); + } + + // LockOn toggle + if(leftBumper1 && ! pastLeftBumper1) + { + lockOn = !lockOn; + + } + + if(y1) + { + scoring[0].setPower(-0.9); + } else + { + scoring[0].setVelocity((hooperPower/60) * CPR); + } + + if (rightBumper1 && a1) + { + scoring[1].setPower(-0.2); + } else if (a1) + { + scoring[1].setVelocity((1150/60)*28); + } else + { + scoring[1].setPower(0); + } + if(rightBumper1 && x1) + { + scoring[2].setPower(-0.9); + } else if(x1) + { + scoring[2].setPower(0.9); + } else + { + scoring[2].setPower(0); + } + + // "Artemis Take the Flywheel" toggle + if(rightTrigger1 == 1 && b1 && ! pastB2) + { + autoShot = !autoShot; + } + + base[0].setPower(frontLeftPower*driveBreakPower); + base[1].setPower(backLeftPower*driveBreakPower); + base[2].setPower(backRightPower*driveBreakPower); + base[3].setPower(frontRightPower*driveBreakPower); + + pastLeftBumper1 = leftBumper1; + pastB2 = b1; + } + } + + @NonNull + private DiagnosticLogger getLogger() + { + DiagnosticLogger logger; + try + { + logger = new DiagnosticLogger( + telemetry, hardwareMap, + null, + new String[] + { + "fl", "rl", + "fr", "rr", + "hooper", "intake1", "intake2" + }, + null, "imu", parameters + ); + } catch (IOException e) + { + throw new RuntimeException(e); + } + return logger; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java new file mode 100644 index 000000000000..024057953edb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java @@ -0,0 +1,373 @@ +package org.firstinspires.ftc.teamcode.TeleOp; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; +import org.firstinspires.ftc.teamcode.util.DiagnosticLogger; + +import java.io.IOException; +import java.util.Locale; + +@TeleOp(group = "Field Centric") +public class Field2Drivers extends LinearOpMode +{ + // Required for the DiagnosticLogger at the bottom of this file: + private final static IMU.Parameters parameters = TeamConstants.getIMUParms(); + + // --- One-time-only constants (non-@Config) --- + // These only update after a restart of the bot. + private final static FtcDashboard DASH = FtcDashboard.getInstance(); + private final static TelemetryPacket packet = new TelemetryPacket(); + + Telemetry telemetryD = DASH.getTelemetry(); + Telemetry telemetryM = new MultipleTelemetry(telemetry, DASH.getTelemetry()); + + @Override + public void runOpMode() + { + // These methods are only initialized when inside of runOpMode: + telemetryD.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + // --- @Config constants --- + final int CPR = TeamConstants.CPR; + final double LOCK_ON_DENOMINATOR = TeamConstants.LOCK_ON_DENOMINATOR; + final double LOCK_ON_OFFSET = TeamConstants.LOCK_ON_OFFSET; + final double LL_MOUNT_ANGLE = TeamConstants.LL_MOUNT_ANGLE; + final double LL_LENS_HEIGHT_INCHES = TeamConstants.LL_LENS_HEIGHT_INCHES; + final double GOAL_HEIGHT_INCHES = TeamConstants.GOAL_HEIGHT_INCHES; + + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + final IMU imu = TeamConstants.getIMU(hardwareMap); + final Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); + + // Final setup before control starts: + imu.initialize(parameters); + limelight.start(); + + telemetryM.addLine("> Ready."); + telemetryM.update(); + + waitForStart(); + + // Ensure telemetry is clear on start. + // FtcDashboard tends to keep telemetry in the buffer even after + // update()-ing. + // (We abuse this a bit later to have nicely formatted text, since + // FtcDashboard also likes alphabetizing data.) + telemetryM.clear(); + + DiagnosticLogger logger = getLogger(); + Thread loggerRuntimeThread = new Thread(logger); + logger.resumeRun(); + loggerRuntimeThread.start(); + + boolean lockOn = false; + boolean autoShot = true; + boolean pastLeftBumper1 = false; + boolean pastB2 = false; + int meanCounter = 0; + double aggregateDistance = 0; + double meanDistance = 0; + double tempDistance = 0; + double pastDistance = 0; + + while(opModeIsActive()) + { + LLResult rawResult = limelight.getLatestResult(); + boolean resultIsValid = rawResult.isValid(); + + // LockOn logic + double rx; + if(lockOn && resultIsValid) + { + double targetX = rawResult.getTx(); + // A simple proportional input; May use PI (proportional-integral) + // control in the future. + // getTx returns a value in the range of -27.25 to 27.25. + // We want to get a value from -1 to 1, so we can just divide + // by 27.25. (27.25/27.25 = 1) + rx = (targetX+LOCK_ON_OFFSET)/LOCK_ON_DENOMINATOR; + } else + { + rx = gamepad1.right_stick_x; + } + + // "Artemis Take the Flywheel" logic + double hooperPower, rawDistance = 0; + if(autoShot) + { + double targetY = rawResult.getTy(); + double distanceCalc = + (GOAL_HEIGHT_INCHES - LL_LENS_HEIGHT_INCHES) / Math.tan( + ((LL_MOUNT_ANGLE + targetY) * (Math.PI/180.0)) + ) + ; + + rawDistance = rawResult.isValid() ? distanceCalc : pastDistance; + + //if (meanCounter < 10) { + // aggregateDistance += rawDistance; + // meanCounter++; + //} + //else + //{ + // oldMean = aggregateDistance / 10; + // + // aggregateDistance -= oldMean; + // + // aggregateDistance += rawDistance; + // + // meanDistance = aggregateDistance / 10; + //} + + aggregateDistance += rawDistance; + meanCounter++; + + if(meanCounter > 10) + { + meanDistance = aggregateDistance / 10; + + meanCounter = 0; + aggregateDistance = 0; + + } + + tempDistance = rawDistance; + pastDistance = tempDistance; + + if(tempDistance >= 65.3) + { + hooperPower = (28.6*tempDistance)+1484; + } else if(tempDistance >= 62.5) + { + hooperPower = (17.9*tempDistance)+2184; + } else if(tempDistance >= 46.2) + { + hooperPower = (4.6*tempDistance)+3012; + } else + { + hooperPower = 3000; + } + + //if(tempDistance >= 81.5) + //{ + // hooperPower = (6.25*tempDistance)-644; + //} else if(tempDistance >= 76.2) + //{ + // hooperPower = (18.9*tempDistance)+2912; + //} else if(tempDistance >= 65.7) + //{ + // hooperPower = (33.3*tempDistance)+1810; + //} else if(tempDistance >= 59.1) + //{ + // hooperPower = (22.7*tempDistance)+2507; + //} else if(tempDistance >= 54.6) + //{ + // hooperPower = (-22.2*tempDistance)+5163; + //} else if(tempDistance >= 47.5) + //{ + // hooperPower = 3950; + //} else if(tempDistance >= 40.4) + //{ + // hooperPower = (-14.1*tempDistance)+4619; + //} else + //{ + // hooperPower = 3000; + //} + meanDistance = 0; + } else + { + // Max target RPM: 4900 + hooperPower = 3000 + (gamepad2.left_trigger * 1900); + } + double driveBreakPower = 1-gamepad1.right_trigger; + + // Gamepad variables: + boolean a1 = gamepad1.a; + boolean a2 = gamepad2.a; + boolean b1 = gamepad1.b; + boolean b2 = gamepad2.b; + boolean x1 = gamepad1.x; + boolean x2 = gamepad2.x; + boolean y1 = gamepad1.y; + boolean y2 = gamepad2.y; + boolean leftBumper1 = gamepad1.left_bumper; + boolean rightBumper2 = gamepad2.right_bumper; + double rightTrigger2 = gamepad2.right_trigger; + boolean start1 = gamepad1.start; + + double lsY1 = -gamepad1.left_stick_y; + double lsX1 = gamepad1.left_stick_x*1.1; + + + double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double botHeadingDegrees = botHeading * (180.0/Math.PI); + // Rotate the movement direction counter to the bot's rotation + double rotX = 1.1*( + lsX1 * Math.cos(-botHeading) - lsY1 * Math.sin(-botHeading) + ); + double rotY = + lsX1 * Math.sin(-botHeading) + lsY1 * Math.cos(-botHeading) + ; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + + // Sample of our code, Please do not touch + + double denominator = Math.max( + Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1 + ); + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY - rotX - rx) / denominator; + double backRightPower = (rotY + rotX - rx) / denominator; + + // -- Runtime Telemetry -- + double hooperSpeed = (scoring[0].getVelocity()*60)/CPR; + String runtimeSeconds = String.format( + Locale.ENGLISH, "%.4f", getRuntime() + ); + + packet.clearLines(); + packet.put("", + "" + + "Runtime (Seconds): " + runtimeSeconds + "
" + + "
" + + "== Shooting Motor (scoring[0]) ==" + ); + + // runtimeSeconds shouldn't be graphable,so we make it a line + // instead of data. (FtcDashboard graph already adds time as X) + DASH.sendTelemetryPacket(packet); + telemetryD.addData("shooting-input-rpm", hooperPower); + telemetryD.addData("shooting-speed-rpm", hooperSpeed); + telemetryD.update(); + telemetryD.addData("
", ""); + telemetryD.update(); + telemetryD.addData("right-stick-x", rx); + telemetryD.addData("bot-heading", botHeadingDegrees); + telemetryD.addData("auto-shot", autoShot); + telemetryD.addData("ll-distance", rawDistance); + telemetryD.addData("lock-on", lockOn); + telemetryD.addData("valid-target", resultIsValid); + telemetryD.update(); + + telemetry.addData("Runtime (Seconds)", runtimeSeconds); + telemetry.addLine(); + telemetry.addLine("== Shooting Motor (scoring[0]) =="); + telemetry.addData("shooting-input-rpm", hooperPower); + telemetry.addData("shooting-speed-rpm", hooperSpeed); + telemetry.addLine(); + telemetry.addData("right-stick-x", rx); + telemetry.addData("bot-heading", botHeadingDegrees); + telemetry.addData("auto-shot", autoShot); + telemetry.addData("ll-distance", rawDistance); + telemetry.addData("lock-on", lockOn); + telemetry.addData("valid-target", resultIsValid); + telemetry.update(); + + if (a1 && b1 && x1 && y1) + { + logger.stopRun(); + } + + if (start1) + { + imu.resetYaw(); + } + + // LockOn toggle + if(leftBumper1 && ! pastLeftBumper1) + { + lockOn = !lockOn; + + } + + if(y2) + { + scoring[0].setPower(-0.9); + } else + { + scoring[0].setVelocity((hooperPower/60) * CPR); + } + + if (rightBumper2 && a2) + { + scoring[1].setPower(-0.2); + } else if (a2) + { + scoring[1].setVelocity((1150/60)*28); + } else + { + scoring[1].setPower(0); + } + if(rightBumper2 && x2) + { + scoring[2].setPower(-0.9); + } else if(x2) + { + scoring[2].setPower(0.9); + } else + { + scoring[2].setPower(0); + } + + // "Artemis Take the Flywheel" toggle + if(rightTrigger2 == 1 && b2 && ! pastB2) + { + autoShot = !autoShot; + } + + base[0].setPower(frontLeftPower*driveBreakPower); + base[1].setPower(backLeftPower*driveBreakPower); + base[2].setPower(backRightPower*driveBreakPower); + base[3].setPower(frontRightPower*driveBreakPower); + + pastLeftBumper1 = leftBumper1; + pastB2 = b2; + } + } + + @NonNull + private DiagnosticLogger getLogger() + { + DiagnosticLogger logger; + try + { + logger = new DiagnosticLogger( + telemetry, hardwareMap, + null, + new String[] + { + "fl", "rl", + "fr", "rr", + "hooper", "intake1", "intake2" + }, + null, "imu", parameters + ); + } catch (IOException e) + { + throw new RuntimeException(e); + } + return logger; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2DriversPower.java similarity index 61% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2DriversPower.java index 81177be001f0..6aa70eb79174 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2DriversPower.java @@ -5,59 +5,57 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; - +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.IMU; -import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; - -import org.firstinspires.ftc.teamcode.util.Constants; import org.firstinspires.ftc.teamcode.util.DiagnosticLogger; +import org.firstinspires.ftc.teamcode.util.TeamConstants; import java.io.IOException; -import java.util.List; import java.util.Locale; @TeleOp(group = "Field Centric") -public class TeleOpMecanumField2Drivers extends LinearOpMode +public class Field2DriversPower extends LinearOpMode { - private final static int CPR = Constants.CPR; - private final static double LOCK_ON_DENOMINATOR = Constants.LOCK_ON_DENOMINATOR; - private static IMU.Parameters parameters; + // Required for the DiagnosticLogger at the bottom of this file: + private final static IMU.Parameters parameters = TeamConstants.getIMUParms(); + // --- One-time-only constants (non-@Config) --- + // These only update after a restart of the bot. private final static FtcDashboard DASH = FtcDashboard.getInstance(); - private final static TelemetryPacket packet = new TelemetryPacket(); + Telemetry telemetryD = DASH.getTelemetry(); + Telemetry telemetryM = new MultipleTelemetry(telemetry, DASH.getTelemetry()); + @Override public void runOpMode() { - Telemetry telemetryD = DASH.getTelemetry(); - Telemetry telemetryM = new MultipleTelemetry(telemetry, DASH.getTelemetry()); - + // These methods are only initialized when inside of runOpMode: telemetryD.setDisplayFormat(Telemetry.DisplayFormat.HTML); telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); - // Note to self: only use hardwareMap in or after the initialization! - DcMotorEx[] base = Constants.getDriveMotors(hardwareMap); - DcMotorEx[] scoring = Constants.getScoringMotors(hardwareMap); - IMU imu = Constants.getIMU(hardwareMap); - Limelight3A limelight = Constants.getLimelight(hardwareMap); + // --- @Config constants --- + final int CPR = TeamConstants.CPR; + final double LOCK_ON_DENOMINATOR = TeamConstants.LOCK_ON_DENOMINATOR; + final double LOCK_ON_OFFSET = TeamConstants.LOCK_ON_OFFSET; - parameters = Constants.getIMUParms(); - imu.initialize(parameters); + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + final IMU imu = TeamConstants.getIMU(hardwareMap); + final Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); + // Final setup before control starts: + imu.initialize(parameters); limelight.start(); - boolean lockOn = false; - telemetryM.addLine("> Ready."); telemetryM.update(); @@ -75,59 +73,72 @@ public void runOpMode() logger.resumeRun(); loggerRuntimeThread.start(); - boolean pastLeftBumper = false; + boolean lockOn = false; + boolean pastLeftBumper1 = false; while(opModeIsActive()) { LLResult rawResult = limelight.getLatestResult(); - List fiducial = rawResult.getFiducialResults(); boolean resultIsValid = rawResult.isValid(); - boolean leftBumper = gamepad1.left_bumper; - double targetX; - - double y = -gamepad1.left_stick_y; - double x = gamepad1.left_stick_x*1.1; + // LockOn logic double rx; if(lockOn && resultIsValid) { - targetX = rawResult.getTx(); + double targetX = rawResult.getTx(); // A simple proportional input; May use PI (proportional-integral) // control in the future. // getTx returns a value in the range of -27.25 to 27.25. // We want to get a value from -1 to 1, so we can just divide // by 27.25. (27.25/27.25 = 1) - rx = targetX/LOCK_ON_DENOMINATOR; + rx = (targetX+LOCK_ON_OFFSET)/LOCK_ON_DENOMINATOR; } else { rx = gamepad1.right_stick_x; } - double brakePower = 1-gamepad1.right_trigger; - // Max target RPM: 4900 - double bigHooperPower = 2500 + (gamepad2.left_trigger * 2400); + double hooperPower = gamepad2.left_trigger; + double driveBreakPower = 1-gamepad1.right_trigger; + + // Gamepad variables: + boolean a1 = gamepad1.a; + boolean a2 = gamepad2.a; + boolean b1 = gamepad1.b; + boolean x1 = gamepad1.x; + boolean x2 = gamepad2.x; + boolean y1 = gamepad1.y; + boolean y2 = gamepad2.y; + boolean leftBumper1 = gamepad1.left_bumper; + boolean rightBumper2 = gamepad2.right_bumper; + boolean start1 = gamepad1.start; + + double lsY1 = -gamepad1.left_stick_y; + double lsX1 = gamepad1.left_stick_x*1.1; + - // --- Trigonometry Warning !!! --- (Continues for 14 lines) double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double botHeadingDegrees = botHeading * (180.0/Math.PI); - // Rotate the movement direction counter to the bot's rotation - double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading); - double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading); - - rotX = rotX * 1.1; + double rotX = 1.1*( + lsX1 * Math.cos(-botHeading) - lsY1 * Math.sin(-botHeading) + ); + double rotY = + lsX1 * Math.sin(-botHeading) + lsY1 * Math.cos(-botHeading) + ; // Denominator is the largest motor power (absolute value) or 1 // This ensures all the powers maintain the same ratio, // but only if at least one is out of the range [-1, 1] - double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); + double denominator = Math.max( + Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1 + ); double frontLeftPower = (rotY + rotX + rx) / denominator; double backLeftPower = (rotY - rotX + rx) / denominator; double frontRightPower = (rotY - rotX - rx) / denominator; double backRightPower = (rotY + rotX - rx) / denominator; // -- Runtime Telemetry -- - double bigHooperSpeed = (scoring[0].getVelocity()*60)/CPR; + double hooperSpeed = (scoring[0].getVelocity()*60)/CPR; String runtimeSeconds = String.format( Locale.ENGLISH, "%.4f", getRuntime() ); @@ -143,8 +154,8 @@ public void runOpMode() // runtimeSeconds shouldn't be graphable,so we make it a line // instead of data. (FtcDashboard graph already adds time as X) DASH.sendTelemetryPacket(packet); - telemetryD.addData("shooting-input-rpm", bigHooperPower); - telemetryD.addData("shooting-speed-rpm", bigHooperSpeed); + telemetryD.addData("shooting-input-rpm", hooperPower); + telemetryD.addData("shooting-speed-rpm", hooperSpeed); telemetryD.update(); telemetryD.addData("
", ""); telemetryD.update(); @@ -157,8 +168,8 @@ public void runOpMode() telemetry.addData("Runtime (Seconds)", runtimeSeconds); telemetry.addLine(); telemetry.addLine("== Shooting Motor (scoring[0]) =="); - telemetry.addData("shooting-input-rpm", bigHooperPower); - telemetry.addData("shooting-speed-rpm", bigHooperSpeed); + telemetry.addData("shooting-input-rpm", hooperPower); + telemetry.addData("shooting-speed-rpm", hooperSpeed); telemetry.addLine(); telemetry.addData("right-stick-x", rx); telemetry.addData("bot-heading", botHeadingDegrees); @@ -166,49 +177,58 @@ public void runOpMode() telemetry.addData("valid-target", resultIsValid); telemetry.update(); - if (gamepad1.a && gamepad1.b && gamepad1.x && gamepad1.y) + if (a1 && b1 && x1 && y1) { logger.stopRun(); } - if (gamepad1.start) + if (start1) { imu.resetYaw(); } - // pastLeftBumper initializes to false - if(leftBumper && ! pastLeftBumper) + // LockOn toggle + if(leftBumper1 && ! pastLeftBumper1) { - // LockOn toggle lockOn = !lockOn; } - if(gamepad2.y) + if(y2) { scoring[0].setPower(-0.9); } else { - scoring[0].setVelocity((bigHooperPower / 60)*CPR); + scoring[0].setPower(hooperPower); } - if (gamepad2.a) - { - scoring[1].setPower(0.9); - } else if (gamepad2.x) + if (rightBumper2 && a2) { scoring[1].setPower(-0.9); + } else if (a2) + { + scoring[1].setPower(0.9); } else { scoring[1].setPower(0); } + if(rightBumper2 && x2) + { + scoring[2].setPower(-0.9); + } else if(x2) + { + scoring[2].setPower(0.9); + } else + { + scoring[2].setPower(0); + } - base[0].setPower(frontLeftPower*brakePower); - base[1].setPower(backLeftPower*brakePower); - base[2].setPower(backRightPower*brakePower); - base[3].setPower(frontRightPower*brakePower); + base[0].setPower(frontLeftPower*driveBreakPower); + base[1].setPower(backLeftPower*driveBreakPower); + base[2].setPower(backRightPower*driveBreakPower); + base[3].setPower(frontRightPower*driveBreakPower); - pastLeftBumper = leftBumper; + pastLeftBumper1 = leftBumper1; } } @@ -223,9 +243,9 @@ private DiagnosticLogger getLogger() null, new String[] { - "frontLeftMotor", "backLeftMotor", - "frontRightMotor", "backRightMotor", - "bigHooperMotor", "smallHooperMotor" + "fl", "rl", + "fr", "rr", + "hooper", "intake1", "intake2" }, null, "imu", parameters ); @@ -235,4 +255,4 @@ private DiagnosticLogger getLogger() } return logger; } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot1Driver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot1Driver.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot1Driver.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot1Driver.java index 96b9ce5f11de..efbc820651e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot1Driver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot1Driver.java @@ -17,7 +17,7 @@ import java.io.IOException; @TeleOp(group = "Robot Centric") -public class TeleOpMecanumRobot1Driver extends LinearOpMode { +public class Robot1Driver extends LinearOpMode { private IMU.Parameters parameters; @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot2Drivers.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot2Drivers.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot2Drivers.java index 5ea06c9e8e5f..b5909bc23471 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot2Drivers.java @@ -17,7 +17,7 @@ import java.io.IOException; @TeleOp(group = "Robot Centric") -public class TeleOpMecanumRobot2Drivers extends LinearOpMode { +public class Robot2Drivers extends LinearOpMode { private IMU.Parameters parameters; @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField1Driver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField1Driver.java deleted file mode 100644 index 421319fa85cb..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField1Driver.java +++ /dev/null @@ -1,184 +0,0 @@ -package org.firstinspires.ftc.teamcode.TeleOp; - -import static com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior.BRAKE; - -import androidx.annotation.NonNull; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.IMU; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.VoltageSensor; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.util.DiagnosticLogger; - -import java.io.IOException; - -@TeleOp(group = "Field Centric") -public class TeleOpMecanumField1Driver extends LinearOpMode { - private IMU.Parameters parameters; - @Override - public void runOpMode() throws InterruptedException { - // CPR of a Rev HD Hex Motor. - final int CPR = 28; - // Declare motors - // Make sure your ID's match your configuration - DcMotorEx frontLeftMotor = hardwareMap.get(DcMotorEx.class, "frontLeftMotor"); - DcMotorEx backLeftMotor = hardwareMap.get(DcMotorEx.class, "backLeftMotor"); - DcMotorEx frontRightMotor = hardwareMap.get(DcMotorEx.class, "frontRightMotor"); - DcMotorEx backRightMotor = hardwareMap.get(DcMotorEx.class, "backRightMotor"); - DcMotorEx bigHooperMotor = hardwareMap.get(DcMotorEx.class, "bigHooperMotor"); - DcMotorEx smallHooperMotor = hardwareMap.get(DcMotorEx.class, "smallHooperMotor"); - - VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next(); - - backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); - bigHooperMotor.setDirection(DcMotorSimple.Direction.REVERSE); - - frontLeftMotor.setZeroPowerBehavior(BRAKE); - frontRightMotor.setZeroPowerBehavior(BRAKE); - backRightMotor.setZeroPowerBehavior(BRAKE); - backLeftMotor.setZeroPowerBehavior(BRAKE); - - // Retrieve the IMU from the hardware map - IMU imu = hardwareMap.get(IMU.class, "imu"); - // The private parameters variable is passed to the logger at the bottom of this class. - this.parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.LEFT, - RevHubOrientationOnRobot.UsbFacingDirection.UP)); - // Without this, the REV Hub's orientation is assumed to be logo UP / USB FORWARD - imu.initialize(parameters); - - DiagnosticLogger logger = getLogger(); - Thread loggerRuntimeThread = new Thread(logger); - logger.resumeRun(); - loggerRuntimeThread.start(); - - waitForStart(); - - if (isStopRequested()) return; - - while (opModeIsActive()) { - double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed - double x = gamepad1.left_stick_x*1.1; - double rx = gamepad1.right_stick_x; - double brakePower = 1-gamepad1.right_trigger; - double bigHooperPower = gamepad1.left_trigger * 6000; - - double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); - - // Rotate the movement direction counter to the bot's rotation - double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading); - double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading); - - rotX = rotX * 1.1; - - // Denominator is the largest motor power (absolute value) or 1 - // This ensures all the powers maintain the same ratio, - // but only if at least one is out of the range [-1, 1] - double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); - double frontLeftPower = (rotY + rotX + rx) / denominator; - double backLeftPower = (rotY - rotX + rx) / denominator; - double frontRightPower = (rotY - rotX - rx) / denominator; - double backRightPower = (rotY + rotX - rx) / denominator; - - telemetry.addData("1, LT", gamepad1.left_trigger); - telemetry.addData("1, RT", gamepad1.right_trigger); - telemetry.addData("1, LB", gamepad1.left_bumper); - telemetry.addData("1, LB", gamepad1.right_bumper); - telemetry.addData("1, LT", gamepad1.left_trigger); - telemetry.addData("1, LS-X", gamepad1.left_stick_x); - telemetry.addData("1, LS-Y", gamepad1.left_stick_y); - telemetry.addData("1, RS-X", gamepad1.right_stick_x); - telemetry.addData("1, RS-Y", gamepad1.right_stick_y); - telemetry.addData("1, DU", gamepad1.dpad_up); - telemetry.addData("1, DL", gamepad1.dpad_left); - telemetry.addData("1, DD", gamepad1.dpad_down); - telemetry.addData("1, DR", gamepad1.dpad_right); - telemetry.addData("1, A", gamepad1.a); - telemetry.addData("1, B", gamepad1.b); - telemetry.addData("1, X", gamepad1.x); - telemetry.addData("1, Y", gamepad1.y); - - telemetry.addData("2, LT", gamepad2.left_trigger); - telemetry.addData("2, RT", gamepad2.right_trigger); - telemetry.addData("2, LB", gamepad2.left_bumper); - telemetry.addData("2, LB", gamepad2.right_bumper); - telemetry.addData("2, LT", gamepad2.left_trigger); - telemetry.addData("2, LS-X", gamepad2.left_stick_x); - telemetry.addData("2, LS-Y", gamepad2.left_stick_y); - telemetry.addData("2, RS-X", gamepad2.right_stick_x); - telemetry.addData("2, RS-Y", gamepad2.right_stick_y); - telemetry.addData("2, DU", gamepad2.dpad_up); - telemetry.addData("2, DL", gamepad2.dpad_left); - telemetry.addData("2, DD", gamepad2.dpad_down); - telemetry.addData("2, DR", gamepad2.dpad_right); - telemetry.addData("2, A", gamepad2.a); - telemetry.addData("2, B", gamepad2.b); - telemetry.addData("2, X", gamepad2.x); - telemetry.addData("2, Y", gamepad2.y); - - telemetry.update(); - - if (gamepad1.a && gamepad1.b && gamepad1.x && gamepad1.y) - { - logger.stopRun(); - } - - if (gamepad1.start) - { - imu.resetYaw(); - } - - - if(gamepad2.y) - { - bigHooperMotor.setPower(-0.9); - } else - { - bigHooperMotor.setVelocity((bigHooperPower / 60)*CPR); - } - - if (gamepad1.a) - { - smallHooperMotor.setPower(0.9); - } else if (gamepad1.x) { - smallHooperMotor.setPower(-0.9); - } else - { - smallHooperMotor.setPower(0); - } - - frontLeftMotor.setPower(frontLeftPower*brakePower); - backLeftMotor.setPower(backLeftPower*brakePower); - frontRightMotor.setPower(frontRightPower*brakePower); - backRightMotor.setPower(backRightPower*brakePower); - } - } - - @NonNull - private DiagnosticLogger getLogger() - { - DiagnosticLogger logger; - try - { - logger = new DiagnosticLogger( - telemetry, hardwareMap, - new String[] - { - "frontLeftMotor", "backLeftMotor", - "frontRightMotor", "backRightMotor", - "bigHooperMotor", "smallHooperMotor" - }, - null, null, "imu", parameters - ); - } catch (IOException e) - { - telemetry.addData("IOException", e.getMessage()); - throw new RuntimeException(e); - } - return logger; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoBlue.java new file mode 100644 index 000000000000..b487fce5fd73 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoBlue.java @@ -0,0 +1,69 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class TwoBallAutoBlue extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1555); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(40); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1875); + sleep(4700); + scoring[1].setPower(0); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(2500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(650); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(260); + base[0].setPower(1); + base[1].setPower(-1); + base[2].setPower(1); + base[3].setPower(-1); + sleep(1000); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoRed.java new file mode 100644 index 000000000000..af419e9886d2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoRed.java @@ -0,0 +1,69 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class TwoBallAutoRed extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1400); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(40); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1800); + sleep(4800); + scoring[1].setPower(0); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(3500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(650); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(260); + base[0].setPower(-1); + base[1].setPower(1); + base[2].setPower(-1); + base[3].setPower(1); + sleep(1000); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java new file mode 100644 index 000000000000..06970b31332f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -0,0 +1,81 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.acmerobotics.dashboard.config.Config; +import com.pedropathing.ftc.localization.constants.DriveEncoderConstants; +import com.pedropathing.ftc.localization.Encoder; + +import com.pedropathing.follower.Follower; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; + +import com.pedropathing.paths.PathConstraints; + +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +@Config +public class Constants { + public static double multiplierForward = 0.006140795931692835; + public static double multiplierStrafe = -0.0072753221875072275; + public static double multiplierTurn = 0.015429377289117809; + + + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(20.4); + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(1) + .leftFrontMotorName("fl") + .leftRearMotorName("rl") + .rightRearMotorName("rr") + .rightFrontMotorName("fr") + .leftFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .leftRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .xVelocity(50.15369745035338) + // .yVelocity(56) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD); + public static DriveEncoderConstants localizerConstants = new DriveEncoderConstants() + .leftFrontMotorName("fl") + .leftRearMotorName("rl") + .rightRearMotorName("rr") + .rightFrontMotorName("fr") + .robotLength(17.6) + .forwardTicksToInches(multiplierForward) + .strafeTicksToInches(multiplierStrafe) + .turnTicksToInches(multiplierTurn) + .robotWidth(16.6) + .leftFrontEncoderDirection(Encoder.FORWARD) + .leftRearEncoderDirection(Encoder.FORWARD) + .rightRearEncoderDirection(Encoder.REVERSE) + .rightFrontEncoderDirection(Encoder.FORWARD); + /** + These are the PathConstraints in order: + tValueConstraint, velocityConstraint, translationalConstraint, headingConstraint, timeoutConstraint, + brakingStrength, BEZIER_CURVE_SEARCH_LIMIT, brakingStart + + The BEZIER_CURVE_SEARCH_LIMIT should typically be left at 10 and shouldn't be changed. + */ + + public static PathConstraints pathConstraints = new PathConstraints( 1.0, // tValueConstraint + 50.0, // velocityConstraint + 50.0, // translationalConstraint + Math.toRadians(180), // headingConstraint + 3.0, // timeoutConstraint + 1.0, // brakingStrength + 10, // BEZIER_CURVE_SEARCH_LIMIT (leave at 10) + 0.3 // brakingStart + ); + + //Add custom localizers or drivetrains here + public static Follower createFollower(HardwareMap hardwareMap) { + return new FollowerBuilder(followerConstants, hardwareMap) + .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) + .driveEncoderLocalizer(localizerConstants) + // .forwardZeroPowerAcceleration(deceleration) + // .lateralZeroPowerAcceleration(deceleration) + .build(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java new file mode 100644 index 000000000000..bd20d3ff08b9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -0,0 +1,1362 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.draw; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM; + +import com.bylazar.configurables.PanelsConfigurables; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.configurables.annotations.IgnoreConfigurable; +import com.bylazar.field.FieldManager; +import com.bylazar.field.PanelsField; +import com.bylazar.field.Style; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.ErrorCalculator; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.*; +import com.pedropathing.math.*; +import com.pedropathing.paths.*; +import com.pedropathing.telemetry.SelectableOpMode; +import com.pedropathing.util.*; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import java.util.ArrayList; +import java.util.List; + +/** + * This is the Tuning class. It contains a selection menu for various tuning OpModes. + * + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 6/26/2025 + */ +@Configurable + +@TeleOp(name = "Tuning", group = "Pedro Pathing") +public class Tuning extends SelectableOpMode { + public static Follower follower; + + @IgnoreConfigurable + static PoseHistory poseHistory; + + @IgnoreConfigurable + + static TelemetryManager telemetryM; + + @IgnoreConfigurable + static ArrayList changes = new ArrayList<>(); + + public Tuning() { + super("Select a Tuning OpMode", s -> { + s.folder("Localization", l -> { + l.add("Localization Test", LocalizationTest::new); + l.add("Forward Tuner", ForwardTuner::new); + l.add("Lateral Tuner", LateralTuner::new); + l.add("Turn Tuner", TurnTuner::new); + }); + s.folder("Automatic", a -> { + a.add("Forward Velocity Tuner", ForwardVelocityTuner::new); + a.add("Lateral Velocity Tuner", LateralVelocityTuner::new); + a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new); + a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new); + }); + s.folder("Manual", p -> { + p.add("Translational Tuner", TranslationalTuner::new); + p.add("Heading Tuner", HeadingTuner::new); + p.add("Drive Tuner", DriveTuner::new); + p.add("Line Tuner", Line::new); + p.add("Centripetal Tuner", CentripetalTuner::new); + }); + s.folder("Tests", p -> { + p.add("Line", Line::new); + p.add("Triangle", Triangle::new); + p.add("Circle", Circle::new); + }); + }); + } + + @Override + public void onSelect() { + if (follower == null) { + follower = Constants.createFollower(hardwareMap); + PanelsConfigurables.INSTANCE.refreshClass(this); + } else { + follower = Constants.createFollower(hardwareMap); + } + + follower.setStartingPose(new Pose()); + + poseHistory = follower.getPoseHistory(); + + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + + Drawing.init(); + } + + @Override + public void onLog(List lines) {} + + public static void drawOnlyCurrent() { + try { + Drawing.drawRobot(follower.getPose()); + Drawing.sendPacket(); + } catch (Exception e) { + throw new RuntimeException("Drawing failed " + e); + } + } + + public static void draw() { + Drawing.drawDebug(follower); + } + + /** This creates a full stop of the robot by setting the drive motors to run at 0 power. */ + public static void stopRobot() { + follower.startTeleopDrive(true); + follower.setTeleOpDrive(0,0,0,true); + } +} + +/** + * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a + * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot. + * You should use this to check the robot's localization. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class LocalizationTest extends OpMode { + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + } + + /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("This will print your robot's position to telemetry while " + + "allowing robot control through a basic mecanum drive on gamepad 1."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + follower.update(); + } + + /** + * This updates the robot's pose estimate, the simple mecanum drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); + follower.update(); + + telemetryM.debug("x:" + follower.getPose().getX()); + telemetryM.debug("y:" + follower.getPose().getY()); + telemetryM.debug("heading:" + follower.getPose().getHeading()); + telemetryM.debug("total heading:" + follower.getTotalHeading()); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the forward ticks to inches in your + * localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class ForwardTuner extends OpMode { + public static double DISTANCE = 96; + + @Override + public void init() { + System.out.println("Hello, World!"); + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Distance Moved: " + (follower.getPose().getX() - 72)); + telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getX() - 72) / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the strafe ticks to inches in your + * localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 2.0, 6/26/2025 + */ +class LateralTuner extends OpMode { + public static double DISTANCE = 96; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Distance Moved: " + (follower.getPose().getY() - 72)); + telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getY() - 72) / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current angle in ticks to the specified angle in radians. So, to use this, run the + * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground. + * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run + * multiple trials and average the results. Then, input the multiplier into the turning ticks to + * radians in your localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class TurnTuner extends OpMode { + public static double ANGLE = 4 * Math.PI; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Total Angle: " + follower.getTotalHeading()); + telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the ForwardVelocityTuner autonomous follower OpMode. This runs the robot forwards at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class ForwardVelocityTuner extends OpMode { + private final ArrayList velocities = new ArrayList<>(); + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); + telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.debug("pose", follower.getPose()); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + follower.startTeleopDrive(true); + follower.update(); + end = false; + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run forward enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + + if (!end) { + if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) { + end = true; + stopRobot(); + } else { + follower.setTeleOpDrive(1,0,0,true); + //double currentVelocity = Math.abs(follower.getVelocity().getXComponent()); + double currentVelocity = Math.abs(follower.poseTracker.getLocalizer().getVelocity().getX()); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + stopRobot(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + telemetryM.debug("Forward Velocity: " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); + + for (int i = 0; i < velocities.size(); i++) { + telemetry.addData(String.valueOf(i), velocities.get(i)); + } + + telemetryM.update(telemetry); + telemetry.update(); + + if (gamepad1.aWasPressed()) { + follower.setXVelocity(average); + String message = "XMovement: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the StrafeVelocityTuner autonomous follower OpMode. This runs the robot left at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class LateralVelocityTuner extends OpMode { + private final ArrayList velocities = new ArrayList<>(); + + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the drive motors as well as the cache of velocities and the Panels + * telemetryM. + */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the left."); + telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); + telemetryM.debug("Press B on Gamepad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run left at full power. */ + @Override + public void start() { + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + follower.startTeleopDrive(true); + follower.update(); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run sideways enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + if (!end) { + if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) { + end = true; + stopRobot(); + } else { + follower.setTeleOpDrive(0,1,0,true); + double currentVelocity = Math.abs(follower.getVelocity().dot(new Vector(1, Math.PI / 2))); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + stopRobot(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + + telemetryM.debug("Strafe Velocity: " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.setYVelocity(average); + String message = "YMovement: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the ForwardZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot + * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +class ForwardZeroPowerAccelerationTuner extends OpMode { + private final ArrayList accelerations = new ArrayList<>(); + public static double VELOCITY = 30; + + private double previousVelocity; + private long previousTimeNano; + + private boolean stopping; + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the Panels telemetryM. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second."); + telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); + telemetryM.debug("Press B on Gamepad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + follower.startTeleopDrive(false); + follower.update(); + follower.setTeleOpDrive(1,0,0,true); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + Vector heading = new Vector(1.0, follower.getPose().getHeading()); + if (!end) { + if (!stopping) { + if (follower.getVelocity().dot(heading) > VELOCITY) { + previousVelocity = follower.getVelocity().dot(heading); + previousTimeNano = System.nanoTime(); + stopping = true; + follower.setTeleOpDrive(0,0,0,true); + } + } else { + double currentVelocity = follower.getVelocity().dot(heading); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { + end = true; + } + } + } else { + double average = 0; + for (double acceleration : accelerations) { + average += acceleration; + } + average /= accelerations.size(); + + telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.getConstants().setForwardZeroPowerAcceleration(average); + String message = "Forward Zero Power Acceleration: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot + * to the left until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class LateralZeroPowerAccelerationTuner extends OpMode { + private final ArrayList accelerations = new ArrayList<>(); + public static double VELOCITY = 30; + private double previousVelocity; + private long previousTimeNano; + private boolean stopping; + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run to the left until it reaches " + VELOCITY + " inches per second."); + telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + follower.startTeleopDrive(false); + follower.update(); + follower.setTeleOpDrive(0,1,0,true); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + Vector heading = new Vector(1.0, follower.getPose().getHeading() - Math.PI / 2); + if (!end) { + if (!stopping) { + if (Math.abs(follower.getVelocity().dot(heading)) > VELOCITY) { + previousVelocity = Math.abs(follower.getVelocity().dot(heading)); + previousTimeNano = System.nanoTime(); + stopping = true; + follower.setTeleOpDrive(0,0,0,true); + } + } else { + double currentVelocity = Math.abs(follower.getVelocity().dot(heading)); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { + end = true; + } + } + } else { + double average = 0; + for (double acceleration : accelerations) { + average += acceleration; + } + average /= accelerations.size(); + + telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.getConstants().setLateralZeroPowerAcceleration(average); + String message = "Lateral Zero Power Acceleration: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the Translational PIDF Tuner OpMode. It will keep the robot in place. + * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class TranslationalTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the Follower and creates the forward and backward Paths. */ + @Override + public void init_loop() { + telemetryM.debug("This will activate the translational PIDF(s)"); + telemetryM.debug("The robot will try to stay in place while you push it laterally."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateTranslational(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent()); + telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent()); + telemetryM.update(telemetry); + } +} + +/** + * This is the Heading PIDF Tuner OpMode. It will keep the robot in place. + * The user should try to turn the robot to test the PIDF and adjust the PIDF values accordingly. + * It will try to keep the robot at a constant heading while the user tries to turn it. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class HeadingTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will activate the heading PIDF(s)."); + telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateHeading(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getHeadingError()); + telemetryM.update(telemetry); + } +} + +/** + * This is the Drive PIDF Tuner OpMode. It will run the robot in a straight line going forward and back. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class DriveTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private PathChain forwards; + private PathChain backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward."); + telemetryM.debug("The robot will go forward and backward continuously along the path."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateDrive(); + + forwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))) + .setConstantHeadingInterpolation(0) + .build(); + + backwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))) + .setConstantHeadingInterpolation(0) + .build(); + + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving forward?: " + forward); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]); + telemetryM.update(telemetry); + } +} + +/** + * This is the Line Test Tuner OpMode. It will drive the robot forward and back + * The user should push the robot laterally and angular to test out the drive, heading, and translational PIDFs. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class Line extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the Follower and creates the forward and backward Paths. */ + @Override + public void init_loop() { + telemetryM.debug("This will activate all the PIDF(s)"); + telemetryM.debug("The robot will go forward and backward continuously along the path while correcting."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving Forward?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Centripetal Tuner OpMode. It runs the robot in a specified distance + * forward and to the left. On reaching the end of the forward Path, the robot runs the backward + * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety + * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the + * centripetal Vector. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +class CentripetalTuner extends OpMode { + public static double DISTANCE = 20; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. + * Additionally, this initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); + telemetryM.debug("The robot will go continuously along the path."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72))); + backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72))); + + backwards.setTangentHeadingInterpolation(); + backwards.reverseHeadingInterpolation(); + + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving away from the origin along the curve?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Triangle autonomous OpMode. + * It runs the robot in a triangle, with the starting point being the bottom-middle point. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge + * @version 1.0, 12/30/2024 + */ +class Triangle extends OpMode { + + private final Pose startPose = new Pose(72, 72, Math.toRadians(0)); + private final Pose interPose = new Pose(24 + 72, -24 + 72, Math.toRadians(90)); + private final Pose endPose = new Pose(24 + 72, 24 + 72, Math.toRadians(45)); + + private PathChain triangle; + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (follower.atParametricEnd()) { + follower.followPath(triangle, true); + } + } + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + @Override + public void init_loop() { + telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point."); + telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** Creates the PathChain for the "triangle".*/ + @Override + public void start() { + follower.setStartingPose(startPose); + + triangle = follower.pathBuilder() + .addPath(new BezierLine(startPose, interPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), interPose.getHeading()) + .addPath(new BezierLine(interPose, endPose)) + .setLinearHeadingInterpolation(interPose.getHeading(), endPose.getHeading()) + .addPath(new BezierLine(endPose, startPose)) + .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading()) + .build(); + + follower.followPath(triangle); + } +} + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class Circle extends OpMode { + public static double RADIUS = 10; + private PathChain circle; + + public void start() { + circle = follower.pathBuilder() + .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .build(); + follower.followPath(circle); + } + + @Override + public void init_loop() { + telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); + telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode."); + telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (follower.atParametricEnd()) { + follower.followPath(circle); + } + } +} + +/** + * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. + * + * @author Lazar - 19234 + * @version 1.1, 5/19/2025 + */ +class Drawing { + public static final double ROBOT_RADIUS = 9; // woah + private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); + + private static final Style robotLook = new Style( + "", "#3F51B5", 0.75 + ); + private static final Style historyLook = new Style( + "", "#4CAF50", 0.75 + ); + + /** + * This prepares Panels Field for using Pedro Offsets + */ + public static void init() { + panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING()); + } + + /** + * This draws everything that will be used in the Follower's telemetryDebug() method. This takes + * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * + * @param follower Pedro Follower instance. + */ + public static void drawDebug(Follower follower) { + if (follower.getCurrentPath() != null) { + drawPath(follower.getCurrentPath(), robotLook); + Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); + drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook); + } + drawPoseHistory(follower.getPoseHistory(), historyLook); + drawRobot(follower.getPose(), historyLook); + + sendPacket(); + } + + /** + * This draws a robot at a specified Pose with a specified + * look. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + * @param style the parameters used to draw the robot with + */ + public static void drawRobot(Pose pose, Style style) { + if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { + return; + } + + panelsField.setStyle(style); + panelsField.moveCursor(pose.getX(), pose.getY()); + panelsField.circle(ROBOT_RADIUS); + + Vector v = pose.getHeadingAsUnitVector(); + v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); + double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; + double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); + + panelsField.setStyle(style); + panelsField.moveCursor(x1, y1); + panelsField.line(x2, y2); + } + + /** + * This draws a robot at a specified Pose. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + */ + public static void drawRobot(Pose pose) { + drawRobot(pose, robotLook); + } + + /** + * This draws a Path with a specified look. + * + * @param path the Path to draw + * @param style the parameters used to draw the Path with + */ + public static void drawPath(Path path, Style style) { + double[][] points = path.getPanelsDrawingPoints(); + + for (int i = 0; i < points[0].length; i++) { + for (int j = 0; j < points.length; j++) { + if (Double.isNaN(points[j][i])) { + points[j][i] = 0; + } + } + } + + panelsField.setStyle(style); + panelsField.moveCursor(points[0][0], points[0][1]); + panelsField.line(points[1][0], points[1][1]); + } + + /** + * This draws all the Paths in a PathChain with a + * specified look. + * + * @param pathChain the PathChain to draw + * @param style the parameters used to draw the PathChain with + */ + public static void drawPath(PathChain pathChain, Style style) { + for (int i = 0; i < pathChain.size(); i++) { + drawPath(pathChain.getPath(i), style); + } + } + + /** + * This draws the pose history of the robot. + * + * @param poseTracker the PoseHistory to get the pose history from + * @param style the parameters used to draw the pose history with + */ + public static void drawPoseHistory(PoseHistory poseTracker, Style style) { + panelsField.setStyle(style); + + int size = poseTracker.getXPositionsArray().length; + for (int i = 0; i < size - 1; i++) { + + panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]); + panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]); + } + } + + /** + * This draws the pose history of the robot. + * + * @param poseTracker the PoseHistory to get the pose history from + */ + public static void drawPoseHistory(PoseHistory poseTracker) { + drawPoseHistory(poseTracker, historyLook); + } + + /** + * This tries to send the current packet to FTControl Panels. + */ + public static void sendPacket() { + panelsField.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Options.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Options.java deleted file mode 100644 index 955a8dbbed9e..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Options.java +++ /dev/null @@ -1,41 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -@Autonomous(group = "Management") -public class Options extends LinearOpMode -{ - private final String[] prompts = { - "Begin?", - "What alliance are you playing for?", - "Where do you plan to start from? (North is where the goals are.)", - "How many drivers?", - "Preferred driving style?" - }; - - private String[] modes = {"", "", "", "", "", ""}; - private final boolean[] inputs = { - gamepad1.a, gamepad1.b, gamepad1.x, gamepad1.y, - gamepad1.left_bumper, gamepad1.right_bumper - }; - - @Override - public void runOpMode() throws InterruptedException - { - String currentPrompt = prompts[0]; - - telemetry.addData("Prompt", currentPrompt); - telemetry.addData(" A", modes[0]); - telemetry.addData(" B", modes[1]); - telemetry.addData(" X", modes[2]); - telemetry.addData(" Y", modes[3]); - telemetry.addData("LB", modes[4]); - telemetry.addData("RB", modes[5]); - - for(String prompt : prompts) - { - System.out.println(prompt); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java index 0f2a4d58048a..374e51cf10b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java @@ -7,17 +7,17 @@ import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @TeleOp(group = "Testing") public class Shooter extends LinearOpMode { - private final int CPR = Constants.CPR; + private final int CPR = TeamConstants.CPR; + private final double LL_MOUNT_ANGLE = TeamConstants.LL_MOUNT_ANGLE; + private final double LL_LENS_HEIGHT_INCHES = TeamConstants.LL_LENS_HEIGHT_INCHES; + private final double GOAL_HEIGHT_INCHES = TeamConstants.GOAL_HEIGHT_INCHES; private final static FtcDashboard DASH = FtcDashboard.getInstance(); @@ -26,9 +26,6 @@ public void runOpMode() { // CONSTANTS: int INCREMENT = 25; - double LL_MOUNT_ANGLE = 30; - double LL_LENS_HEIGHT_INCHES = 14.5; - double GOAL_HEIGHT_INCHES = 38.75; // Telemetry telemetryM = new MultipleTelemetry( @@ -36,9 +33,9 @@ public void runOpMode() ); telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); - DcMotorEx[] scoring = Constants.getScoringMotors(hardwareMap); + DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); - Limelight3A limelight = Constants.getLimelight(hardwareMap); + Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); limelight.start(); @@ -81,13 +78,15 @@ public void runOpMode() if(intake) { scoring[1].setPower(0.9); + scoring[2].setPower(0.9); } else if(intakeReverse) { scoring[1].setPower(-0.9); + scoring[2].setPower(-0.9); } else { scoring[1].setPower(0); - + scoring[2].setPower(0); } scoring[0].setVelocity((shootingInputRPM / 60) * CPR); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java similarity index 67% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java index baa0bc2355d5..e9d487cd02d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java @@ -10,26 +10,26 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; -import com.pedropathing.follower.Follower; -import com.pedropathing.follower.FollowerConstants; -import com.pedropathing.ftc.FollowerBuilder; -import com.pedropathing.paths.PathConstraints; - @Config -public class Constants +public class TeamConstants { // Counts-Per-Revolution of a REV HD Hex Motor. public static int CPR = 28; - public static double LOCK_ON_DENOMINATOR = 27.25; + public static double LOCK_ON_DENOMINATOR = 35.25; + public static double LOCK_ON_OFFSET = 0.0; + + public static double LL_MOUNT_ANGLE = 30; + public static double LL_LENS_HEIGHT_INCHES = 14.5; + public static double GOAL_HEIGHT_INCHES = 38.75; public static DcMotorEx[] getDriveMotors(HardwareMap hardwareMap) { // --- Get Motor Objects --- DcMotorEx[] motors = { - hardwareMap.get(DcMotorEx.class, "frontLeftMotor"), - hardwareMap.get(DcMotorEx.class, "backLeftMotor"), - hardwareMap.get(DcMotorEx.class, "backRightMotor"), - hardwareMap.get(DcMotorEx.class, "frontRightMotor") + hardwareMap.get(DcMotorEx.class, "fl"), + hardwareMap.get(DcMotorEx.class, "rl"), + hardwareMap.get(DcMotorEx.class, "rr"), + hardwareMap.get(DcMotorEx.class, "fr") }; @@ -59,20 +59,22 @@ public static DcMotorEx[] getScoringMotors(HardwareMap hardwareMap) { // --- Get Motor Objects --- DcMotorEx[] motors = { - hardwareMap.get(DcMotorEx.class, "bigHooperMotor"), - hardwareMap.get(DcMotorEx.class, "smallHooperMotor") + hardwareMap.get(DcMotorEx.class, "hooper"), + hardwareMap.get(DcMotorEx.class, "intake1"), + hardwareMap.get(DcMotorEx.class, "intake2") }; // --- Motor Placement --- // "F": Front; "B": Back // + --F-- + // | 1 | - // | 0 | + // | 20 | // + --B-- + // --- Set Motor Directions --- motors[0].setDirection(DcMotorSimple.Direction.REVERSE); - motors[1].setDirection(DcMotorSimple.Direction.FORWARD); + motors[1].setDirection(DcMotorSimple.Direction.REVERSE); + motors[2].setDirection(DcMotorSimple.Direction.REVERSE); return motors; } @@ -102,16 +104,5 @@ public static Limelight3A getLimelight(HardwareMap hardwareMap) return limelight3a; } - // --- PedroPathing Constants --- - public static FollowerConstants followerConstants = new FollowerConstants(); - public static PathConstraints pathConstraints = new PathConstraints( - 0.99, 100, 1, 1 - ); - - public static Follower createFollower(HardwareMap hardwareMap) { - return new FollowerBuilder(followerConstants, hardwareMap) - .pathConstraints(pathConstraints) - .build(); - } } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 778dc043f31e..70c240ea555f 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -2,6 +2,7 @@ repositories { mavenCentral() google() // Needed for androidx maven { url = "https://maven.brott.dev/"} + maven {url = "https://mymaven.bylazar.com/releases"} } dependencies { @@ -15,6 +16,9 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.pedropathing:ftc:2.0.4' - implementation 'com.pedropathing:telemetry:1.0.0' implementation 'com.acmerobotics.dashboard:dashboard:0.5.1' + + implementation 'com.pedropathing:ftc:2.0.5' + implementation 'com.pedropathing:telemetry:1.0.0' + implementation 'com.bylazar:fullpanels:1.0.12' } \ No newline at end of file