mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-13 06:22:33 +00:00
Debugging arm motor, resolved
This commit is contained in:
Sodi
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user