From c29ce160a796062ebc54ed45388de4a7be025b3d Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sat, 7 Jan 2023 16:06:40 -0600 Subject: [PATCH] driving straight added --- .../RightFourConeAutonomousEngine.java | 31 +++++-- .../States/CollectorDistanceState.java | 39 ++++++--- .../States/DriverStateWithOdometer.java | 80 ++++++++++++++----- .../Autonomous/States/RotationState.java | 7 ++ .../TeleOp/engine/DriveDebugEngine.java | 15 ++++ 5 files changed, 132 insertions(+), 40 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java index 509c155..c6ccea2 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java @@ -50,6 +50,9 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // 6-1 drive forward or backwards if needed this is customizable so we can adapt addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1")); + //pause + addState(new BottomArm(robot, "RightFourCone", "06-2")); + // 7 Drop bottom arm down on the junction to place cone addState(new BottomArm(robot, "RightFourCone", "07-0")); @@ -70,18 +73,30 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // 11 Rotate towards stack addState(new RotationState(robot, "RightFourCone", "11-0")); // -// // 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor) -// addState(new TopArm(robot, "RightFourCone", "12-0")); + // 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor) + addState(new TopArm(robot, "RightFourCone", "12-0")); + + // drive forward at the stack without sensor + addState(new DriverStateWithOdometer(robot, "RightFourCone", "12-1")); + + // turn and align at stack + addState(new RotationState(robot, "RightFourCone", "12-2")); + // -// // 13 Drive at stack while collecting and check to see when we grab it -// addState(new CollectorDistanceState(robot, "RightFourCone", "13-0")); + // 13 Drive at stack while collecting and check to see when we grab it + addState(new CollectorDistanceState(robot, "RightFourCone", "13-0")); + + // // // 14 Back up and raise arm -// addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0")); -// addState(new TopArm(robot, "RightFourCone", "14-1")); + addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0")); + addState(new TopArm(robot, "RightFourCone", "14-1")); + + // 14-2 align perpendicular too the wall + addState(new RotationState(robot, "RightFourCone", "14-2")); // -// // 15 Drive All the way back to the medium Junction and raise upper arm (parallel state) -// addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0")); + // 15 Drive All the way back to the tall Junction and raise upper arm (parallel state) + addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0")); // // // 16 Rotate and use sensor to find junction // addState(new RotationState(robot, "RightFourCone", "16-0")); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java index 69b36c2..545fe1a 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -44,6 +44,7 @@ public class CollectorDistanceState extends CyberarmState { engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoder.getCurrentPosition()); engine.telemetry.addLine(); engine.telemetry.addData("traveledDistance", traveledDistance); @@ -74,12 +75,14 @@ public class CollectorDistanceState extends CyberarmState { robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); robot.collectorLeft.setPower(1); robot.collectorRight.setPower(1); @@ -96,6 +99,10 @@ public class CollectorDistanceState extends CyberarmState { @Override public void exec() { if (stateDisabled){ + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); setHasFinished(true); return; } @@ -108,7 +115,7 @@ public class CollectorDistanceState extends CyberarmState { distanceDelta = LastDistanceRead - currentDistance; - if (distanceDelta >= 0.0 || currentDistance > 500) { + if (distanceDelta >= -50.0 || currentDistance > 500) { // I am moving forward // and im close too my target. LastDistanceRead = currentDistance; @@ -119,18 +126,24 @@ public class CollectorDistanceState extends CyberarmState { debugStatus = "Nothing Collected"; robot.collectorLeft.setPower(0); robot.collectorRight.setPower(0); + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + setHasFinished(true); + return; } } - if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { - double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); + if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 70) { + double delta = traveledDistance - Math.abs(robot.OdometerEncoder.getCurrentPosition()); - if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) { + if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) <= RampUpDistance) { // ramping up - drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15; - } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) { + drivePower = (Math.abs((float) robot.OdometerEncoder.getCurrentPosition()) / RampUpDistance) + 0.15; + } else if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) >= delta) { // ramping down drivePower = ((delta / RampDownDistance) + 0.15); } else { @@ -147,7 +160,7 @@ public class CollectorDistanceState extends CyberarmState { drivePower = drivePower * -1; } - if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) { + if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) { robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower); @@ -167,6 +180,10 @@ public class CollectorDistanceState extends CyberarmState { if (runTime() - inRangeTime >= collectTime){ robot.collectorRight.setPower(0); robot.collectorLeft.setPower(0); + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); setHasFinished(true); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java index 030ef21..acd6ad7 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java @@ -10,10 +10,11 @@ import org.timecrafters.TeleOp.states.PhoenixBot1; public class DriverStateWithOdometer extends CyberarmState { private final boolean stateDisabled; PhoenixBot1 robot; - private int RampUpDistance; - private int RampDownDistance; + private double RampUpDistance; + private double RampDownDistance; private int maximumTolerance; private float direction; + private boolean targetAchieved = false; public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) { this.robot = robot; this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); @@ -34,11 +35,13 @@ public class DriverStateWithOdometer extends CyberarmState { robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -52,18 +55,24 @@ public class DriverStateWithOdometer extends CyberarmState { return; } - double CurrentPosition = robot.OdometerEncoder.getCurrentPosition(); - double delta = traveledDistance - Math.abs(CurrentPosition); + double CurrentPosition = Math.abs(robot.OdometerEncoder.getCurrentPosition()); if (Math.abs(CurrentPosition) <= RampUpDistance){ // ramping up - float ratio = (Math.abs((float)CurrentPosition) / RampUpDistance); - drivePower = (targetDrivePower - 0.25) * ratio + 0.25; +// double ratio = (Math.abs(CurrentPosition) / RampUpDistance); + if (targetDrivePower > 0) { + drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25; + } else { + drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25; + } } else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){ // ramping down - double ratio = (1.0 - ((traveledDistance - RampDownDistance) / RampDownDistance)); - drivePower = (targetDrivePower - 0.25) * ratio + 0.25; + if (targetDrivePower > 0){ + drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25); + } else { + drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25); + } } else { // middle ground @@ -75,11 +84,22 @@ public class DriverStateWithOdometer extends CyberarmState { drivePower = targetDrivePower; } - if (targetDrivePower < 0 && drivePower != targetDrivePower) { + if (targetDrivePower < 0 && drivePower > 0) { drivePower = drivePower * -1; } if (Math.abs(CurrentPosition) < traveledDistance - maximumTolerance){ + if (targetAchieved) { + drivePower = drivePower * 0.25; + + if (Math.abs(drivePower) < 0.25){ + if (drivePower < 0) { + drivePower = -0.25; + } else { + drivePower = 0.25; + } + } + } robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower); @@ -87,10 +107,24 @@ public class DriverStateWithOdometer extends CyberarmState { } else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) { - robot.backLeftDrive.setPower(targetDrivePower * -0.25); - robot.backRightDrive.setPower(targetDrivePower * -0.25); - robot.frontLeftDrive.setPower(targetDrivePower * -0.25); - robot.frontRightDrive.setPower(targetDrivePower * -0.25); + targetAchieved = true; + + drivePower = targetDrivePower * -0.25; + + if (Math.abs(drivePower) < 0.25){ + if (drivePower < 0) { + drivePower = -0.25; + } else { + drivePower = 0.25; + } + } + + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + + } else { robot.backLeftDrive.setPower(0); robot.backRightDrive.setPower(0); @@ -98,24 +132,25 @@ public class DriverStateWithOdometer extends CyberarmState { robot.frontRightDrive.setPower(0); setHasFinished(true); } - if (!getHasFinished()){ + + if (!getHasFinished() && !targetAchieved){ float angle = robot.imu.getAngularOrientation().firstAngle - direction; + if (targetDrivePower < 0) { angle = angle * -1;} + if (angle < -0.25){ - robot.backLeftDrive.setPower(drivePower * 0.25); + robot.backLeftDrive.setPower(drivePower * 0); robot.backRightDrive.setPower(drivePower * 1.25); - robot.frontLeftDrive.setPower(drivePower * 0.25); + robot.frontLeftDrive.setPower(drivePower * 0); robot.frontRightDrive.setPower(drivePower * 1.25); } - if (angle > 0.25){ + if (angle > 0.25) { robot.backLeftDrive.setPower(drivePower * 1.25); - robot.backRightDrive.setPower(drivePower * 0.25); + robot.backRightDrive.setPower(drivePower * 0); robot.frontLeftDrive.setPower(drivePower * 1.25); - robot.frontRightDrive.setPower(drivePower * 0.25); + robot.frontRightDrive.setPower(drivePower * 0); } } - - } @Override @@ -133,6 +168,9 @@ public class DriverStateWithOdometer extends CyberarmState { engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle); engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle); + engine.telemetry.addData("Target Achieved", targetAchieved); + + engine.telemetry.addData("drivePower", drivePower); engine.telemetry.addData("targetDrivePower", targetDrivePower); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java index 6c4ce04..dc60c74 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -36,6 +36,13 @@ public class RotationState extends CyberarmState { if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){ drivePowerVariable = 0.3 * drivePower; + if (Math.abs(drivePowerVariable) < 0.3) { + if (drivePowerVariable < 0){ + drivePowerVariable = -0.3; + } else { + drivePowerVariable = 0.3; + } + } debugStatus = "Rotate Slow"; } // end of if else { diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java new file mode 100644 index 0000000..ac6cec4 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java @@ -0,0 +1,15 @@ +package org.timecrafters.TeleOp.engine; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.TeleOp.states.PhoenixBot1; + +public class DriveDebugEngine extends CyberarmEngine { + + PhoenixBot1 robot; + + @Override + public void setup() { + robot = new PhoenixBot1(this); + + } +}