From 91a4b432656ee843299556b561f3379ff7b1fdc8 Mon Sep 17 00:00:00 2001 From: Sodi Date: Sun, 5 Feb 2023 20:19:19 -0600 Subject: [PATCH] Debugging arm motor, resolved --- .../TeleOp/states/TeleOPArmDriver.java | 352 ++++++++---------- 1 file changed, 145 insertions(+), 207 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java index c7026f2..fa877cb 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java @@ -15,21 +15,21 @@ public class TeleOPArmDriver extends CyberarmState { 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 -// private double servoCollectHigh = 0.40; //High servos, A button - private double servoLowLow = 0.5; //Low servos, X button -// private double servoLowHigh = 0.75; //High servos, X button - private double servoMedLow = 0.5; //Low servos, B button -// private double servoMedHigh = 0.9; //High servos, B button - private double servoHighLow = 0.8; //Low servos, Y button -// private double servoHighHigh = 0.9; //High servos, Y button + private double servoCollect = 0.45; //Low servos, A button + // private double servoCollectHigh = 0.40; //High servos, A button + private double servoLow = 0.45; //Low servos, X button + // private double servoLowHigh = 0.75; //High servos, X button + private double servoMed = 0.45; //Low servos, B button + // private double servoMedHigh = 0.9; //High servos, B button + private double servoHigh = 0.8; //Low servos, Y button + // private double servoHighHigh = 0.9; //High servos, Y button private double ArmNeededPower; private int armMotorCollect = -100; - private int armMotorLow = 240; - private int armMotorMed = 380; - private int armMotorHigh = 463; + private int armMotorLow = 280; + private int armMotorMed = 430; + private int armMotorHigh = 433; private int ArmMotorStepSize = 2; - private int TargetPosition = 0; + private int TargetPosition = 0, OverrideTarget = -64; public TeleOPArmDriver(PhoenixBot1 robot) { this.robot = robot; @@ -56,13 +56,11 @@ public class TeleOPArmDriver extends CyberarmState { robot.ArmMotor.setPower(0.5); robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.ArmMotor.setTargetPositionTolerance(5); - armPower = 0.5; + armPower = 0.4; JunctionHeight = 0; } - - @Override public void exec() { @@ -70,9 +68,8 @@ public class TeleOPArmDriver extends CyberarmState { // ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25; -// armPower = 0.5; - robot.ArmMotor.setTargetPosition(TargetPosition); - robot.ArmMotor.setPower(armPower); +// robot.ArmMotor.setTargetPosition(TargetPosition); +// robot.ArmMotor.setPower(armPower); if (engine.gamepad2.y) { JunctionHeight = 4; @@ -85,211 +82,152 @@ public class TeleOPArmDriver extends CyberarmState { if (engine.gamepad2.x) { JunctionHeight = 2; TargetPosition = armMotorLow; - armPower = -0.5; } if (engine.gamepad2.a) { - armPower = 1; JunctionHeight = 1; TargetPosition = armMotorCollect; } - } -public void eexec() { - - double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - TargetPosition) / 400.0 + 0.1; - - -// ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25; - armPower = 0.5; - robot.ArmMotor.setTargetPosition(TargetPosition); - robot.ArmMotor.setPower(armPower); - - if (engine.gamepad2.y) { - JunctionHeight = 4; - TargetPosition = armMotorHigh; - } - if (engine.gamepad2.b) { - JunctionHeight = 3; - TargetPosition = armMotorMed; - } - if (engine.gamepad2.x) { - JunctionHeight = 2; - TargetPosition = armMotorLow; - } - if (engine.gamepad2.a) { - JunctionHeight = 1; - TargetPosition = armMotorCollect; - } - - if (JunctionHeight == 4 && System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - - if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ { -// robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + ArmMotorStepSize); - } - else 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 (robot.ArmMotor.getCurrentPosition() >= armMotorHigh && - robot.LowRiserLeft.getPosition() >= servoHighLow) { - JunctionHeight = 0; - } - } - - if (JunctionHeight == 3) { - 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); - } - } - else 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); - } - } - else 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.getCurrentPosition() - ArmMotorStepSize); - } - } - else 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.getCurrentPosition() + ArmMotorStepSize); - } - } - else if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 && - robot.LowRiserLeft.getPosition() <= servoMedLow && - robot.ArmMotor.getCurrentPosition() >= armMotorMed) { - JunctionHeight = 0; - } - } - - if (JunctionHeight == 2) { - - 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); - } - } - else 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); - } - } - else 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.getCurrentPosition() - ArmMotorStepSize); - } - } - else 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.getCurrentPosition() + ArmMotorStepSize); - } - } - else if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && - robot.LowRiserLeft.getPosition() <= servoLowLow && - robot.ArmMotor.getCurrentPosition() >= armMotorLow) { - JunctionHeight = 0; - } - } - - if (JunctionHeight == 1) { - TargetPosition = 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.getCurrentPosition() - ArmMotorStepSize); - } - } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && - robot.ArmMotor.getCurrentPosition() <= armMotorCollect) { - JunctionHeight = 0; - } - } - -// if (engine.gamepad2.dpad_left && Ingenuity != 1) { -// Ingenuity = 1; -// } -// -// if (engine.gamepad2.dpad_right && Ingenuity != 2) { -// Ingenuity = 2; -// } -// -// 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 && robot.ArmMotor.getCurrentPosition() < armMotorHigh + 5) { - lastStepTime = System.currentTimeMillis(); - robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + 1); - robot.ArmMotor.setPower(armPower); - } else if (engine.gamepad2.left_trigger > 0.1) { - JunctionHeight = 0; - if (System.currentTimeMillis() - lastStepTime >= 100 && robot.ArmMotor.getCurrentPosition() > armMotorCollect - 5) { - lastStepTime = System.currentTimeMillis(); - robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - 1); + if (JunctionHeight == 4) { + if (robot.LowRiserLeft.getPosition() <= servoHigh && robot.ArmMotor.getTargetPosition() != armMotorHigh) { + robot.ArmMotor.setPower(0); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); robot.ArmMotor.setPower(armPower); + robot.ArmMotor.setTargetPosition(armMotorHigh); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + if (robot.ArmMotor.getCurrentPosition() >= armMotorLow && robot.LowRiserLeft.getPosition() < servoHigh) { + 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.ArmMotor.getTargetPosition() == armMotorHigh && robot.LowRiserLeft.getPosition() == servoHigh) { + JunctionHeight = 0; } } + if (JunctionHeight == 3) { + if (robot.LowRiserLeft.getPosition() <= servoMed && robot.ArmMotor.getTargetPosition() != armMotorMed) { + robot.ArmMotor.setPower(0); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ArmMotor.setPower(armPower); + robot.ArmMotor.setTargetPosition(armMotorMed); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + if (robot.ArmMotor.getCurrentPosition() >= armMotorLow && robot.LowRiserLeft.getPosition() < servoMed) { + 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() > servoMed) { + 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.ArmMotor.getTargetPosition() == armMotorMed && robot.LowRiserLeft.getPosition() == servoMed) { + JunctionHeight = 0; + } + } + + if (JunctionHeight == 2) { + if (robot.LowRiserLeft.getPosition() <= servoLow && robot.ArmMotor.getTargetPosition() != armMotorLow) { + robot.ArmMotor.setPower(0); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ArmMotor.setPower(armPower); + robot.ArmMotor.setTargetPosition(armMotorLow); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + if (robot.ArmMotor.getCurrentPosition() >= armMotorLow && robot.LowRiserLeft.getPosition() < servoLow) { + 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() > servoLow) { + 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.ArmMotor.getTargetPosition() == armMotorLow && robot.LowRiserLeft.getPosition() == servoLow) { + JunctionHeight = 0; + } + } + + if (JunctionHeight == 1) { + if (robot.LowRiserLeft.getPosition() <= servoCollect && robot.ArmMotor.getTargetPosition() != armMotorCollect) { + robot.ArmMotor.setPower(0); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ArmMotor.setPower(armPower); + robot.ArmMotor.setTargetPosition(armMotorCollect); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + if (robot.LowRiserLeft.getPosition() > servoCollect) { + 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.ArmMotor.getTargetPosition() == armMotorCollect && robot.LowRiserLeft.getPosition() == servoCollect) { + JunctionHeight = 0; + } + } + + if (engine.gamepad2.left_bumper) { + if (robot.ArmMotor.getTargetPosition() != OverrideTarget) { + OverrideTarget = robot.ArmMotor.getCurrentPosition() - 80; + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ArmMotor.setPower(armPower); + robot.ArmMotor.setTargetPosition(OverrideTarget); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + } + + if (engine.gamepad2.right_bumper) { + if (robot.ArmMotor.getTargetPosition() != OverrideTarget) { + OverrideTarget = robot.ArmMotor.getCurrentPosition() + 80; + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ArmMotor.setPower(armPower); + robot.ArmMotor.setTargetPosition(OverrideTarget); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + } + + if (engine.gamepad2.dpad_left) { + robot.collectorRight.setPower(-1); + robot.collectorLeft.setPower(-1); + } else if (engine.gamepad2.dpad_right) { + robot.collectorLeft.setPower(1); + robot.collectorRight.setPower(1); + } else { + robot.collectorLeft.setPower(0); + robot.collectorRight.setPower(0); + } + + } + + @Override + public void buttonUp(Gamepad gamepad, String button) { + if (engine.gamepad2 == gamepad) { + if (button.equals("right_bumper")) { + OverrideTarget = -64; + } else if (button.equals("left_bumper")) { + OverrideTarget = -64; + } + } } -} @Override public void buttonDown(Gamepad gamepad, String button) { if (engine.gamepad2 == gamepad) { - if (button.equals("dpad_left")) { - robot.collectorRight.setPower(-1); - robot.collectorLeft.setPower(-1); - } else if (button.equals("dpad_right")) { - robot.collectorLeft.setPower(1); - robot.collectorRight.setPower(1); + if (button.equals("right_bumper")) { + + } else if (button.equals("left_bumper")) { + } } - - } - public void buttonUp(Gamepad gamepad, String button) { - if (engine.gamepad2 == gamepad) { - if (button.equals("dpad_left")) { - robot.collectorRight.setPower(0); - robot.collectorLeft.setPower(0); - } else if (button.equals("dpad_right")) { - robot.collectorLeft.setPower(0); - robot.collectorRight.setPower(0); - } - } - } } \ No newline at end of file