TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 15:32: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:22:45 -05:00
parent 3494ddaa4f
commit 54f95918d5
1 changed files with 91 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -129,8 +129,8 @@ public class PrototypeTeleOPState extends CyberarmState {
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (engine.gamepad1.right_trigger < 0.1&&engine.gamepad1.left_trigger < 0.1 if (engine.gamepad1.right_trigger < 0.1 && engine.gamepad1.left_trigger < 0.1
&& !engine.gamepad1.y&& !engine.gamepad1.x&& !engine.gamepad1.a&& !engine.gamepad1.b) { && !engine.gamepad1.y && !engine.gamepad1.x && !engine.gamepad1.a && !engine.gamepad1.b) {
drivePower = 0; drivePower = 0;
robot.backLeftDrive.setPower(-drivePower); robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
@@ -147,14 +147,14 @@ public class PrototypeTeleOPState extends CyberarmState {
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
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);
} }
if (RobotRotation <-177 || RobotRotation >177) { if (RobotRotation < -177 || RobotRotation > 177) {
drivePower = 0; drivePower = 0;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
@@ -172,14 +172,14 @@ public class PrototypeTeleOPState extends CyberarmState {
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if(RobotRotation > 3) { if (RobotRotation > 3) {
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);
} }
if (RobotRotation >-3 && RobotRotation <3) { if (RobotRotation > -3 && RobotRotation < 3) {
drivePower = 0; drivePower = 0;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
@@ -189,21 +189,21 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.x) { if (engine.gamepad1.x) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (RobotRotation > -45&& RobotRotation <132) {//CCW if (RobotRotation > -45 && RobotRotation < 132) {//CCW
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);
} }
if(RobotRotation < -45 || RobotRotation > 138) {//CW if (RobotRotation < -45 || RobotRotation > 138) {//CW
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);
} }
if (RobotRotation <138 && RobotRotation >132) { if (RobotRotation < 138 && RobotRotation > 132) {
drivePower = 0; drivePower = 0;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
@@ -213,21 +213,21 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.b) { if (engine.gamepad1.b) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (RobotRotation < 45&& RobotRotation > -132) {//CCW if (RobotRotation < 45 && RobotRotation > -132) {//CCW
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);
} }
if(RobotRotation > 45||RobotRotation < -138) {//CW if (RobotRotation > 45 || RobotRotation < -138) {//CW
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);
} }
if (RobotRotation <-132 && RobotRotation >-138) { if (RobotRotation < -132 && RobotRotation > -138) {
drivePower = 0; drivePower = 0;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
@@ -247,7 +247,40 @@ public class PrototypeTeleOPState extends CyberarmState {
robot.collectorRight.setPower(0); robot.collectorRight.setPower(0);
} }
if (engine.gamepad2.y) {
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 (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);
}
}//end of y
if (engine.gamepad2.a) {
if (robot.HighRiserLeft.getPosition() > 0.45 && robot.LowRiserLeft.getPosition() < 0.5) {
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.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
// //
@@ -376,54 +409,54 @@ public class PrototypeTeleOPState extends CyberarmState {
//// if (System.currentTimeMillis() - lastStepTime >= 150) { //// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis(); // lastStepTime = System.currentTimeMillis();
switch (CyclingArmUpAndDown) { switch (CyclingArmUpAndDown) {
// upper arm up // upper arm up
case 0: case 0:
if (robot.HighRiserLeft.getPosition() < 1) { if (robot.HighRiserLeft.getPosition() < 1) {
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 {
CyclingArmUpAndDown = CyclingArmUpAndDown +1; CyclingArmUpAndDown = CyclingArmUpAndDown + 1;
} }
break; break;
// lower arm up // lower arm up
case 1: case 1:
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 {
CyclingArmUpAndDown = CyclingArmUpAndDown +1; CyclingArmUpAndDown = CyclingArmUpAndDown + 1;
} }
break; break;
// lower arm down // lower arm down
case 2: case 2:
if (robot.LowRiserLeft.getPosition() >= 0.44) { if (robot.LowRiserLeft.getPosition() >= 0.44) {
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 {
CyclingArmUpAndDown = CyclingArmUpAndDown +1; CyclingArmUpAndDown = CyclingArmUpAndDown + 1;
} }
break; break;
// upper arm down // upper arm down
case 3: case 3:
if (robot.HighRiserLeft.getPosition() >= 0.45) { if (robot.HighRiserLeft.getPosition() >= 0.45) {
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 {
CyclingArmUpAndDown = 0; CyclingArmUpAndDown = 0;
} }
break; break;
default: default:
break; break;
} // end of switch } // end of switch
// end of time if statement // end of time if statement
// end of start button press // 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);
@@ -646,8 +679,8 @@ public class PrototypeTeleOPState extends CyberarmState {
// break; // break;
}
} }
}
}