From 03302501940c281807b3ed9b6106430e8b9d86b5 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Wed, 27 Dec 2023 21:04:32 -0600 Subject: [PATCH 1/2] More work on Pizza bot's teleop- arm is fully functional --- .../TeleOp/States/SodiPizzaTeleOPState.java | 69 +++++++++++++------ 1 file changed, 47 insertions(+), 22 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java index 724e81e..eeb05fa 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java @@ -2,8 +2,8 @@ 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 com.qualcomm.robotcore.hardware.DcMotor; @@ -23,6 +23,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 +36,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 @@ -131,21 +134,24 @@ 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) { + buttonPressed = 'a'; + + if (Math.abs(drivePower) > 0.25) { drivePower = 0.15; } else { - - if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 250) { + //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 +161,25 @@ 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) { + buttonPressed = 'x'; + + if (Math.abs(drivePower) > 0.25) { drivePower = 0.15; } else { - - if (robot.shoulder.getPosition() > ARM_HOVER_5_STACK && System.currentTimeMillis() - lastMoveTime >= 250) { + //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 +188,29 @@ 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) { + buttonPressed = 'b'; + + if (Math.abs(drivePower) > 0.25) { drivePower = 0.15; } else { - - if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 250) { + //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 +219,27 @@ 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) { + buttonPressed = 'y'; + + if (Math.abs(drivePower) > 0.25) { drivePower = 0.15; } else { - - if (robot.shoulder.getPosition() > ARM_HOVER_5_STACK && System.currentTimeMillis() - lastMoveTime >= 250) { - robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05); + //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 +247,8 @@ public class SodiPizzaTeleOPState extends CyberarmState { } //End of code for armPos = 4 + + } } From 3e3a82ac6b835825cf8b1e051863093464840551 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Thu, 28 Dec 2023 20:38:21 -0600 Subject: [PATCH 2/2] More work on Pizza bot's teleop- Gripper works, safeguards in place, PID pending. --- .../TeleOp/States/SodiPizzaTeleOPState.java | 39 ++++++++++++++++--- 1 file changed, 33 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java index eeb05fa..d8a7ba5 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java @@ -4,6 +4,8 @@ import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.ARM_COL import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.ARM_DELIVER; 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; @@ -80,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); @@ -95,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); @@ -104,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*/) { @@ -115,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); @@ -124,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); @@ -145,6 +150,10 @@ public class SodiPizzaTeleOPState extends CyberarmState { 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. @@ -172,6 +181,10 @@ public class SodiPizzaTeleOPState extends CyberarmState { 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. @@ -199,6 +212,10 @@ public class SodiPizzaTeleOPState extends CyberarmState { 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. @@ -230,6 +247,10 @@ public class SodiPizzaTeleOPState extends CyberarmState { 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. @@ -247,6 +268,12 @@ 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); + } + }