diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java index da091ba..ffeb038 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java @@ -1,5 +1,6 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionStates; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.controller.PIDController; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -7,13 +8,13 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import org.timecrafters.CenterStage.Common.PrototypeRobot; import dev.cyberarm.engine.V2.CyberarmState; - +@Config public class DriveToCoordinatesState extends CyberarmState { CompetitionRobotV1 robot; - public double xTarget; - public double yTarget; - public double hTarget; + public static double xTarget = 0; + public static double yTarget = 0; + public static double hTarget= 0; public boolean posAchieved = false; public DriveToCoordinatesState(CompetitionRobotV1 robot/*, String groupName, String actionName*/) { @@ -28,15 +29,16 @@ public class DriveToCoordinatesState extends CyberarmState { robot.hTarget = hTarget; robot.yTarget = yTarget; robot.xTarget = xTarget; + robot.DriveToCoordinates(); + robot.OdometryLocalizer(); if (posAchieved){ // setHasFinished(true); } else { - robot.OdometryLocalizer(); if (((robot.positionX > xTarget - 1) && (robot.positionX < xTarget + 1)) && ((robot.positionH > hTarget - 1) && (robot.positionH < hTarget + 1)) && ((robot.positionY > yTarget - 1) && (robot.positionY < yTarget + 1))){ - posAchieved = true; +// posAchieved = true; } } } @@ -47,9 +49,14 @@ public class DriveToCoordinatesState extends CyberarmState { engine.telemetry.addData("x pos", robot.positionX); engine.telemetry.addData("y pos", robot.positionY); engine.telemetry.addData("h pos odo", robot.positionH); - engine.telemetry.addData("aux encoder", robot.odometerA.getCurrentPosition()); - engine.telemetry.addData("left encoder", robot.odometerL.getCurrentPosition()); - engine.telemetry.addData("right encoder", robot.odometerR.getCurrentPosition()); + engine.telemetry.addData("aux encoder", robot.currentAuxPosition); + engine.telemetry.addData("left encoder", robot.currentLeftPosition); + engine.telemetry.addData("right encoder", robot.currentRightPosition); engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("front right power", robot.frontRightPower); + engine.telemetry.addData("front left power", robot.frontLeftPower); + engine.telemetry.addData("back right power", robot.backRightPower); + engine.telemetry.addData("back left power", robot.backLeftPower); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java index 53e28be..17d8462 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java @@ -1,6 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; import org.timecrafters.CenterStage.Common.CompetitionRobotV1; @@ -13,6 +14,7 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class OdometryTestEngine extends CyberarmEngine { CompetitionRobotV1 robot; + @Override public void setup() { this.robot = new CompetitionRobotV1("Autonomous odometry test"); @@ -20,4 +22,11 @@ public class OdometryTestEngine extends CyberarmEngine { addState(new DriveToCoordinatesState(robot/*,"odoTest", "00-1"*/)); } + @Override + public void init() { + + robot.backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java index 588a8d6..7405508 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -1,5 +1,7 @@ package org.timecrafters.CenterStage.Common; +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -7,12 +9,13 @@ 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; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import dev.cyberarm.engine.V2.CyberarmEngine; - +@Config public class CompetitionRobotV1 extends Robot { // --------------------------------------------------------------------------------------------------- engine and state setup variables: private String string; @@ -33,7 +36,7 @@ public class CompetitionRobotV1 extends Robot { public static double Xp = 0, Xi = 0, Xd = 0; public static double Yp = 0, Yi = 0, Yd = 0; private double drivePower = 1; - + public double rx; public double xMultiplier = 1; public double yMultiplier = 1; public double positionX = 0; @@ -49,33 +52,36 @@ public class CompetitionRobotV1 extends Robot { public int oldRightPosition = 0; public int oldLeftPosition = 0; public int oldAuxPosition = 0; - public double rx; - public final static double L = 23.425; // distance between left and right encoder in cm - final static double B = 10; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm - public final static double R = 4.5; // wheel radius in cm + public final static double L = 22.5; // distance between left and right encoder in cm + final static double B = 11.5; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm + public final static double R = 3; // wheel radius in cm final static double N = 8192; // encoder ticks per revolution (REV encoder) // heading math variables for pid with imu - public double integralSum = 0; + public double headingIntegralSum = 0; + public double xIntegralSum = 0; + public double yIntegralSum = 0; public double targetHeading; public final double cm_per_tick = (2 * Math.PI * R) / N; - private double lastError = 0; - ElapsedTime timer = new ElapsedTime(); + private double headingLastError = 0; + private double xLastError = 0; + private double yLastError = 0; + ElapsedTime headingTimer = new ElapsedTime(); + ElapsedTime xTimer = new ElapsedTime(); + ElapsedTime yTimer = new ElapsedTime(); + public double frontLeftPower; + public double backLeftPower; + public double frontRightPower; + public double backRightPower; //-------------------------------------------------------------------------------------------------------------- arm sequence variables: - public int armPosition = 0; - public long startOfSequencerTime; - public int oldArmPosition = 0; - public float DEPOSITOR_SHOULDER_IN; - public float DEPOSITOR_SHOULDER_OUT; - public float DEPOSITOR_ELBOW_IN; - public float DEPOSITOR_ELBOW_OUT; - public float COLLECTOR_SHOULDER_IN; - public float COLLECTOR_SHOULDER_PASSIVE; - public float COLLECTOR_SHOULDER_OUT; - public float COLLECTOR_ELBOW_IN; - public float COLLECTOR_ELBOW_PASSIVE; - public float COLLECTOR_ELBOW_OUT; + public static double shoulderCollect = 0.25; + public static double shoulderDeposit = 0.55; + public static double shoulderPassive = 1; + public static double elbowCollect = 0.7; + public static double elbowDeposit = 0.8; + public static double elbowPassive = 0.28; + private HardwareMap hardwareMap; @@ -87,16 +93,7 @@ public class CompetitionRobotV1 extends Robot { } public void initConstants() { - DEPOSITOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_IN").value(); - DEPOSITOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_OUT").value(); - DEPOSITOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_IN").value(); - DEPOSITOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_OUT").value(); - COLLECTOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_IN").value(); - COLLECTOR_SHOULDER_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_PASSIVE").value(); - COLLECTOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_OUT").value(); - COLLECTOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_IN").value(); - COLLECTOR_ELBOW_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_PASSIVE").value(); - COLLECTOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_OUT").value(); + } @Override @@ -113,22 +110,16 @@ public class CompetitionRobotV1 extends Robot { backRight = engine.hardwareMap.dcMotor.get("backRight"); backLeft = engine.hardwareMap.dcMotor.get("backLeft"); lift = engine.hardwareMap.dcMotor.get("Lift"); - odometerL = engine.hardwareMap.dcMotor.get("leftodo"); - odometerR = engine.hardwareMap.dcMotor.get("rightodo"); - odometerA = engine.hardwareMap.dcMotor.get("auxodo"); frontRight.setDirection(DcMotorSimple.Direction.FORWARD); - backRight.setDirection(DcMotorSimple.Direction.FORWARD); + backRight.setDirection(DcMotorSimple.Direction.REVERSE); backLeft.setDirection(DcMotorSimple.Direction.REVERSE); frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); lift.setDirection(DcMotorSimple.Direction.FORWARD); - odometerR.setDirection(DcMotorSimple.Direction.FORWARD); - odometerL.setDirection(DcMotorSimple.Direction.REVERSE); - odometerA.setDirection(DcMotorSimple.Direction.FORWARD); - odometerR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - odometerL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - odometerA.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -137,6 +128,16 @@ public class CompetitionRobotV1 extends Robot { frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + //IMU + IMU.Parameters parameters = new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + + imu.initialize(parameters); + + imu.resetYaw(); + configuration = new TimeCraftersConfiguration("Blue Crab"); initConstants(); @@ -148,6 +149,9 @@ public class CompetitionRobotV1 extends Robot { leftClaw = hardwareMap.servo.get("leftClaw"); rightClaw = hardwareMap.servo.get("leftClaw"); + shoulder.setPosition(shoulderPassive); + elbow.setPosition(elbowPassive); + } @@ -160,9 +164,9 @@ public class CompetitionRobotV1 extends Robot { oldLeftPosition = currentLeftPosition; oldAuxPosition = currentAuxPosition; - currentRightPosition = -odometerR.getCurrentPosition(); - currentLeftPosition = -odometerL.getCurrentPosition(); - currentAuxPosition = odometerA.getCurrentPosition(); + currentRightPosition = -frontLeft.getCurrentPosition(); + currentLeftPosition = -backRight.getCurrentPosition(); + currentAuxPosition = backLeft.getCurrentPosition(); int dnl1 = currentLeftPosition - oldLeftPosition; int dnr2 = currentRightPosition - oldRightPosition; @@ -180,4 +184,74 @@ public class CompetitionRobotV1 extends Robot { positionH += dtheta; } + 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(current - reference); + headingIntegralSum += error * headingTimer.seconds(); + double derivative = (headingLastError - error) / headingTimer.seconds(); + + headingTimer.reset(); + + double output = (error * Hp) + (derivative * Hd) + (headingIntegralSum * Hi); + return output; + } + + public double XPIDControl ( double reference, double current){ + double error = (current - reference); + xIntegralSum += error * xTimer.seconds(); + double derivative = (error - xLastError) / xTimer.seconds(); + + xTimer.reset(); + + double output = (error * Xp) + (derivative * Xd) + (xIntegralSum * Xi); + return output; + } + + public double YPIDControl ( double reference, double current){ + double error = (current - reference); + yIntegralSum += error * yTimer.seconds(); + double derivative = (error - yLastError) / yTimer.seconds(); + + yTimer.reset(); + + double output = (error * Yp) + (derivative * Yd) + (yIntegralSum * Yi); + return output; + } + + public void DriveToCoordinates () { + + // determine the velocities needed for each direction + // this uses PID to adjust needed Power for robot to move to target + double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double rx = HeadingPIDControl(Math.toRadians(hTarget), heading); + double pidX = XPIDControl(xTarget, positionX); + double pidY = YPIDControl(yTarget, positionY); + + double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1); + + // field oriented math, (rotating the global field to the relative field) + double rotY = pidY * Math.cos(heading) - pidX * Math.sin(heading); + double rotX = pidY * Math.sin(heading) + pidX * Math.cos(heading); + + + // finding approximate power for each wheel. + frontLeftPower = (rotY + rotX + rx) / denominator; + backLeftPower = (rotY - rotX + rx) / denominator; + frontRightPower = (rotY - rotX - rx) / denominator; + backRightPower = (rotY - rotX + rx) / denominator; + + // apply my powers + frontLeft.setPower(frontLeftPower); + backLeft.setPower(backLeftPower); + backRight.setPower(backRightPower); + frontRight.setPower(frontRightPower); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/CompetitionRobotV1Engine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/CompetitionRobotV1Engine.java new file mode 100644 index 0000000..00774fc --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/CompetitionRobotV1Engine.java @@ -0,0 +1,22 @@ +package org.timecrafters.CenterStage.TeleOp.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; +import org.timecrafters.CenterStage.Common.PrototypeRobot; +import org.timecrafters.CenterStage.TeleOp.States.CompetitionTeleOpState; +import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@TeleOp(name = "Competition V1 TeleOp", group = "Competition V1") +public class CompetitionRobotV1Engine extends CyberarmEngine { + private CompetitionRobotV1 robot; + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Hello World"); + this.robot.setup(); + + addState(new CompetitionTeleOpState(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java index 6b53787..bb76a7c 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java @@ -24,19 +24,19 @@ public class CompetitionTeleOpState extends CyberarmState { private double currentHeading; private boolean headingLock = false; - public static double Kp = 0; + public static double Kp = 1; public static double Ki = 0; public static double Kd = 0; private double lastError = 0; ElapsedTime timer = new ElapsedTime(); // ---------------------------------------------------------------------------------------------------- collector / depositor variables: - public static float leftOpen = 0; - public static float leftClose = 0; - public static float rightOpen = 0; - public static float rightClose = 0; - public float collectorPosLeft; - public float collectorPosRight; + public static double leftOpen = 0; + public static double leftClose = 0; + public static double rightOpen = 0.6; + public static double rightClose = 0.25; + public double collectorPosLeft = leftClose; + public double collectorPosRight = rightClose; public boolean lbsVar2; public boolean rbsVar2; public boolean bVar2; @@ -52,13 +52,7 @@ public class CompetitionTeleOpState extends CyberarmState { public boolean depositMode = false; // ---------------------------------------------------------------------------------------------------------------Arm control Variables: - public String armPos = "collect"; - public static float shoulderCollect = 0; - public static float shoulderDeposit = 0; - public static float shoulderPassive = 0; - public static float elbowCollect = 0; - public static float elbowDeposit = 0; - public static float elbowPassive = 0; + public String armPos = "passive"; @@ -105,6 +99,7 @@ public class CompetitionTeleOpState extends CyberarmState { robot.rx = engine.gamepad1.right_stick_x; } + boolean lbs1 = engine.gamepad1.left_stick_button; if (lbs1 && !lbsVar1) { if (drivePower == 1) { @@ -115,8 +110,8 @@ public class CompetitionTeleOpState extends CyberarmState { } lbsVar1 = lbs1; - double y = -engine.gamepad1.left_stick_y; - double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + double y = engine.gamepad1.left_stick_y; + double x = -engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); @@ -126,10 +121,10 @@ public class CompetitionTeleOpState extends CyberarmState { double rotY = x * Math.sin(heading) + y * Math.cos(heading); // joystick math to find the approximate power across each wheel for a movement - double frontLeftPower = (rotY + rotX + rx) / denominator; - double backLeftPower = (rotY - rotX + rx) / denominator; - double frontRightPower = (rotY + rotX - rx) / denominator; - double backRightPower = (rotY - rotX - rx) / denominator; + double frontLeftPower = (rotY + rotX + robot.rx) / denominator; + double backLeftPower = (rotY - rotX + robot.rx) / denominator; + double frontRightPower = (rotY - rotX - robot.rx) / denominator; + double backRightPower = (rotY + rotX - robot.rx) / denominator; // setting each power determined previously from the math above // as well as multiplying it by a drive power that can be changed. @@ -208,25 +203,25 @@ public class CompetitionTeleOpState extends CyberarmState { if (armPos == "collect"){ if (robot.lift.getCurrentPosition() >= 1){ - robot.lift.setPower(-0.3); + robot.lift.setPower(-0.6); } else { robot.lift.setPower(0); - robot.shoulder.setPosition(shoulderCollect); - robot.elbow.setPosition(elbowCollect); + robot.shoulder.setPosition(robot.shoulderCollect); + robot.elbow.setPosition(robot.elbowCollect); } } if (armPos == "passive"){ if (robot.lift.getCurrentPosition() >= 1){ - robot.lift.setPower(-0.3); + robot.lift.setPower(-0.6); } else { robot.lift.setPower(0); - robot.shoulder.setPosition(shoulderPassive); - robot.elbow.setPosition(elbowPassive); + robot.shoulder.setPosition(robot.shoulderPassive); + robot.elbow.setPosition(robot.elbowPassive); } } if (armPos == "deposit"){ - robot.shoulder.setPosition(shoulderDeposit); - robot.elbow.setPosition(elbowDeposit); + robot.shoulder.setPosition(robot.shoulderDeposit); + robot.elbow.setPosition(robot.elbowDeposit); } } @@ -237,9 +232,6 @@ public class CompetitionTeleOpState extends CyberarmState { @Override public void exec() { // ---------------------------------------------------------------------------------------------------------- Game Pad 1, drivetrain - - currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); - if (engine.gamepad1.right_stick_button) { robot.imu.resetYaw(); } @@ -251,6 +243,8 @@ public class CompetitionTeleOpState extends CyberarmState { } + currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + // drivetrain DriveTrainTeleOp(); @@ -274,7 +268,30 @@ public class CompetitionTeleOpState extends CyberarmState { // ------------------------------------------------------------------------------------------------------------------- Lift Control: SliderTeleOp(); - ClawControlTeleOp(); + + robot.leftClaw.setPosition(collectorPosLeft); + robot.rightClaw.setPosition(collectorPosRight); + + boolean lbs2 = engine.gamepad2.left_stick_button; + if (lbs2 && !lbsVar2) { + if (collectorPosLeft == leftClose) { + collectorPosLeft = leftOpen; + } else { + collectorPosLeft = leftClose; + } + } + lbsVar2 = lbs2; + + boolean rbs2 = engine.gamepad2.right_stick_button; + if (rbs2 && !rbsVar2) { + if (collectorPosRight == rightClose) { + collectorPosRight = rightOpen; + } else { + collectorPosRight = rightClose; + } + } + rbsVar2 = rbs2; + ArmPosControl(); } @@ -282,14 +299,15 @@ public class CompetitionTeleOpState extends CyberarmState { @Override public void telemetry () { engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition()); - engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); - engine.telemetry.addData("arm Pos", robot.armPosition); - engine.telemetry.addData("old arm pos", robot.oldArmPosition); + engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)); + engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)); engine.telemetry.addData("pid power", power); engine.telemetry.addData("heading Lock?", headingLock); engine.telemetry.addData("Kp", Kp); engine.telemetry.addData("Ki", Ki); engine.telemetry.addData("Kd", Kd); + engine.telemetry.addData("arm pos", armPos); } }