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

Cleaning up TeleOP

Browse Source
This commit is contained in:
Sodi
2022-10-22 12:00:27 -05:00
parent bb5fc381b0
commit 3494ddaa4f
1 changed files with 263 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files

523
TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
View File

@@ -15,7 +15,6 @@ public class PrototypeTeleOPState extends CyberarmState {
private boolean bprev; // sticky key variable for the gamepad private boolean bprev; // sticky key variable for the gamepad
private double drivePower = 1; private double drivePower = 1;
private boolean UpDPad; private boolean UpDPad;
private int armTargetPosition = 0, armCollectPosition = 125, armDeliverPosition = 400;
private double collectorRiserPosition; private double collectorRiserPosition;
private boolean raiseHighRiser = true; private boolean raiseHighRiser = true;
private long lastStepTime = 0, BeginningofActionTime; private long lastStepTime = 0, BeginningofActionTime;
@@ -237,6 +236,20 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
} }
if (engine.gamepad1.dpad_left) {
robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(1);
} else if (engine.gamepad1.dpad_right) {
robot.collectorLeft.setPower(-1);
robot.collectorRight.setPower(-1);
} else {
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
}
// //
// } // }
// //
@@ -254,13 +267,6 @@ public class PrototypeTeleOPState extends CyberarmState {
// //
// } // }
//// ////
//// if (engine.gamepad1.a) {
//// robot.LowRiserLeft.setPosition(0);
//// robot.LowRiserRight.setPosition(1);
//// }
//// if (engine.gamepad1.x) {
//// robot.LowRiserLeft.setPosition(1);
//// robot.LowRiserRight.setPosition(0);
//// } //// }
//// ////
//// if (engine.gamepad2.right_stick_y < -0.1) { //// if (engine.gamepad2.right_stick_y < -0.1) {
@@ -365,59 +371,59 @@ public class PrototypeTeleOPState extends CyberarmState {
// //
// } // }
// //
// if (engine.gamepad2.start){ //// if (engine.gamepad2.start){
// ////
// if (System.currentTimeMillis() - lastStepTime >= 150) { //// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis(); // lastStepTime = System.currentTimeMillis();
//
// switch (CyclingArmUpAndDown) {
//
// // upper arm up
// case 0:
// if (robot.HighRiserLeft.getPosition() < 1) {
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
// } else {
// CyclingArmUpAndDown = CyclingArmUpAndDown +1;
// }
// break;
//
// // lower arm up
// case 1:
// if (robot.LowRiserLeft.getPosition() < 1) {
// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
// } else {
// CyclingArmUpAndDown = CyclingArmUpAndDown +1;
// }
// break;
//
// // lower arm down
// case 2:
// if (robot.LowRiserLeft.getPosition() >= 0.44) {
// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
// } else {
// CyclingArmUpAndDown = CyclingArmUpAndDown +1;
// }
// break;
//
// // upper arm down
// case 3:
// if (robot.HighRiserLeft.getPosition() >= 0.45) {
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
// } else {
// CyclingArmUpAndDown = 0;
// }
// break;
//
// default:
// break;
// } // end of switch switch (CyclingArmUpAndDown) {
// } // end of time if statement
// } // end of start button press // upper arm up
case 0:
if (robot.HighRiserLeft.getPosition() < 1) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
} else {
CyclingArmUpAndDown = CyclingArmUpAndDown +1;
}
break;
// lower arm up
case 1:
if (robot.LowRiserLeft.getPosition() < 1) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} else {
CyclingArmUpAndDown = CyclingArmUpAndDown +1;
}
break;
// lower arm down
case 2:
if (robot.LowRiserLeft.getPosition() >= 0.44) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
} else {
CyclingArmUpAndDown = CyclingArmUpAndDown +1;
}
break;
// upper arm down
case 3:
if (robot.HighRiserLeft.getPosition() >= 0.45) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
} else {
CyclingArmUpAndDown = 0;
}
break;
default:
break;
} // end of switch
// end of time if statement
// end of start button press
// if (engine.gamepad2.left_stick_y > 0.1) { // if (engine.gamepad2.left_stick_y > 0.1) {
// robot.HighRiserLeft.setPosition(0.5); // robot.HighRiserLeft.setPosition(0.5);
@@ -440,204 +446,204 @@ public class PrototypeTeleOPState extends CyberarmState {
// } // }
// For raising, high risers ALWAYS raise first, for lowering, low risers ALWAYS lower first. // For raising, high risers ALWAYS raise first, for lowering, low risers ALWAYS lower first.
if (engine.gamepad2.back) { // if (engine.gamepad2.back) {
RobotPosition = robot.backRightDrive.getCurrentPosition(); // RobotPosition = robot.backRightDrive.getCurrentPosition();
//
switch (AutoChain) { // switch (AutoChain) {
//
case 0://Initialize // case 0://Initialize
RobotStartingPosition = RobotPosition; // RobotStartingPosition = RobotPosition;
AutoChain += 1; // AutoChain += 1;
break; // break;
//
case 1://Drive 1 square forward // case 1://Drive 1 square forward
if (RobotPosition - RobotStartingPosition < 2500){ // if (RobotPosition - RobotStartingPosition < 2500){
drivePower = 1; // drivePower = 1;
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 // } else
{ // {
AutoChain += 1; // AutoChain += 1;
} // }
break; // break;
//
case 2://Rotate Counterclockwise for 45 degrees // case 2://Rotate Counterclockwise for 45 degrees
RobotRotation = robot.imu.getAngularOrientation().firstAngle; // RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (RobotRotation <= 45) { // if (RobotRotation <= 45) {
drivePower = 0.4; // drivePower = 0.4;
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 // } else
{ // {
AutoChain += 1; // AutoChain += 1;
drivePower = 0; // drivePower = 0;
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);
} // }
break; // break;
//
case 3://Raise upper arm fully // case 3://Raise upper arm fully
if (robot.HighRiserLeft.getPosition() < 1) { // if (robot.HighRiserLeft.getPosition() < 1) {
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);
} // }
} else { // } else {
AutoChain +=1; // AutoChain +=1;
} // }
break; // break;
//
case 4://Raise lower arm fully // case 4://Raise lower arm fully
if (robot.LowRiserLeft.getPosition() < 1) { // if (robot.LowRiserLeft.getPosition() < 1) {
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);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); // robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} // }
} else { // } else {
AutoChain += 1; // AutoChain += 1;
//
} // }
break; // break;
//
case 5://initialize for moving forward // case 5://initialize for moving forward
RobotStartingPosition = robot.backRightDrive.getCurrentPosition(); // RobotStartingPosition = robot.backRightDrive.getCurrentPosition();
AutoChain += 1; // AutoChain += 1;
break; // break;
//
case 6://Drive forward 1/4 square // case 6://Drive forward 1/4 square
if (RobotPosition - RobotStartingPosition < 1200){ // if (RobotPosition - RobotStartingPosition < 1200){
drivePower = 1; // drivePower = 1;
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 // } else
{ // {
drivePower = 0; // drivePower = 0;
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);
AutoChain += 1; // AutoChain += 1;
} // }
break; // break;
//
case 7://Lower low arm fully // case 7://Lower low arm fully
if (robot.LowRiserLeft.getPosition() > 0.5) { // if (robot.LowRiserLeft.getPosition() > 0.5) {
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);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); // robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
//
} // }
}else // }else
{ // {
AutoChain += 1; // AutoChain += 1;
} // }
break; // break;
//
case 8: // case 8:
BeginningofActionTime = System.currentTimeMillis(); // BeginningofActionTime = System.currentTimeMillis();
AutoChain += 1; // AutoChain += 1;
//
break; // break;
case 9://Eject // case 9://Eject
if (System.currentTimeMillis() - BeginningofActionTime < 2000) { // if (System.currentTimeMillis() - BeginningofActionTime < 2000) {
robot.collectorRight.setPower(-1); // robot.collectorRight.setPower(-1);
robot.collectorLeft.setPower(1); // robot.collectorLeft.setPower(1);
//
} else { // } else {
robot.collectorLeft.setPower(0); // robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0); // robot.collectorRight.setPower(0);
AutoChain += 1; // AutoChain += 1;
} // }
break; // break;
//
case 10://Raise low arm // case 10://Raise low arm
if (robot.LowRiserLeft.getPosition() < 1) { // if (robot.LowRiserLeft.getPosition() < 1) {
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 { // } else {
AutoChain += 1; // AutoChain += 1;
} // }
break; // break;
//
case 11://Initialize backup // case 11://Initialize backup
RobotStartingPosition = RobotPosition; // RobotStartingPosition = RobotPosition;
AutoChain += 1; // AutoChain += 1;
break; // break;
//
case 12://Drive backwards 1/4 square // case 12://Drive backwards 1/4 square
if (RobotPosition - RobotStartingPosition > -1200){ // if (RobotPosition - RobotStartingPosition > -1200){
drivePower = -1; // drivePower = -1;
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 // } else
{ // {
drivePower = 0; // drivePower = 0;
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);
AutoChain += 1; // AutoChain += 1;
} // }
break; // break;
//
case 13://Turn 45 degrees clockwise // case 13://Turn 45 degrees clockwise
RobotRotation = robot.imu.getAngularOrientation().firstAngle; // RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (RobotRotation > 0) { // if (RobotRotation > 0) {
drivePower = 0.4; // drivePower = 0.4;
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 // } else
{ // {
AutoChain += 1; // AutoChain += 1;
drivePower = 0; // drivePower = 0;
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);
} // }
break; // break;
//
case 14://Initialize // case 14://Initialize
RobotStartingPosition = RobotPosition; // RobotStartingPosition = RobotPosition;
AutoChain += 1; // AutoChain += 1;
break; // break;
//
case 15://Drive 1 square forward // case 15://Drive 1 square forward
if (RobotPosition - RobotStartingPosition < 2500){ // if (RobotPosition - RobotStartingPosition < 2500){
drivePower = 1; // drivePower = 1;
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 // } else
{ // {
AutoChain = 999; // AutoChain = 999;
drivePower = 0; // drivePower = 0;
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);
} // }
break; // break;
//
case 999: // case 999:
//
break; // break;
@@ -645,6 +651,3 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
}
}