TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 20:12:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Adding run-to on arm heights and telemetry, finished the high junction

Browse Source
This commit is contained in:
Sodi
2022-12-20 20:31:40 -06:00
parent ea666420ab
commit a8acf7c5be
1 changed files with 33 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java
View File

@@ -21,7 +21,7 @@ public class PhoenixTeleOPState extends CyberarmState {
private double RotationTarget, DeltaRotation; private double RotationTarget, DeltaRotation;
private double MinimalPower = 0.2; private double MinimalPower = 0.2;
private GamepadChecker gamepad1Checker, gamepad2Checker; private GamepadChecker gamepad1Checker, gamepad2Checker;
private int OCD, OCDSwitchHigh, OCDSwitchMed, OCDSwitchLow; private int OCD;
public PhoenixTeleOPState(PhoenixBot1 robot) { public PhoenixTeleOPState(PhoenixBot1 robot) {
this.robot = robot; this.robot = robot;
@@ -80,15 +80,15 @@ public class PhoenixTeleOPState extends CyberarmState {
if (engine.gamepad1.right_trigger > 0) { if (engine.gamepad1.right_trigger > 0) {
drivePower = engine.gamepad1.right_trigger; drivePower = engine.gamepad1.right_trigger;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower - 0.05);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower - 0.05);
} else if (engine.gamepad1.left_trigger > 0) { } else if (engine.gamepad1.left_trigger > 0) {
drivePower = engine.gamepad1.left_trigger; drivePower = engine.gamepad1.left_trigger;
robot.backLeftDrive.setPower(-drivePower); robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower + 0.05);
robot.frontLeftDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower + 0.05);
} }
if (Math.abs(engine.gamepad1.left_stick_y) > 0.05) { if (Math.abs(engine.gamepad1.left_stick_y) > 0.05) {
@@ -394,7 +394,7 @@ public class PhoenixTeleOPState extends CyberarmState {
OCD = 1; OCD = 1;
} }
if (engine.gamepad2.left_bumper) { if (engine.gamepad2.a) {
OCD = 2; OCD = 2;
} }
@@ -408,14 +408,14 @@ public class PhoenixTeleOPState extends CyberarmState {
if (OCD == 1) { if (OCD == 1) {
if (robot.HighRiserLeft.getPosition() < 0.84) { if (robot.HighRiserLeft.getPosition() < 0.84) {
if (System.currentTimeMillis() - lastStepTime >= 150) { if (System.currentTimeMillis() - lastStepTime >= 125) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
} }
} }
if (robot.LowRiserLeft.getPosition() <= 0.64) { if (robot.LowRiserLeft.getPosition() <= 0.64) {
if (System.currentTimeMillis() - lastStepTime >= 150) { if (System.currentTimeMillis() - lastStepTime >= 125) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
@@ -429,36 +429,33 @@ public class PhoenixTeleOPState extends CyberarmState {
if (OCD == 2) { if (OCD == 2) {
if (robot.HighRiserLeft.getPosition() < 0.85) { if (robot.LowRiserLeft.getPosition() >= 0.5) {
if (robot.LowRiserLeft.getPosition() > 0.52) { if (System.currentTimeMillis() - lastStepTime >= 125) {
if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis();
lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
} else {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
} }
if (robot.HighRiserRight.getPosition() > 0.87) { } else if (robot.LowRiserLeft.getPosition() <= 0.5 && robot.HighRiserLeft.getPosition() > 0.5) {
if (robot.LowRiserLeft.getPosition() > 0.52) { if (System.currentTimeMillis() - lastStepTime >= 125) {
if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis();
lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
} else {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
} }
} else if (robot.LowRiserLeft.getPosition() <= 0.5 && robot.HighRiserLeft.getPosition() <= 0.5) {
OCD = 0;
}
}
if (OCD == 3) {
if (robot.LowRiserLeft.getPosition() > 0.5 && robot.HighRiserLeft.getPosition() < 0.75) {
if (System.currentTimeMillis() - lastStepTime >= 125) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
} else if (robot.HighRiserLeft.getPosition() < 0.75) {
}
} }