TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

TeleOp heading lock and added acme robotics library

Browse Source
This commit is contained in:
SpencerPiha
2023-11-25 12:01:09 -06:00
parent 65c55b8f47
commit 8bc7c69cb3
4 changed files with 338 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
View File

@@ -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) {

393
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java
View File

@@ -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

99
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java Normal file
View File

@@ -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);
}
}