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

Adding run-to on arm heights

Browse Source
This commit is contained in:
Sodi
2022-12-06 20:34:27 -06:00
parent 7d2a0d89ba
commit 78eec25840
1 changed files with 168 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,14 +27,11 @@ public class PhoenixTeleOPState extends CyberarmState {
public void CalculateDeltaRotation() { public void CalculateDeltaRotation() {
if (RotationTarget >= 0 && RobotRotation >= 0) { if (RotationTarget >= 0 && RobotRotation >= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation); DeltaRotation = Math.abs(RotationTarget - RobotRotation);
} } else if (RotationTarget <= 0 && RobotRotation <= 0) {
else if (RotationTarget <= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation); DeltaRotation = Math.abs(RotationTarget - RobotRotation);
} } else if (RotationTarget >= 0 && RobotRotation <= 0) {
else if (RotationTarget >= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget + RobotRotation); DeltaRotation = Math.abs(RotationTarget + RobotRotation);
} } else if (RotationTarget <= 0 && RobotRotation >= 0) {
else if (RotationTarget <=0 && RobotRotation >= 0) {
DeltaRotation = Math.abs(RobotRotation + RotationTarget); DeltaRotation = Math.abs(RobotRotation + RotationTarget);
} }
} }
@@ -77,9 +74,7 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.backRightDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower);
} } 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);
@@ -107,7 +102,7 @@ public class PhoenixTeleOPState extends CyberarmState {
!engine.gamepad1.b && !engine.gamepad1.b &&
!engine.gamepad1.dpad_left && !engine.gamepad1.dpad_left &&
!engine.gamepad1.dpad_right && !engine.gamepad1.dpad_right &&
Math.abs (engine.gamepad1.left_stick_y) < 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1 &&
Math.abs(engine.gamepad1.right_stick_y) < 0.1) { Math.abs(engine.gamepad1.right_stick_y) < 0.1) {
drivePower = 0; drivePower = 0;
robot.backLeftDrive.setPower(-drivePower); robot.backLeftDrive.setPower(-drivePower);
@@ -121,20 +116,18 @@ public class PhoenixTeleOPState extends CyberarmState {
RotationTarget = 180; RotationTarget = 180;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation < 0 && RobotRotation > -179) { if (RobotRotation < 0 && RobotRotation > -179) {
drivePower = (1 * DeltaRotation/180) + MinimalPower; drivePower = (1 * DeltaRotation / 180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} } else if (RobotRotation >= 0) {
else if (RobotRotation >= 0) { drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} } else if (RobotRotation <= -179 || RobotRotation >= 179) {
else if (RobotRotation <= -179 || RobotRotation >= 179) {
drivePower = 0; drivePower = 0;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
@@ -148,14 +141,14 @@ public class PhoenixTeleOPState extends CyberarmState {
RotationTarget = 0; RotationTarget = 0;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation < -1) { if (RobotRotation < -1) {
drivePower = (-1 * DeltaRotation/180) - MinimalPower; drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation > 1) { if (RobotRotation > 1) {
drivePower = (1 * DeltaRotation/180) + MinimalPower; drivePower = (1 * DeltaRotation / 180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -174,14 +167,14 @@ public class PhoenixTeleOPState extends CyberarmState {
RotationTarget = -45; RotationTarget = -45;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation > -45 && RobotRotation <= 135) {//CCW if (RobotRotation > -45 && RobotRotation <= 135) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower; drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation < -45 || RobotRotation > 136) {//CW if (RobotRotation < -45 || RobotRotation > 136) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower; drivePower = (1 * DeltaRotation / 180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -201,14 +194,14 @@ public class PhoenixTeleOPState extends CyberarmState {
RotationTarget = -45; RotationTarget = -45;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation < -46 || RobotRotation > 135) {//CCW if (RobotRotation < -46 || RobotRotation > 135) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower; drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation > -44 && RobotRotation < 135) {//CW if (RobotRotation > -44 && RobotRotation < 135) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower; drivePower = (1 * DeltaRotation / 180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -228,14 +221,14 @@ public class PhoenixTeleOPState extends CyberarmState {
RotationTarget = 45; RotationTarget = 45;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation > -135 && RobotRotation < 44) {//CCW if (RobotRotation > -135 && RobotRotation < 44) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower; drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation < -135 || RobotRotation < 46) {//CW if (RobotRotation < -135 || RobotRotation < 46) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower; drivePower = (1 * DeltaRotation / 180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -255,14 +248,14 @@ public class PhoenixTeleOPState extends CyberarmState {
RotationTarget = 45; RotationTarget = 45;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation > -135 && RobotRotation < 44) {//CCW if (RobotRotation > -135 && RobotRotation < 44) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower; drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation < -135 || RobotRotation > 44) {//CW if (RobotRotation < -135 || RobotRotation > 44) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower; drivePower = (1 * DeltaRotation / 180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -326,46 +319,163 @@ public class PhoenixTeleOPState extends CyberarmState {
} }
}//end of y }//end of y
if (engine.gamepad2.a) { if (engine.gamepad2.a) {
if (robot.HighRiserLeft.getPosition() > 0.45 && robot.LowRiserLeft.getPosition() < 0.5) { if (robot.HighRiserLeft.getPosition() > 0.45 && robot.LowRiserLeft.getPosition() < 0.5) {
if (System.currentTimeMillis() - lastStepTime >= 150) { if (System.currentTimeMillis() - lastStepTime >= 150) {
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.45) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
}//end of a
if (engine.gamepad2.back) {
robot.backLeftDrive.setPower(1);
robot.backRightDrive.setPower(1);
robot.frontLeftDrive.setPower(1);
robot.frontRightDrive.setPower(1);
if (System.currentTimeMillis() - lastStepTime >= 1500) {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
} }
if (System.currentTimeMillis() - lastStepTime >= 150) {
if (robot.HighRiserLeft.getPosition() < 1) { if (robot.LowRiserLeft.getPosition() > 0.45) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
}//end of a
// if (engine.gamepad2.back) {
// robot.backLeftDrive.setPower(1);
// robot.backRightDrive.setPower(1);
// robot.frontLeftDrive.setPower(1);
// robot.frontRightDrive.setPower(1);
// if (System.currentTimeMillis() - lastStepTime >= 1500) {
// robot.backLeftDrive.setPower(0);
// robot.backRightDrive.setPower(0);
// robot.frontLeftDrive.setPower(0);
// robot.frontRightDrive.setPower(0);
// }
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// if (robot.HighRiserLeft.getPosition() < 1) {
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
// }
// }
// }
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// if (robot.LowRiserLeft.getPosition() < 1 && robot.HighRiserLeft.getPosition() == 1) {
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis();
// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
// }
// }
// }
// if (System.currentTimeMillis() >= 250) {
// robot.backLeftDrive.setPower(1);
// robot.backRightDrive.setPower(1);
// robot.frontLeftDrive.setPower(1);
// robot.frontRightDrive.setPower(1);
// if (System.currentTimeMillis() - lastStepTime >= 250) {
// robot.backLeftDrive.setPower(0);
// robot.backRightDrive.setPower(0);
// robot.frontLeftDrive.setPower(0);
// robot.frontRightDrive.setPower(0);
// }
// }
//
// }
if (engine.gamepad2.left_bumper) {
if (robot.HighRiserLeft.getPosition() < 0.73) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (!engine.gamepad2.left_bumper) {
if (robot.HighRiserLeft.getPosition() < 0.73) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(0.73);
robot.HighRiserRight.setPosition(0.73);
}
}
}
}
if (engine.gamepad2.right_bumper) {
if (robot.HighRiserLeft.getPosition() < 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < 0.6 && robot.HighRiserLeft.getPosition() >= 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() > 0.8)
if (robot.HighRiserLeft.getPosition() > 0.87 && robot.LowRiserLeft.getPosition() < 0.7) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
}
}
if (!engine.gamepad2.right_bumper) {
if (robot.HighRiserLeft.getPosition() < 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) { if (System.currentTimeMillis() - lastStepTime >= 150) {
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.6 && robot.HighRiserLeft.getPosition() >= 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.HighRiserLeft.getPosition() > 0.87) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
}
}
} }
if (System.currentTimeMillis() - lastStepTime >= 150) { }
if (robot.LowRiserLeft.getPosition() < 1 && robot.HighRiserLeft.getPosition() == 1) {
if (engine.gamepad2.right_stick_button) {
if (robot.HighRiserLeft.getPosition() < 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.7) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (!engine.gamepad2.right_stick_button) {
if (robot.HighRiserLeft.getPosition() < 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.7) {
if (System.currentTimeMillis() - lastStepTime >= 150) { 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);
@@ -373,27 +483,14 @@ public class PhoenixTeleOPState extends CyberarmState {
} }
} }
} }
if (System.currentTimeMillis() >= 250) {
robot.backLeftDrive.setPower(1);
robot.backRightDrive.setPower(1);
robot.frontLeftDrive.setPower(1);
robot.frontRightDrive.setPower(1);
if (System.currentTimeMillis() - lastStepTime >= 250) {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
}
}
} }
gamepad1Checker.update(); gamepad1Checker.update();
gamepad2Checker.update(); gamepad2Checker.update();
} }
@Override @Override
public void buttonUp(Gamepad gamepad, String button) { public void buttonUp(Gamepad gamepad, String button) {
if (engine.gamepad1 == gamepad && button.equals("start")) { if (engine.gamepad1 == gamepad && button.equals("start")) {