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

More work on Pizza bot's teleop- arm

Browse Source
This commit is contained in:
NerdyBirdy460
2023-12-21 20:35:05 -06:00
parent e803df0cd9
commit ae85337b5e
1 changed files with 47 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.TeleOp.States;
import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.ARM_COLLECT;
import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.ARM_DELIVER;
import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.ARM_HOVER_5_STACK;
import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.ARM_PRECOLLECT;
@@ -30,6 +31,12 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.setup();
}
@Override
public void telemetry() {
engine.telemetry.addData("Arm should be at Position ", armPos);
engine.telemetry.addData("Arm servo is at ", robot.shoulder.getPosition());
}
@Override
public void init() {
drivePower = 0;
@@ -126,9 +133,13 @@ public class SodiPizzaTeleOPState extends CyberarmState {
if (engine.gamepad2.a && !engine.gamepad2.start) {
armPos = 1;
}
if (Math.abs(drivePower) < 0.5) {
drivePower = 0.25;
if (armPos == 1) {
if (Math.abs(drivePower) > 0.5) {
drivePower = 0.15;
} else {
if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 250) {
@@ -136,6 +147,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
lastMoveTime = System.currentTimeMillis();
} else if (System.currentTimeMillis() - lastMoveTime >= 250) {
robot.shoulder.setPosition(ARM_COLLECT);
armPos = 0;
}
}
@@ -144,14 +156,14 @@ public class SodiPizzaTeleOPState extends CyberarmState {
//End of code for armPos = 1
if (engine.gamepad2.y) {
if (engine.gamepad2.x) {
armPos = 2;
}
if (armPos == 2) {
if (Math.abs(drivePower) < 0.5) {
drivePower = 0.25;
if (Math.abs(drivePower) > 0.5) {
drivePower = 0.15;
} else {
if (robot.shoulder.getPosition() > ARM_HOVER_5_STACK && System.currentTimeMillis() - lastMoveTime >= 250) {
@@ -159,21 +171,44 @@ public class SodiPizzaTeleOPState extends CyberarmState {
lastMoveTime = System.currentTimeMillis();
} else if (System.currentTimeMillis() - lastMoveTime >= 250) {
robot.shoulder.setPosition(ARM_PRECOLLECT);
armPos = 0;
}
}
}
//End of code for armPos = 2
if (engine.gamepad2.y) {
armPos = 2;
if (engine.gamepad2.b && !engine.gamepad2.start) {
armPos = 3;
}
if (armPos == 3) {
if (Math.abs(drivePower) < 0.5) {
drivePower = 0.25;
if (Math.abs(drivePower) > 0.5) {
drivePower = 0.15;
} else {
if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 250) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
lastMoveTime = System.currentTimeMillis();
} else if (System.currentTimeMillis() - lastMoveTime >= 250) {
robot.shoulder.setPosition(ARM_DELIVER);
armPos = 0;
}
}
}
//End of code for armPos = 3
if (engine.gamepad2.y) {
armPos = 4;
}
if (armPos == 4) {
if (Math.abs(drivePower) > 0.5) {
drivePower = 0.15;
} else {
if (robot.shoulder.getPosition() > ARM_HOVER_5_STACK && System.currentTimeMillis() - lastMoveTime >= 250) {
@@ -182,11 +217,12 @@ public class SodiPizzaTeleOPState extends CyberarmState {
}
else if (System.currentTimeMillis() - lastMoveTime >= 250) {
robot.shoulder.setPosition(ARM_PRECOLLECT);
armPos = 0;
}
}
}
//End of code for armPos = 2
//End of code for armPos = 4
}