From 0217495975f2cc7b4f4822b492846f09c24457f3 Mon Sep 17 00:00:00 2001 From: Sodi Date: Sat, 4 Feb 2023 12:54:14 -0600 Subject: [PATCH] Debugging arm motor --- .../States/DriverStateWithOdometer.java | 13 +- .../States/JunctionAllignmentAngleState.java | 3 +- .../Autonomous/States/RotationState.java | 3 +- .../TeleOp/states/PhoenixBot1.java | 21 +- .../TeleOp/states/PhoenixTeleOPState.java | 33 +- .../TeleOp/states/TeleOPArmDriver.java | 281 +++++++++--------- .../TeleOp/states/TeleOPTankDriver.java | 107 ++++--- 7 files changed, 242 insertions(+), 219 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java index 7e0f482..c7a94f5 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java @@ -5,6 +5,7 @@ import android.util.Log; import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.TeleOp.states.PhoenixBot1; public class DriverStateWithOdometer extends CyberarmState { @@ -189,9 +190,9 @@ public class DriverStateWithOdometer extends CyberarmState { engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower()); engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower()); engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition()); - engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle); - engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle); - engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle); + engine.telemetry.addData("imu yaw", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("imu pitch", robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)); + engine.telemetry.addData("imu roll", robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)); engine.telemetry.addData("Target Achieved", targetAchieved); @@ -204,9 +205,9 @@ public class DriverStateWithOdometer extends CyberarmState { engine.telemetry.addData("RampUpDistance", RampUpDistance); engine.telemetry.addData("RampDownDistance", RampDownDistance); - Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle); - Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle); - Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle); + Log.i("TELEMETRY", "imu yaw:: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + Log.i("TELEMETRY", "imu pitch:: " + robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)); + Log.i("TELEMETRY", "imu roll:: " + robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java index c48c346..21a7af8 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java @@ -3,6 +3,7 @@ package org.timecrafters.Autonomous.States; import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.TeleOp.states.PhoenixBot1; @@ -88,7 +89,7 @@ public class JunctionAllignmentAngleState extends CyberarmState { @Override public void exec() { - currentAngle = robot.imu.getAngularOrientation().firstAngle; + currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); if (stateDisabled){ diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java index 39a15da..12749d4 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -1,6 +1,7 @@ package org.timecrafters.Autonomous.States; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.TeleOp.states.PhoenixBot1; public class RotationState extends CyberarmState { @@ -40,7 +41,7 @@ public class RotationState extends CyberarmState { return; } // end of if - RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RobotRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){ drivePowerVariable = 0.4 * drivePower; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java index 5a6866d..d9e1a1d 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -3,9 +3,11 @@ package org.timecrafters.TeleOp.states; import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor; import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; import org.cyberarm.engine.V2.CyberarmEngine; @@ -55,7 +57,7 @@ public class PhoenixBot1 { public CRServo collectorLeft, collectorRight; - public BNO055IMU imu; + public IMU imu; public TimeCraftersConfiguration configuration; @@ -121,17 +123,20 @@ public class PhoenixBot1 { leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance"); rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right Pole Distance"); - BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD, + RevHubOrientationOnRobot.UsbFacingDirection.UP + )); - parameters.mode = BNO055IMU.SensorMode.IMU; - parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; - parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; - parameters.loggingEnabled = false; +// parameters.mode = BNO055IMU.SensorMode.IMU; +// parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; +// parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; +// parameters.loggingEnabled = false; - this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu"); + this.imu = engine.hardwareMap.get(IMU.class, "imu"); imu.initialize(parameters); - imu.startAccelerationIntegration(new Position(), new Velocity(), 10); +// imu.startAccelerationIntegration(new Position(), new Velocity(), 10); configuration = new TimeCraftersConfiguration("Phoenix"); // AdafruitEncoder = engine.hardwareMap.AdafruitI2cColorSensor.get("adafruit"); diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index 20ceaed..8846446 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -10,6 +10,7 @@ import com.qualcomm.robotcore.hardware.Servo; import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.GamepadChecker; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class PhoenixTeleOPState extends CyberarmState { @@ -52,7 +53,7 @@ public class PhoenixTeleOPState extends CyberarmState { public void telemetry() { engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition()); engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition()); - engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle); + engine.telemetry.addData("IMU", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); engine.telemetry.addData("Drive Power", drivePower); engine.telemetry.addData("Delta Rotation", DeltaRotation); engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM)); @@ -131,7 +132,7 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad1.dpad_right) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); RotationTarget = 90; CalculateDeltaRotation(); if (RobotRotation > -90 && RobotRotation < 89) {//CCW @@ -158,7 +159,7 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad1.dpad_left) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); RotationTarget = -90; CalculateDeltaRotation(); if (RobotRotation > -89 && RobotRotation <= 90) {//CW @@ -185,7 +186,7 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad1.y) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); RotationTarget = 180; CalculateDeltaRotation(); if (RobotRotation < 0 && RobotRotation > -179) { @@ -210,7 +211,7 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad1.a && !engine.gamepad1.start) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); RotationTarget = 0; CalculateDeltaRotation(); if (RobotRotation < -1) { @@ -237,7 +238,7 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad1.x) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); RotationTarget = -45; CalculateDeltaRotation(); if (RobotRotation < -46 || RobotRotation > 135) {//CCW @@ -264,7 +265,7 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad1.b) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); RotationTarget = 45; CalculateDeltaRotation(); if (RobotRotation > -135 && RobotRotation < 44) {//CCW @@ -470,24 +471,10 @@ public class PhoenixTeleOPState extends CyberarmState { @Override public void buttonUp(Gamepad gamepad, String button) { if (engine.gamepad1 == gamepad && button.equals("start")) { - BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); - parameters.mode = BNO055IMU.SensorMode.IMU; - parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; - parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; - parameters.loggingEnabled = false; - - robot.imu.initialize(parameters); + robot.imu.resetYaw(); } } -// public double downSensor() { -// double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5; -// Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5; -// return Distance; + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java index f46cca8..94a9531 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java @@ -9,10 +9,10 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class TeleOPArmDriver extends CyberarmState { private final PhoenixBot1 robot; - private long lastStepTime = 0; + private long lastStepTime = 0, Spirit; private int CyclingArmUpAndDown = 0; private GamepadChecker gamepad2Checker; - private int Opportunity, Endeavour, Peanut; + private int Opportunity, JunctionHeight, Ingenuity; private double drivePower, armPower; private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05; private double servoCollectLow = 0.40; //Low servos, A button @@ -42,7 +42,7 @@ public class TeleOPArmDriver extends CyberarmState { engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition()); engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition()); engine.telemetry.addData("Arm Motor TargetPosition", robot.ArmMotor.getTargetPosition()); - engine.telemetry.addData("Target (Endeavour)", Endeavour); + engine.telemetry.addData("Target Junction Height", JunctionHeight); } @Override @@ -52,9 +52,9 @@ public class TeleOPArmDriver extends CyberarmState { robot.LowRiserLeft.setPosition(0.45); robot.LowRiserRight.setPosition(0.45); robot.ArmMotor.setTargetPosition(0); - robot.ArmMotor.setPower(0.25); + robot.ArmMotor.setPower(0.5); Opportunity = 0; - Endeavour = 0; + JunctionHeight = 0; } @@ -62,167 +62,176 @@ public class TeleOPArmDriver extends CyberarmState { @Override public void exec() { - if (Math.abs(robot.ArmMotor.getCurrentPosition() - robot.ArmMotor.getTargetPosition()) > 46) { - ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25; - armPower = ArmNeededPower; + Spirit = System.currentTimeMillis(); + double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - robot.ArmMotor.getTargetPosition()) / 400.0 + 0.1; + +// ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25; + armPower = ratio; robot.ArmMotor.setPower(armPower); + + if (engine.gamepad2.y) { + JunctionHeight = 4; + } + if (engine.gamepad2.b) { + JunctionHeight = 3; + } + if (engine.gamepad2.x) { + JunctionHeight = 2; + } + if (engine.gamepad2.a) { + JunctionHeight = 1; } - if (engine.gamepad2.y) { - Endeavour = 4; + if (JunctionHeight == 4 && System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setTargetPosition(armMotorHigh); + if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ { + robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); } - if (engine.gamepad2.b) { - Endeavour = 3; + if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ { + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } - if (engine.gamepad2.x) { - Endeavour = 2; - } - if (engine.gamepad2.a) { - Endeavour = 1; + if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh && + robot.LowRiserLeft.getPosition() >= servoHighLow) { + JunctionHeight = 0; } + } - if (Endeavour == 4 && System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.ArmMotor.setTargetPosition(armMotorHigh); - if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ { - robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); + if (JunctionHeight == 3) { + robot.ArmMotor.setTargetPosition(armMotorMed); + if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } - if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ { + } + if (robot.LowRiserLeft.getPosition() < servoMedLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } - if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh && - robot.LowRiserLeft.getPosition() >= servoHighLow) { - Endeavour = 0; + } + if (robot.LowRiserLeft.getPosition() <= servoMedLow && + robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); } } - - if (Endeavour == 3) { - robot.ArmMotor.setTargetPosition(armMotorMed); - if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } - } - if (robot.LowRiserLeft.getPosition() < servoMedLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } - } - if (robot.LowRiserLeft.getPosition() <= servoMedLow && - robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); - } - } - if (robot.LowRiserLeft.getPosition() < servoMedLow && - robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); - } - } - if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 && - robot.LowRiserLeft.getPosition() <= servoMedLow && - robot.ArmMotor.getCurrentPosition() >= armMotorMed) { - Endeavour = 0; + if (robot.LowRiserLeft.getPosition() < servoMedLow && + robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); } } + if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 && + robot.LowRiserLeft.getPosition() <= servoMedLow && + robot.ArmMotor.getCurrentPosition() >= armMotorMed) { + JunctionHeight = 0; + } + } - if (Endeavour == 2) { - robot.ArmMotor.setTargetPosition(armMotorLow); - if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } - } - if (robot.LowRiserLeft.getPosition() < servoLowLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } - } - if (robot.LowRiserLeft.getPosition() <= servoLowLow && - robot.LowRiserLeft.getPosition() > servoLowLow - 5 && - robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); - } - } - if (robot.LowRiserLeft.getPosition() <= servoLowLow && - robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); - } - } - if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && - robot.LowRiserLeft.getPosition() <= servoLowLow && - robot.ArmMotor.getCurrentPosition() >= armMotorLow) { - Endeavour = 0; + if (JunctionHeight == 2) { + robot.ArmMotor.setTargetPosition(armMotorLow); + if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } } - - if (Endeavour == 1) { - robot.ArmMotor.setTargetPosition(armMotorCollect); - if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- low level too high*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } - } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && - robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); - } - } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && - robot.ArmMotor.getCurrentPosition() <= armMotorCollect) { - Endeavour = 0; + if (robot.LowRiserLeft.getPosition() < servoLowLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } } - - if (engine.gamepad2.dpad_left && Peanut != 1) { - Peanut = 1; + if (robot.LowRiserLeft.getPosition() <= servoLowLow && + robot.LowRiserLeft.getPosition() > servoLowLow - 5 && + robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); + } } - - if (engine.gamepad2.dpad_right && Peanut != 2) { - Peanut = 2; + if (robot.LowRiserLeft.getPosition() <= servoLowLow && + robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); + } } - - if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) { - robot.collectorLeft.setPower(0); - robot.collectorRight.setPower(0); - Peanut = 0; + if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && + robot.LowRiserLeft.getPosition() <= servoLowLow && + robot.ArmMotor.getCurrentPosition() >= armMotorLow) { + JunctionHeight = 0; } + } - if (Peanut == 1) { - robot.collectorRight.setPower(1); - robot.collectorLeft.setPower(-1); + if (JunctionHeight == 1) { + robot.ArmMotor.setTargetPosition(armMotorCollect); + if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- low level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && + robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); + } + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && + robot.ArmMotor.getCurrentPosition() <= armMotorCollect) { + JunctionHeight = 0; } + } - if (Peanut == 2) { - robot.collectorLeft.setPower(1); - robot.collectorRight.setPower(-1); - } + if (engine.gamepad2.dpad_left && Ingenuity != 1) { + Ingenuity = 1; + } - if (engine.gamepad2.right_trigger > 0.1) { + if (engine.gamepad2.dpad_right && Ingenuity != 2) { + Ingenuity = 2; + } + if (engine.gamepad2.dpad_left && Ingenuity == 1 || engine.gamepad2.dpad_right && Ingenuity == 2) { + robot.collectorLeft.setPower(0); + robot.collectorRight.setPower(0); + Ingenuity = 0; + } + + if (Ingenuity == 1) { + robot.collectorRight.setPower(1); + robot.collectorLeft.setPower(-1); + } + + if (Ingenuity == 2) { + robot.collectorLeft.setPower(1); + robot.collectorRight.setPower(-1); + } + + if (engine.gamepad2.right_trigger > 0.1) { + JunctionHeight = 0; + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + 50); + robot.ArmMotor.setPower(armPower); } else if (engine.gamepad2.left_trigger > 0.1) { - - } else if (engine.gamepad2.right_trigger == 0 && engine.gamepad2.left_trigger == 0 && Endeavour == 0) { - + JunctionHeight = 0; + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - 50); + robot.ArmMotor.setPower(armPower); + } } } +} } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java index 3fbf337..bc0243f 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.Gamepad; import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.GamepadChecker; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; public class TeleOPTankDriver extends CyberarmState { @@ -14,7 +15,7 @@ public class TeleOPTankDriver extends CyberarmState { private double RobotRotation; private double RotationTarget, DeltaRotation; private double MinimalPower = 0.2; - private int DeltaOdometerR, Endeavour, Spirit; + private int DeltaOdometerR, Spirit; private GamepadChecker gamepad1Checker; public TeleOPTankDriver(PhoenixBot1 robot) { @@ -24,19 +25,20 @@ public class TeleOPTankDriver extends CyberarmState { @Override public void telemetry() { engine.telemetry.addLine("Tank Driver"); - engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle); + engine.telemetry.addData("IMU", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); engine.telemetry.addData("Drive Power", drivePower); engine.telemetry.addData("Delta Rotation", DeltaRotation); } @Override public void init() { + } @Override public void exec() { - double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative + double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative //ORLY? double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing double rx = engine.gamepad1.right_stick_x; double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); @@ -45,44 +47,61 @@ public class TeleOPTankDriver extends CyberarmState { double frontRightPower = (y - x - rx) / denominator; double backRightPower = (y + x - rx) / denominator; - if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) { - drivePower = engine.gamepad1.left_stick_y; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } - if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1) { - drivePower = engine.gamepad1.left_stick_x; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); - } + double heading = -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double rotX = x * Math.cos(heading) - y * Math.sin(heading); + double rotY = x * Math.sin(heading) + y * Math.cos(heading); - if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) > 0.1) { - robot.frontLeftDrive.setPower(frontLeftPower * drivePower); - robot.backLeftDrive.setPower(backLeftPower * drivePower); - robot.frontRightDrive.setPower(frontRightPower * drivePower); - robot.backRightDrive.setPower(backRightPower * drivePower); - } + frontLeftPower = (rotY + rotX + rx) / denominator; + backLeftPower = (rotY - rotX + rx) / denominator; + frontRightPower = (rotY - rotX - rx) / denominator; + backRightPower = (rotY + rotX - rx) / denominator; - if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { - drivePower = engine.gamepad1.right_stick_x; - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); - } + robot.backLeftDrive.setPower(backLeftPower * drivePower); + robot.backRightDrive.setPower(backRightPower * drivePower); + robot.frontLeftDrive.setPower(frontLeftPower * drivePower); + robot.frontRightDrive.setPower(frontRightPower * drivePower); - if (engine.gamepad1.left_stick_y == 0 && engine.gamepad1.left_stick_x == 0 && engine.gamepad1.right_stick_x == 0) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } + + +// if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) { +// drivePower = engine.gamepad1.left_stick_y; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } +// +// if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1) { +// drivePower = engine.gamepad1.left_stick_x; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(-drivePower); +// robot.frontLeftDrive.setPower(-drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } +// +// if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) > 0.1) { +// robot.frontLeftDrive.setPower(frontLeftPower * drivePower); +// robot.backLeftDrive.setPower(backLeftPower * drivePower); +// robot.frontRightDrive.setPower(frontRightPower * drivePower); +// robot.backRightDrive.setPower(backRightPower * drivePower); +// } +// +// if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { +// drivePower = engine.gamepad1.right_stick_x; +// robot.backLeftDrive.setPower(-drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(-drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } +// +// if (engine.gamepad1.left_stick_y == 0 && engine.gamepad1.left_stick_x == 0 && engine.gamepad1.right_stick_x == 0) { +// drivePower = 0; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } @@ -111,14 +130,14 @@ public class TeleOPTankDriver extends CyberarmState { @Override public void buttonUp(Gamepad gamepad, String button) { if (engine.gamepad1 == gamepad && button.equals("start")) { - BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); - parameters.mode = BNO055IMU.SensorMode.IMU; - parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; - parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; - parameters.loggingEnabled = false; - - robot.imu.initialize(parameters); + robot.imu.resetYaw(); + } + if (engine.gamepad1 == gamepad && button.equals("y")) { + drivePower = 1; + } + if (engine.gamepad1 == gamepad && button.equals("a")) { + drivePower = 0.5; } }