From b6f8883110cc9bb5e05c93fe8871c813412058ef Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Thu, 7 Dec 2023 01:25:53 -0600 Subject: [PATCH] Odometry was tuned, and then the only issue i had to fix was how to reset the encoders on the "motors" (which are just used for the odometers to shadow) and everything I have tried to use to stop and reset the encoders makes them unresponsive and not wanting to drive. but the bateries are completely drained on the robot and my battery is also drained making me unresponsive. good night (still gotta push CAD) --- .../DriveToCoordinatesState.java | 25 ++- .../Engines/OdometryTestEngine.java | 9 + .../Common/CompetitionRobotV1.java | 164 +++++++++++++----- .../Engines/CompetitionRobotV1Engine.java | 22 +++ .../TeleOp/States/CompetitionTeleOpState.java | 88 ++++++---- 5 files changed, 219 insertions(+), 89 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/CompetitionRobotV1Engine.java 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); } }