diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java index cca4e0b..50f5bce 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java @@ -47,12 +47,27 @@ public class TestAutonomousEngine extends CyberarmEngine { addState(new RotationState(robot, "TestAutonomous", "12-0")); //adjust arm height to cone. addState(new TopArm(robot, "TestAutonomous", "13-0")); - //drive towards stack of cones - addState(new CollectorDistanceState(robot, "", "")); -// DriverState driverState = new DriverState(robot, "TestAutonomous", "14-0"); -// addState(driverState); -// //collect next cone -// driverState.addParallelState(new CollectorState(robot, "TestAutonomous", "15-0")); + //drive towards stack of cones while collecting + addState(new CollectorDistanceState(robot, "TestAutonomous", "14-0")); + //drive slightly back + addState(new DriverState(robot, "TestAutonomous", "15-0")); + //lift arm up + addState(new TopArm(robot, "TestAutonomous", "16-0")); + // drive backwards too position + addState(new DriverState(robot, "TestAutonomous", "17-0")); + // rotate + addState(new RotationState(robot, "TestAutonomous", "18-0")); + //lift the upper arm + addState(new TopArm(robot, "TestAutonomous", "19-0")); + //lift the lower arm + addState(new BottomArm(robot, "TestAutonomous", "20-0")); + //drive forward to allign + addState(new DriverState(robot, "TestAutonomous", "21-0")); + // bring arm down. + addState(new TopArm(robot, "TestAutonomous", "22-0")); + // get rid of cone + addState(new CollectorState(robot, "TestAutonomous", "23-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 3e8abe6..e15daac 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -9,6 +9,37 @@ import org.timecrafters.testing.states.PrototypeBot1; public class CollectorDistanceState extends CyberarmState { private final PrototypeBot1 robot; + private int traveledDistance; + private int RampUpDistance; + private int RampDownDistance; + private double drivePower; + private double targetDrivePower; + + public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); + this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value(); + this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value(); + this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); + + } + + @Override + public void telemetry() { + engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM)); + engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition()); + engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); + + engine.telemetry.addData("drivePower", drivePower); + engine.telemetry.addData("targetDrivePower", targetDrivePower); + + engine.telemetry.addData("traveledDistance", traveledDistance); + engine.telemetry.addData("RampUpDistance", RampUpDistance); + engine.telemetry.addData("RampDownDistance", RampDownDistance); + + } @Override public void start() { @@ -33,10 +64,35 @@ public class CollectorDistanceState extends CyberarmState { @Override public void exec() { if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { - robot.frontRightDrive.setPower(0.15); - robot.frontLeftDrive.setPower(0.15); - robot.backRightDrive.setPower(0.15); - robot.backLeftDrive.setPower(0.15); + double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){ + // ramping up + drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15; + } + else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){ + // ramping down + drivePower = ((delta / RampDownDistance) + 0.15); + } else { + // middle ground + drivePower = targetDrivePower; + } + + if (Math.abs(drivePower) > Math.abs(targetDrivePower)){ + // This is limiting drive power to the targeted drive power + drivePower = targetDrivePower; + } + + if (targetDrivePower < 0 && drivePower != targetDrivePower) { + drivePower = drivePower * -1; + } + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){ + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + } } else { robot.frontRightDrive.setPower(0); robot.frontLeftDrive.setPower(0); @@ -49,9 +105,7 @@ public class CollectorDistanceState extends CyberarmState { } } - public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) { - this.robot = robot; -} + } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java index d1e4a5e..e48552f 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java @@ -42,24 +42,22 @@ public class DriverState extends CyberarmState { return; } - double delta = traveledDistance - robot.frontRightDrive.getCurrentPosition(); + double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){ - drivePower = (((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25; + // ramping up + drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25; } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){ - drivePower = (((delta - RampDownDistance) / RampDownDistance) + 0.25) - 1; - - if (drivePower < 0) { - - drivePower = 0.25; - - } - } else { + // ramping down + drivePower = ((delta / RampDownDistance) + 0.25); + } else { + // middle ground drivePower = targetDrivePower; } if (Math.abs(drivePower) > Math.abs(targetDrivePower)){ + // This is limiting drive power to the targeted drive power drivePower = 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 4cd2124..25626bd 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -18,6 +18,24 @@ public class RotationState extends CyberarmState { private double drivePower; private float targetRotation; float RobotRotation; + private double RotationTarget, DeltaRotation; + private double RotationDirectionMinimum; + + + public void CalculateDeltaRotation() { + if (RotationTarget >= 0 && RobotRotation >= 0) { + DeltaRotation = Math.abs(RotationTarget - RobotRotation); + } + else if (RotationTarget <= 0 && RobotRotation <= 0) { + DeltaRotation = Math.abs(RotationTarget - RobotRotation); + } + else if (RotationTarget >= 0 && RobotRotation <= 0) { + DeltaRotation = Math.abs(RotationTarget + RobotRotation); + } + else if (RotationTarget <=0 && RobotRotation >= 0) { + DeltaRotation = Math.abs(RobotRotation + RotationTarget); + } + } @Override public void exec() { @@ -27,6 +45,14 @@ public class RotationState extends CyberarmState { } RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RotationTarget = targetRotation; + CalculateDeltaRotation(); + if (drivePower < 0){ + RotationDirectionMinimum = -0.3; + } else { + RotationDirectionMinimum = 0.3; + } + drivePower = (drivePower * DeltaRotation/180) + RotationDirectionMinimum; if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) { robot.backLeftDrive.setPower(-drivePower); robot.backRightDrive.setPower(drivePower); diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index 4b7a5e1..13f1eda 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -117,8 +117,8 @@ public class PrototypeBot1 { LowRiserLeft.setDirection(Servo.Direction.FORWARD); LowRiserRight.setDirection(Servo.Direction.REVERSE); - LowRiserLeft.setPosition(0.45); - LowRiserRight.setPosition(0.45); + LowRiserLeft.setPosition(0.35); + LowRiserRight.setPosition(0.35); HighRiserLeft.setPosition(0.45); HighRiserRight.setPosition(0.45);