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-19 20:20:25 -06:00
parent a325016a44
commit ef1a6e9d0c
1 changed files with 57 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
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_HOVER_5_STACK;
import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.ARM_PRECOLLECT;
import com.qualcomm.robotcore.hardware.DcMotor;
@@ -20,6 +21,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
public double drivePower;
public final double minInput = 0.1 /* <- Minimum input from stick to send command */;
public double lastToldAngle /* <- The angle the bot was last told to stop at */;
private int armPos;
YawPitchRollAngles imuInitAngle;
@@ -42,6 +44,9 @@ public class SodiPizzaTeleOPState extends CyberarmState {
GamepadChecker gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
GamepadChecker gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
lastMoveTime = System.currentTimeMillis();
armPos = 0;
}
@Override
@@ -119,11 +124,12 @@ public class SodiPizzaTeleOPState extends CyberarmState {
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
}
if (engine.gamepad2.a && !engine.gamepad2.start) {
if (Math.abs(drivePower) < 0.5) {
drivePower = 0.25;
}
} else {
if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 250) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
@@ -132,7 +138,56 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.shoulder.setPosition(ARM_COLLECT);
}
}
}
//End of code for armPos = 1
if (engine.gamepad2.y) {
armPos = 2;
}
if (armPos == 2) {
if (Math.abs(drivePower) < 0.5) {
drivePower = 0.25;
} else {
if (robot.shoulder.getPosition() > ARM_HOVER_5_STACK && 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_PRECOLLECT);
}
}
}
//End of code for armPos = 2
if (engine.gamepad2.y) {
armPos = 2;
}
if (armPos == 3) {
if (Math.abs(drivePower) < 0.5) {
drivePower = 0.25;
} else {
if (robot.shoulder.getPosition() > ARM_HOVER_5_STACK && 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_PRECOLLECT);
}
}
}
//End of code for armPos = 2
}
}
}