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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
SpencerPiha
2023-12-29 22:35:38 -06:00
parent 08cfacf2ea 3e3a82ac6b
commit 4e9e646947
1 changed files with 84 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,8 +2,10 @@ 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;
import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.ARM_STOW;
import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.GRIPPER_CLOSED;
import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.GRIPPER_OPEN;
import com.qualcomm.robotcore.hardware.DcMotor;
@@ -23,6 +25,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
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;
private char buttonPressed;
YawPitchRollAngles imuInitAngle;
@@ -35,6 +38,8 @@ public class SodiPizzaTeleOPState extends CyberarmState {
public void telemetry() {
engine.telemetry.addData("Arm should be at Position ", armPos);
engine.telemetry.addData("Arm servo is at ", robot.shoulder.getPosition());
engine.telemetry.addData("Button Pressed = ", buttonPressed);
}
@Override
@@ -77,8 +82,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.rightBack.setPower(robot.leftBack.getPower() * 0.8);
}
} else
if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle - 0.5 &&
} else if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle - 0.5 &&
Math.abs(engine.gamepad1.left_stick_y) > minInput) {
robot.leftFront.setPower(robot.rightFront.getPower() * 0.8);
@@ -92,7 +96,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.rightBack.setPower(drivePower);
}
if (engine.gamepad1.start && !engine.gamepad1.a) {
if (engine.gamepad1.start && !engine.gamepad1.a) /*<-- reset everything: encoders, imu, and armPos int*/ {
robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
@@ -101,7 +105,9 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.imu.resetYaw();
}
if (Math.abs(engine.gamepad1.left_stick_y) > minInput && Math.abs(engine.gamepad1.left_stick_x) < minInput/* &&
if (Math.abs(engine.gamepad1.left_stick_y) > minInput &&
Math.abs(engine.gamepad1.left_stick_x) < minInput &&
armPos == 0/* &&
robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle - 0.5 &&
robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle + 0.5*/) {
@@ -112,7 +118,8 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.rightBack.setPower(drivePower);
}
if (Math.abs(engine.gamepad1.left_stick_x) > minInput) {
if (Math.abs(engine.gamepad1.left_stick_x) > minInput &&
armPos == 0) {
drivePower = engine.gamepad1.left_stick_x;
robot.leftFront.setPower(-drivePower);
@@ -121,7 +128,8 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.rightBack.setPower(-drivePower);
}
if (Math.abs(engine.gamepad1.right_stick_x) > minInput) {
if (Math.abs(engine.gamepad1.right_stick_x) > minInput &&
armPos == 0) {
drivePower = engine.gamepad1.right_stick_x;
robot.leftFront.setPower(-drivePower);
@@ -131,21 +139,28 @@ public class SodiPizzaTeleOPState extends CyberarmState {
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
}
// This moves the arm to Collect position, which is at servo position 0.00.
if (engine.gamepad2.a && !engine.gamepad2.start) {
armPos = 1;
}
if (armPos == 1) {
if (Math.abs(drivePower) > 0.5) {
drivePower = 0.15;
} else {
buttonPressed = 'a';
if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 250) {
if (Math.abs(drivePower) > 0.25) {
drivePower = 0.15;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} else {
//if servo's position is greater than Collect position with a run-to tolerance of 0.05,
//decrement position at a rate of 0.05 per 150 milliseconds.
if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
lastMoveTime = System.currentTimeMillis();
} else if (System.currentTimeMillis() - lastMoveTime >= 250) {
} else if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(ARM_COLLECT);
armPos = 0;
}
@@ -155,21 +170,29 @@ public class SodiPizzaTeleOPState extends CyberarmState {
}
//End of code for armPos = 1
// This moves the arm to Precollect position, which is at servo position 0.05.
if (engine.gamepad2.x) {
armPos = 2;
}
if (armPos == 2) {
if (Math.abs(drivePower) > 0.5) {
drivePower = 0.15;
} else {
buttonPressed = 'x';
if (robot.shoulder.getPosition() > ARM_HOVER_5_STACK && System.currentTimeMillis() - lastMoveTime >= 250) {
if (Math.abs(drivePower) > 0.25) {
drivePower = 0.15;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} else {
//if servo's position is greater than Precollect position with a run-to tolerance of 0.05,
//decrement position at a rate of 0.05 per 150 milliseconds.
if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
lastMoveTime = System.currentTimeMillis();
} else if (System.currentTimeMillis() - lastMoveTime >= 250) {
}//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to.
else if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(ARM_PRECOLLECT);
armPos = 0;
}
@@ -178,20 +201,33 @@ public class SodiPizzaTeleOPState extends CyberarmState {
}
//End of code for armPos = 2
// This moves the arm to Deliver position, which is at servo position 0.28.
if (engine.gamepad2.b && !engine.gamepad2.start) {
armPos = 3;
}
if (armPos == 3) {
if (Math.abs(drivePower) > 0.5) {
drivePower = 0.15;
} else {
buttonPressed = 'b';
if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 250) {
if (Math.abs(drivePower) > 0.25) {
drivePower = 0.15;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} else {
//if servo's position is less than Deliver position with a run-to tolerance of 0.05,
//increment position at a rate of 0.05 per 150 milliseconds.
if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
lastMoveTime = System.currentTimeMillis();
}//if servo's position is greater than Deliver position with a run-to tolerance of 0.05,
//decrement position at a rate of 0.05 per 150 milliseconds.
else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
lastMoveTime = System.currentTimeMillis();
} else if (System.currentTimeMillis() - lastMoveTime >= 250) {
} else if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(ARM_DELIVER);
armPos = 0;
}
@@ -200,23 +236,31 @@ public class SodiPizzaTeleOPState extends CyberarmState {
}
//End of code for armPos = 3
// This moves the arm to Stow position, which is at servo position 0.72.
if (engine.gamepad2.y) {
armPos = 4;
}
if (armPos == 4) {
if (Math.abs(drivePower) > 0.5) {
drivePower = 0.15;
} else {
buttonPressed = 'y';
if (robot.shoulder.getPosition() > ARM_HOVER_5_STACK && System.currentTimeMillis() - lastMoveTime >= 250) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
if (Math.abs(drivePower) > 0.25) {
drivePower = 0.15;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} else {
//if servo's position is less than Collect position with a run-to tolerance of 0.05,
//increment position at a rate of 0.05 per 150 milliseconds.
if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
lastMoveTime = System.currentTimeMillis();
}
else if (System.currentTimeMillis() - lastMoveTime >= 250) {
robot.shoulder.setPosition(ARM_PRECOLLECT);
//Decrementing is unnecessary, because servo is mechanically inhibited from further advancing.
else if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(ARM_STOW);
armPos = 0;
}
@@ -224,6 +268,14 @@ public class SodiPizzaTeleOPState extends CyberarmState {
}
//End of code for armPos = 4
if (engine.gamepad2.dpad_left) {
robot.gripper.setPosition(GRIPPER_OPEN);
} else if (engine.gamepad2.dpad_right) {
robot.gripper.setPosition(GRIPPER_CLOSED);
}
}
}