From f6d85000b95e641d91e3ca0c44ef1611b1c7a7b2 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Thu, 4 Jan 2024 17:57:21 -0600 Subject: [PATCH] Pizza bot is now fieldcentric and arm has been commented out. --- .../Common/SodiPizzaMinibotObject.java | 7 +- .../TeleOp/States/SodiPizzaTeleOPState.java | 349 ++++++++---------- 2 files changed, 162 insertions(+), 194 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java index 9deae07..25c736a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -19,7 +19,8 @@ public class SodiPizzaMinibotObject extends Robot { public HardwareMap hardwareMap; public DcMotor leftFront, rightFront, leftBack, rightBack; - public Servo shoulder, gripper, launcher; +// public Servo shoulder, gripper; + public Servo launcher; public IMU imu; public Rev2mDistanceSensor distSensor; private String string; @@ -70,8 +71,8 @@ public class SodiPizzaMinibotObject extends Robot { rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); //Servo Defining - shoulder = engine.hardwareMap.servo.get("arm"); - gripper = engine.hardwareMap.servo.get("gripper"); +// shoulder = engine.hardwareMap.servo.get("arm"); +// gripper = engine.hardwareMap.servo.get("gripper"); launcher = engine.hardwareMap.servo.get("launcher"); //Distance Sensor 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 bbd8167..1f65b1b 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 @@ -26,7 +26,9 @@ 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 */, proportion, integral, derivative; public float approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3; - private int armPos, objectPos; + private double lfPower, rfPower, lbPower, rbPower; + private float yTransitPercent, xTransitPercent, rotPercent, percentDenom; + private int objectPos; private boolean droneLaunched; private char buttonPressed; private GamepadChecker gp1checker, gp2checker; @@ -48,12 +50,6 @@ public class SodiPizzaTeleOPState extends CyberarmState { @Override 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); - - engine.telemetry.addLine(); engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition()); engine.telemetry.addData("Drone Launched?", droneLaunched); } @@ -77,15 +73,18 @@ public class SodiPizzaTeleOPState extends CyberarmState { gp2checker = new GamepadChecker(engine, engine.gamepad2); lastMoveTime = System.currentTimeMillis(); - armPos = 0; + lastDistRead = System.currentTimeMillis(); } @Override public void exec() { + + + if (Math.abs(engine.gamepad1.left_stick_y) < minInput && Math.abs(engine.gamepad1.left_stick_x) < minInput && - Math.abs(engine.gamepad1.right_stick_x) < minInput) /* <- input from ONLY left stick y means to move forward or backward */{ + Math.abs(engine.gamepad1.right_stick_x) < minInput){ drivePower = 0; robot.leftFront.setPower(drivePower); @@ -94,69 +93,37 @@ public class SodiPizzaTeleOPState extends CyberarmState { robot.rightBack.setPower(drivePower); } - if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle + 0.5) { - if (Math.abs(engine.gamepad1.right_stick_x) > minInput && - Math.abs(engine.gamepad1.left_stick_y) > minInput) { - robot.rightFront.setPower(robot.leftFront.getPower() * 0.8); - robot.rightBack.setPower(robot.leftBack.getPower() * 0.8); - } - - } 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); - robot.leftBack.setPower(robot.rightBack.getPower() * 0.8); + if (Math.abs(yTransitPercent) > 0.01) { + percentDenom = 100; } else { - - robot.leftFront.setPower(drivePower); - robot.rightFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightBack.setPower(drivePower); + percentDenom = 0; } - if (engine.gamepad1.start && !engine.gamepad1.a) /*<-- reset everything: encoders, imu, and armPos int*/ { + if (Math.abs(xTransitPercent) > 0.01) { - robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.imu.resetYaw(); + percentDenom = percentDenom + 100; } - 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*/) { + if (Math.abs(rotPercent) > 0.01) { - drivePower = engine.gamepad1.left_stick_y; - robot.leftFront.setPower(drivePower); - robot.rightFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightBack.setPower(drivePower); + percentDenom = percentDenom + 100; } + yTransitPercent = engine.gamepad1.left_stick_y * 100; + xTransitPercent = engine.gamepad1.left_stick_x * 100; + rotPercent = engine.gamepad1.right_stick_x * -100; - if (Math.abs(engine.gamepad1.left_stick_x) > minInput && - armPos == 0) { + lfPower = ((yTransitPercent + -xTransitPercent + rotPercent) / percentDenom); + robot.leftFront.setPower(lfPower); - drivePower = engine.gamepad1.left_stick_x; - robot.leftFront.setPower(-drivePower); - robot.rightFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightBack.setPower(-drivePower); - } + rfPower = ((yTransitPercent + xTransitPercent + -rotPercent) / percentDenom); + robot.rightFront.setPower(rfPower); - if (Math.abs(engine.gamepad1.right_stick_x) > minInput && - armPos == 0) { + lbPower = ((yTransitPercent + xTransitPercent + rotPercent) / percentDenom); + robot.leftBack.setPower(lbPower); - drivePower = engine.gamepad1.right_stick_x; - robot.leftFront.setPower(-drivePower); - robot.rightFront.setPower(drivePower); - robot.leftBack.setPower(-drivePower); - robot.rightBack.setPower(drivePower); - lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - } + rbPower = ((yTransitPercent + -xTransitPercent + -rotPercent) / percentDenom); + robot.rightBack.setPower(rbPower); if (engine.gamepad2.left_stick_button) { if (System.currentTimeMillis() - lastMoveTime >= 200) { @@ -182,140 +149,140 @@ public class SodiPizzaTeleOPState extends CyberarmState { } } - // 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) { - - buttonPressed = 'a'; - - 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 >= 150) { - robot.shoulder.setPosition(ARM_COLLECT); - armPos = 0; - } - - } - - } - //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) { - - buttonPressed = 'x'; - - 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(); - }//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; - } - - } - } - //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) { - - buttonPressed = 'b'; - - 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 >= 150) { - robot.shoulder.setPosition(ARM_DELIVER); - armPos = 0; - } - - } - } - //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) { - - buttonPressed = 'y'; - - 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(); - } - //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing. - else if (System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(ARM_STOW); - armPos = 0; - } - - } - } +// // 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) { +// +// buttonPressed = 'a'; +// +// 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 >= 150) { +// robot.shoulder.setPosition(ARM_COLLECT); +// armPos = 0; +// } +// +// } +// +// } +// //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) { +// +// buttonPressed = 'x'; +// +// 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(); +// }//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; +// } +// +// } +// } +// //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) { +// +// buttonPressed = 'b'; +// +// 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 >= 150) { +// robot.shoulder.setPosition(ARM_DELIVER); +// armPos = 0; +// } +// +// } +// } +// //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) { +// +// buttonPressed = 'y'; +// +// 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(); +// } +// //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing. +// else if (System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(ARM_STOW); +// armPos = 0; +// } +// +// } +// } //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); - } +// if (engine.gamepad2.dpad_left) { +// robot.gripper.setPosition(GRIPPER_OPEN); +// } else if (engine.gamepad2.dpad_right) { +// robot.gripper.setPosition(GRIPPER_CLOSED); +// } gp1checker.update(); gp2checker.update();