From 8bc7c69cb3017db312cb20547c5d8d698a387fb4 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sat, 25 Nov 2023 12:01:09 -0600 Subject: [PATCH] TeleOp heading lock and added acme robotics library --- .../CenterStage/Common/PrototypeRobot.java | 31 +- .../States/PrototypeRobotDrivetrainState.java | 393 +++++++++--------- .../TeleOp/States/headingLockTeleOp.java | 99 +++++ build.dependencies.gradle | 5 + 4 files changed, 338 insertions(+), 190 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java index 3d7ca10..9ccf95e 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -8,6 +8,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.Library.Robot; @@ -15,8 +16,19 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; + public class PrototypeRobot extends Robot { +// public double integralSum = 0; +// double Kp = 0; +// double Ki = 0; +// double Kd = 0; +// public double lastError = 0; + public double PIDrx; + public double targetHeading; + public boolean headingLock = false; + public double backDropLock = Math.toRadians(90); + ElapsedTime timer = new ElapsedTime(); public int armPosition = 0; public boolean stateFinished; public long startOfSequencerTime; @@ -80,6 +92,7 @@ public class PrototypeRobot extends Robot { private boolean rbsVar2; public float depositorPos; public float collectorPos; + public double rx; @@ -181,7 +194,12 @@ public class PrototypeRobot extends Robot { double y = -engine.gamepad1.left_stick_y; double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing - double rx = engine.gamepad1.right_stick_x; + + if (headingLock){ + ; + } else if (headingLock == false){ + rx = engine.gamepad1.right_stick_x; + } double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); // angle math to make things field oriented @@ -251,6 +269,17 @@ public class PrototypeRobot extends Robot { } +// public double PIDControl(double reference, double current){ +// double error = reference - current; +// integralSum += error * timer.seconds(); +// double derivative = (error - lastError) / timer.seconds(); +// +// timer.reset(); +// +// double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); +// return output; +// } + public void CollectorToggle(){ boolean lbs2 = engine.gamepad2.left_stick_button; if (lbs2 && !lbsVar2) { diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java index 8dae6a0..e480496 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java @@ -1,6 +1,7 @@ package org.timecrafters.CenterStage.TeleOp.States; import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.CenterStage.Common.PrototypeRobot; @@ -13,15 +14,18 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { private int minExtension = 0; private PIDController HeadingPidController; private double targetHeading; - private final double p = 0, i = 0, d = 0; - + public double integralSum = 0; + double Kp = 0; + double Ki = 0; + double Kd = 0; + private double lastError = 0; + ElapsedTime timer = new ElapsedTime(); private long lastCheckedTime; public PrototypeRobotDrivetrainState(PrototypeRobot robot) { this.robot = robot; - HeadingPidController = new PIDController(p, i, d); } @@ -60,16 +64,22 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { } private void HeadingLock(){ - HeadingPidController.setPID(p, i, d); double currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - double pid = HeadingPidController.calculate(currentHeading, targetHeading); - double r = pid; } + public double HeadingPIDControl(double reference, double current){ + double error = angleWrap(reference - current); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); + return output; + } @Override public void init() { - HeadingPidController = new PIDController(p, i, d); } @Override @@ -99,192 +109,197 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { // arm sequencer // robot.ArmSequences(); - switch (robot.armPosition) { - case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos - switch (robot.oldArmPosition) { - case 0: - // transfer - robot.oldArmPosition = 0; - break; - case 1: - // driving - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); - robot.oldArmPosition = 0; - } - break; - case 2: - // collect - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { - robot.oldArmPosition = 0; - } - } - break; - case 3: - // deposit - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); // drive the shoulder to the out position - robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met - robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) { - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { // wait to move till time is met - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) { - robot.oldArmPosition = 0; - } - } - } - } - break; - } - break; - case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos - switch (robot.oldArmPosition) { - case 0: - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { - robot.oldArmPosition = 1; - } - } - break; - case 1: - // drive pos - robot.oldArmPosition = 1; +// switch (robot.armPosition) { +// case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos +// switch (robot.oldArmPosition) { +// case 0: +// // transfer +// robot.oldArmPosition = 0; +// break; +// case 1: +// // driving +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); +// robot.oldArmPosition = 0; +// } +// break; +// case 2: +// // collect +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { +// robot.oldArmPosition = 0; +// } +// } +// break; +// case 3: +// // deposit +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); // drive the shoulder to the out position +// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met +// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) { +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { // wait to move till time is met +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) { +// robot.oldArmPosition = 0; +// } +// } +// } +// } +// break; +// } +// break; +// case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos +// switch (robot.oldArmPosition) { +// case 0: +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { +// robot.oldArmPosition = 1; +// } +// } +// break; +// case 1: +// // drive pos +// robot.oldArmPosition = 1; +// +// break; +// case 2: +// // collect +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 900) { // wait to move till time is met +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { +// robot.oldArmPosition = 1; +// } +// } +// break; +// case 3: +// // deposit +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT - 0.1); // drive the shoulder to the out position +// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met +// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) { +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); +// robot.oldArmPosition = 1; +// } +// } +// break; +// } +// break; +// +// case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos +// switch (robot.oldArmPosition) { +// case 0: +// // transfer +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) { +// robot.oldArmPosition = 2; +// } +// } +// break; +// case 1: +// // driving +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { +// robot.oldArmPosition = 2; +// } +// } +// break; +// case 2: +// // collect +// break; +// case 3: +// // deposit +// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met +// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 700) { +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1000) { +// robot.oldArmPosition = 2; +// } +// } +// } +// } +// } +// } +// break; +// case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos +// switch (robot.oldArmPosition) { +// case 0: +// // transfer +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) { +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){ +// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){ +// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){ +// robot.oldArmPosition = 3; +// } +// } +// } +// } +// } +// break; +// case 1: +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 760) { // wait to move till time is met +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) { +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){ +// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){ +// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){ +// robot.oldArmPosition = 3; +// } +// } +// } +// } +// } +// break; +// case 2: +// // collect +// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met +// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); +// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { +// robot.oldArmPosition = 3; +// } +// } +// break; +// case 3: +// // deposit +// break; +// } +// break; - break; - case 2: - // collect - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 900) { // wait to move till time is met - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { - robot.oldArmPosition = 1; - } - } - break; - case 3: - // deposit - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT - 0.1); // drive the shoulder to the out position - robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met - robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) { - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); - robot.oldArmPosition = 1; - } - } - break; - } - break; - - case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos - switch (robot.oldArmPosition) { - case 0: - // transfer - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) { - robot.oldArmPosition = 2; - } - } - break; - case 1: - // driving - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { - robot.oldArmPosition = 2; - } - } - break; - case 2: - // collect - break; - case 3: - // deposit - robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met - robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 700) { - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1000) { - robot.oldArmPosition = 2; - } - } - } - } - } - } - break; - case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos - switch (robot.oldArmPosition) { - case 0: - // transfer - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) { - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){ - robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){ - robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){ - robot.oldArmPosition = 3; - } - } - } - } - } - break; - case 1: - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 760) { // wait to move till time is met - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) { - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){ - robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){ - robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){ - robot.oldArmPosition = 3; - } - } - } - } - } - break; - case 2: - // collect - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met - robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { - robot.oldArmPosition = 3; - } - } - break; - case 3: - // deposit - break; - } - break; - - } +// } // drivetrain robot.driveTrainTeleOp(); + + if (engine.gamepad1.b){ + targetHeading = robot.backDropLock; + + } // lift SliderTeleOp(); // collector depositor diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java new file mode 100644 index 0000000..3345675 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java @@ -0,0 +1,99 @@ +package org.timecrafters.CenterStage.TeleOp.States; + +import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.timecrafters.CenterStage.Common.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmState; + + +@Config +public class headingLockTeleOp extends CyberarmState { + private PrototypeRobot robot; + private PIDController HeadingPidController; + public double integralSum = 0; + public static double Kp = 0; + public static double Ki = 0; + public static double Kd = 0; + private double lastError = 0; + ElapsedTime timer = new ElapsedTime(); + + + + public headingLockTeleOp(PrototypeRobot robot) { + this.robot = robot; + + } + + public double angleWrap(double radians) { + while (radians > Math.PI) { + radians -= 2 * Math.PI; + } + while (radians < -Math.PI){ + radians += 2 * Math.PI; + } + return radians; + } + + public double headingPIDControl(double reference, double current){ + double error = angleWrap(reference - current); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); + return output; + } + + @Override + public void init() { + robot.headingLock = false; + } + + @Override + public void exec() { + if (robot.headingLock) { + double currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double error = angleWrap(robot.targetHeading - currentHeading); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); + robot.PIDrx = output; + } + + // drivetrain + robot.driveTrainTeleOp(); + + + + + if (engine.gamepad1.right_stick_button) { + robot.imu.resetYaw(); + } + + if (engine.gamepad1.b){ + robot.headingLock = true; + robot.targetHeading = robot.backDropLock; + } + + if (engine.gamepad1.right_stick_x != 0){ + robot.headingLock = false; + } + + } + + @Override + public void telemetry () { + engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("rx", robot.rx); + engine.telemetry.addData("PIDrx", robot.PIDrx); + } + } + diff --git a/build.dependencies.gradle b/build.dependencies.gradle index c6f1ebd..f244976 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,6 +1,10 @@ repositories { mavenCentral() google() // Needed for androidx + flatDir { + dirs rootProject.file('libs') + } + maven { url = 'https://maven.brott.dev/' } } dependencies { @@ -15,6 +19,7 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:9.0.1' implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0' implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3' + implementation 'com.acmerobotics.dashboard:dashboard:0.4.8' runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0' implementation 'androidx.appcompat:appcompat:1.2.0' }