From 6e1e3981c68f659bc636d8470b35d0a6bb39ee67 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Thu, 9 Feb 2023 18:13:48 -0600 Subject: [PATCH] Autonomous work --- .../States/CollectorDistanceState.java | 16 +- .../States/DriverParkPlaceState.java | 267 ++++++++++++++++-- .../Autonomous/States/RotationState.java | 54 ++-- .../Autonomous/States/TopArmResetState.java | 48 ++++ .../Autonomous/States/TopArmv2.java | 1 + .../Autonomous/TeleOp/states/PhoenixBot1.java | 2 +- .../TeleOp/states/TeleOPArmDriver.java | 2 +- 7 files changed, 334 insertions(+), 56 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmResetState.java 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 2673dbc..b87775b 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -24,6 +24,10 @@ public class CollectorDistanceState extends CyberarmState { private double inRangeTime; private boolean stateDisabled; private double distanceLimit; + private long maximumLookTime; + private long startOfSequencerTime; + public final double WHEEL_CIRCUMFERENCE = 7.42108499; + public final double COUNTS_PER_REVOLUTION = 8192; public CollectorDistanceState(PhoenixBot1 robot, String groupName, String actionName) { @@ -34,6 +38,7 @@ public class CollectorDistanceState extends CyberarmState { this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); this.collectTime = robot.configuration.variable(groupName, actionName, "collectTime").value(); this.distanceLimit = robot.configuration.variable(groupName, actionName, "distanceLimit").value(); + this.maximumLookTime = robot.configuration.variable(groupName, actionName, "maximumLookTime").value(); this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; @@ -41,6 +46,8 @@ public class CollectorDistanceState extends CyberarmState { @Override public void telemetry() { + engine.telemetry.addData("Time left", System.currentTimeMillis() - startOfSequencerTime); + engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition()); engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); @@ -81,8 +88,10 @@ public class CollectorDistanceState extends CyberarmState { robot.collectorRight.setPower(1); lastMeasuredTime = System.currentTimeMillis(); + startOfSequencerTime = System.currentTimeMillis(); LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM); + traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER); } @@ -91,13 +100,14 @@ public class CollectorDistanceState extends CyberarmState { @Override public void exec() { - if (stateDisabled) { + if (stateDisabled || System.currentTimeMillis() - startOfSequencerTime > maximumLookTime) { + 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 (System.currentTimeMillis() - lastMeasuredTime > 150) { @@ -125,8 +135,6 @@ public class CollectorDistanceState extends CyberarmState { robot.backLeftDrive.setPower(0); setHasFinished(true); - - return; } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java index 8bb3f6b..4a43010 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java @@ -3,6 +3,7 @@ package org.timecrafters.Autonomous.States; import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class DriverParkPlaceState extends CyberarmState { @@ -11,6 +12,25 @@ public class DriverParkPlaceState extends CyberarmState { private int RampUpDistance; private int RampDownDistance; private String intendedPlacement; + public final double WHEEL_CIRCUMFERENCE = 7.42108499; + public final double COUNTS_PER_REVOLUTION = 8192; + private double maximumTolerance; + private float direction; + private boolean targetAchieved = false; + public double startOfRampUpRight; + public double startOfRampDownRight; + public double startOfRampUpLeft; + public double startOfRampDownLeft; + public double endOfRampUpRight; + public double endOfRampDownRight; + public double endOfRampUpLeft; + public double endOfRampDownLeft; + public int driveStage; + public float currentAngle; + public double currentHorizontalEncoder; + + + public DriverParkPlaceState(PhoenixBot1 robot, String groupName, String actionName) { this.robot = robot; @@ -18,6 +38,7 @@ public class DriverParkPlaceState extends CyberarmState { 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(); + this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value(); this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value(); this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; } @@ -35,6 +56,38 @@ public class DriverParkPlaceState extends CyberarmState { robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER); + RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER); + RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER); + maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER); + + if (targetDrivePower > 0) { + startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - 100; + endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance; + startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance; + endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance; + + startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - 100; + endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance; + startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance; + endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance; + + } else { + + startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + 100; + endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance; + startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance; + endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance; + + startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100; + endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance; + startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance; + endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance; + + } + + driveStage = 0; } @Override @@ -44,45 +97,201 @@ public class DriverParkPlaceState extends CyberarmState { return; } String placement = engine.blackboardGetString("parkPlace"); - if (placement != null) { - if (!placement.equals(intendedPlacement)){ - setHasFinished(true); - } - if (placement.equals(intendedPlacement)) { - double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition()); + if (placement == null || !placement.equals(intendedPlacement)) { + + setHasFinished(true); + + } else { + + double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition(); + double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition(); + + // Driving Forward + if (targetDrivePower > 0 && driveStage == 0) { - if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) { // ramping up - drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.25; - } else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) { - // ramping down - drivePower = ((delta / RampDownDistance) + 0.25); - } else { - // middle ground - drivePower = targetDrivePower; + if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) || + (LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) { + + drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) * + (Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER; + + } + + // Driving Normal + else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) || + (LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) { + + drivePower = targetDrivePower; + + } + + // Ramping down going forward + else if ((RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight) || + (LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft)) { + drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) * + (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) + robot.DRIVETRAIN_MINIMUM_POWER; + + } else if (driveStage == 0){ + driveStage = 1; + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + } } - if (Math.abs(drivePower) > Math.abs(targetDrivePower)) { - // This is limiting drive power to the targeted drive power - drivePower = targetDrivePower; + // Driving Backwards .................................................................................................................................Backwards + if (targetDrivePower < 0 && driveStage == 0) { + + // ramping up + if ((RightCurrentPosition <= startOfRampUpRight && RightCurrentPosition >= endOfRampUpRight) || + (LeftCurrentPosition <= startOfRampUpLeft && LeftCurrentPosition >= endOfRampUpLeft)) { + + drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) * + (Math.abs(startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - robot.DRIVETRAIN_MINIMUM_POWER; + + } + + // Driving Normal + else if ((RightCurrentPosition <= endOfRampUpRight && RightCurrentPosition >= startOfRampDownRight) || + (LeftCurrentPosition <= endOfRampUpLeft && LeftCurrentPosition >= startOfRampDownLeft)) { + + drivePower = targetDrivePower; + + } + + // Ramping down going backward + else if ((RightCurrentPosition <= startOfRampDownRight && RightCurrentPosition >= endOfRampDownRight) || + (LeftCurrentPosition <= startOfRampDownLeft && LeftCurrentPosition >= endOfRampDownLeft)) { + + drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) * + (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) - robot.DRIVETRAIN_MINIMUM_POWER; + } else if (driveStage == 0){ + driveStage = 1; + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + + } + + // end of ramp down } - if (targetDrivePower < 0 && drivePower != targetDrivePower) { - drivePower = drivePower * -1; + // Forwards distance adjust...............................................................................................................................STAGE 1 + if (driveStage == 1 && targetDrivePower > 0) { + + if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) && + RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) { + + drivePower = robot.DRIVETRAIN_MINIMUM_POWER; + + } else if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) && + RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) { + + drivePower = -robot.DRIVETRAIN_MINIMUM_POWER; + + } else { + driveStage = 2; + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + } } - if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) { - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); + // backwards distance adjust + if (driveStage == 1 && targetDrivePower < 0) { + + if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) && + RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) { + + drivePower = -robot.DRIVETRAIN_MINIMUM_POWER; + + } else if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) && + RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) { + + drivePower = robot.DRIVETRAIN_MINIMUM_POWER; + + } else { + driveStage = 2; + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + } + } + + if (driveStage == 0 || driveStage == 1) { robot.frontRightDrive.setPower(drivePower); - } else { - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL); + robot.backRightDrive.setPower(drivePower); + robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL); + } + // Heading adjustment + if (driveStage == 2 || driveStage == 4) { + + currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + + if (currentAngle - direction > robot.ROTATION_TOLERANCE) { + + robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER ); + robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER ); + robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER ); + robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER ); + + } + else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) { + + robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER); + robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER); + robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER); + robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER); + + } else { + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + + driveStage ++; + } + } + + // ...........................................................................................................................................Strafe Adjustment + if ( driveStage == 3 ){ + + currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition(); + if (currentHorizontalEncoder > 200){ + + robot.frontRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER ); + robot.frontLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER ); + robot.backRightDrive.setPower(robot.STRAFE_MINIMUM_POWER ); + robot.backLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER ); + + } + else if (currentHorizontalEncoder < -200){ + + robot.frontRightDrive.setPower(robot.STRAFE_MINIMUM_POWER ); + robot.frontLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER ); + robot.backRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER ); + robot.backLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER ); + + } else { + + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + + driveStage = 4; + + } + + if (driveStage == 5) { setHasFinished(true); - } // else ending + } } // placement equals if statement // setHasFinished(true); } // end of placement doesn't equal null 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 1e09d22..62d0504 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -7,6 +7,7 @@ import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class RotationState extends CyberarmState { private final boolean stateDisabled; PhoenixBot1 robot; + public RotationState(PhoenixBot1 robot, String groupName, String actionName) { this.robot = robot; this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value(); @@ -52,13 +53,17 @@ public class RotationState extends CyberarmState { drivePowerVariable = ((Math.abs(CurrentRotation - targetRotation) / 90) * (drivePower - robot.ROTATION_MINIMUM_POWER)) + robot.ROTATION_MINIMUM_POWER; - if (ClockWiseRotation) { rotationDirection = 1;} else { rotationDirection = -1;} + if (ClockWiseRotation) { + rotationDirection = 1; + } else { + rotationDirection = -1; + } - robot.backLeftDrive.setPower( drivePowerVariable * rotationDirection ); - robot.backRightDrive.setPower( -drivePowerVariable * rotationDirection ); - robot.frontLeftDrive.setPower( drivePowerVariable * rotationDirection ); - robot.frontRightDrive.setPower( -drivePowerVariable * rotationDirection ); + robot.backLeftDrive.setPower(drivePowerVariable * rotationDirection); + robot.backRightDrive.setPower(-drivePowerVariable * rotationDirection); + robot.frontLeftDrive.setPower(drivePowerVariable * rotationDirection); + robot.frontRightDrive.setPower(-drivePowerVariable * rotationDirection); if (Math.abs(Math.abs(CurrentRotation) - Math.abs(targetRotation)) <= robot.ROTATION_TOLERANCE && (RotationStage == 0) && @@ -66,14 +71,14 @@ public class RotationState extends CyberarmState { RotationStage = 1; lastStepTime = System.currentTimeMillis(); } - } + } - if (RotationStage == 1){ - robot.backLeftDrive.setPower( 0 ); - robot.backRightDrive.setPower( 0 ); - robot.frontLeftDrive.setPower( 0 ); - robot.frontRightDrive.setPower( 0 ); - if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION ){ + if (RotationStage == 1) { + robot.backLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.frontRightDrive.setPower(0); + if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) { RotationStage = 2; } } @@ -83,13 +88,14 @@ public class RotationState extends CyberarmState { if (CurrentRotation - targetRotation > robot.ROTATION_TOLERANCE) { // CW - robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER ); - robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER ); - robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER ); - robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER ); + robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER); + robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER); + robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER); + robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER); - } - else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) { + lastStepTime = System.currentTimeMillis(); + + } else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) { // CCW robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER); @@ -97,18 +103,24 @@ public class RotationState extends CyberarmState { robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER); robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER); + lastStepTime = System.currentTimeMillis(); + } else { robot.frontRightDrive.setPower(0); robot.frontLeftDrive.setPower(0); robot.backRightDrive.setPower(0); robot.backLeftDrive.setPower(0); - RotationStage ++; - setHasFinished(true); + if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) { + RotationStage = 3; + } } } - + if (RotationStage == 3) { + RotationStage ++; + setHasFinished(true); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmResetState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmResetState.java new file mode 100644 index 0000000..c5fe128 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmResetState.java @@ -0,0 +1,48 @@ +package org.timecrafters.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; + +public class TopArmResetState extends CyberarmState { + private final PhoenixBot1 robot; + private double targetPower; + private int targetPosition; + private int tolerance; + private long lastMeasuredTime; + private long pausingTime; + + + public TopArmResetState(PhoenixBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value(); + this.targetPower = robot.configuration.variable(groupName, actionName, "targetPower").value(); + this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value(); + this.pausingTime = robot.configuration.variable(groupName, actionName, "pausingTime").value(); + } + + @Override + public void start() { + robot.ArmMotor.setTargetPosition(targetPosition); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.ArmMotor.setPower(targetPower); + lastMeasuredTime = System.currentTimeMillis(); + + } + + @Override + public void exec() { + + if (System.currentTimeMillis() - lastMeasuredTime > pausingTime) { + + robot.ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.ArmMotor.setTargetPosition(0); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.ArmMotor.setPower(0.2); + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java index 043a9a3..dfac375 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java @@ -24,6 +24,7 @@ public class TopArmv2 extends CyberarmState { robot.ArmMotor.setTargetPosition(targetPosition); robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.ArmMotor.setPower(targetPower); + } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java index 5ad7768..5e193f2 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java @@ -240,7 +240,7 @@ public class PhoenixBot1 { ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD); ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); CameraServo.setDirection(Servo.Direction.FORWARD); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java index 7ad6183..407f6fc 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java @@ -21,7 +21,7 @@ public class TeleOPArmDriver extends CyberarmState { // private double servoLowHigh = 0.75; //High servos, X button private double servoMed = 0.45; //Low servos, B button // private double servoMedHigh = 0.9; //High servos, B button - private double servoHigh = 0.7; //Low servos, Y button + private double servoHigh = 0.8; //Low servos, Y button // private double servoHighHigh = 0.9; //High servos, Y button private double ArmNeededPower; private int armMotorCollect = -100;