diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java index f981830..aa004fb 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java @@ -15,6 +15,8 @@ public class IMUDrive extends CyberarmState { private int tickTarget; private float angleRelative; private float angleTarget; + private int tickStart; + private long finishDelay; public IMUDrive(Robot robot, String groupName, String actionName) { this.robot = robot; @@ -30,11 +32,17 @@ public class IMUDrive extends CyberarmState { double inchesTarget = robot.stateConfiguration.variable(groupName, actionName, "inches").value(); tickTarget = (int) robot.inchesToTicks(inchesTarget); angleTarget = robot.stateConfiguration.variable(groupName, actionName, "angle").value(); + finishDelay = robot.stateConfiguration.variable(groupName,actionName,"delay").value(); } @Override public void start() { + if (!robot.stateConfiguration.action(groupName,actionName).enabled) { + setHasFinished(true); + } + angleTarget=robot.getRotation(); + tickStart = robot.encoderRight.getCurrentPosition(); } @Override @@ -42,9 +50,10 @@ public class IMUDrive extends CyberarmState { robot.updateLocation(); - if (Math.abs(robot.encoderRight.getCurrentPosition()) > tickTarget) { - robot.encoderRight.setPower(0); - robot.encoderLeft.setPower(0); + int ticksTraveled = Math.abs( robot.encoderRight.getCurrentPosition()-tickStart); + if (ticksTraveled > tickTarget) { + robot.setDrivePower(0,0); + sleep(finishDelay); setHasFinished(true); } else { @@ -57,8 +66,21 @@ public class IMUDrive extends CyberarmState { double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower))); - robot.encoderRight.setPower(rightPower * powerAdjust); - robot.encoderLeft.setPower(leftPower * powerAdjust); + robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower); } } + + @Override + public void telemetry() { + engine.telemetry.addLine("Measured Values"); + engine.telemetry.addData("Y", robot.ticksToInches(robot.getLocationY())); + engine.telemetry.addData("X", robot.ticksToInches(robot.getLocationX())); + engine.telemetry.addLine(); + engine.telemetry.addData("Rotation", robot.getRotation()); + engine.telemetry.addLine(); + engine.telemetry.addLine("Total Travel"); + engine.telemetry.addData("Left", robot.ticksToInches(robot.traveledLeft)); + engine.telemetry.addData("Right", robot.ticksToInches(robot.traveledRight)); + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java index 80451c5..1784f2b 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java @@ -9,7 +9,7 @@ public class IMUTurn extends CyberarmState { private String actionName; private String groupName; private double power; - private double angleTarget; + private float angleTarget; private int turnDirection; private float angleAllowance; private boolean useOptimalDirection; @@ -29,21 +29,18 @@ public class IMUTurn extends CyberarmState { useOptimalDirection = (turnDirection==0); } + @Override + public void start() { + if (!robot.stateConfiguration.action(groupName,actionName).enabled) { + setHasFinished(true); + } + } + @Override public void exec() { robot.updateLocation(); - int OptimalDirection; - - if (angleTarget > 180 + robot.getRotation()){ - OptimalDirection = -1; - } else if (angleTarget < -180 + robot.getRotation()){ - OptimalDirection = 1; - } else if (angleTarget > robot.getRotation()){ - OptimalDirection = 1; - }else { - OptimalDirection = -1; - } + int OptimalDirection = getOptimalDirection(angleTarget, robot.getRotation()); if (!useOptimalDirection && OptimalDirection == turnDirection){ useOptimalDirection = true; @@ -53,14 +50,37 @@ public class IMUTurn extends CyberarmState { turnDirection = OptimalDirection; } - robot.encoderLeft.setPower(-power * turnDirection); - robot.encoderRight.setPower(power * turnDirection); + robot.setDrivePower(power * turnDirection,-power * turnDirection); if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) { - robot.encoderRight.setPower(0); - robot.encoderLeft.setPower(0); + robot.setDrivePower(0,0); setHasFinished(true); } + + + } + + private int getOptimalDirection(float angleTarget, float angleCurrent){ + + if (angleCurrent < 0) { + angleCurrent += 360; + } + + if (angleTarget < 0) { + angleTarget += 360; + } + + float degreeDifferance = angleTarget - angleCurrent; + if (degreeDifferance > 180 || (degreeDifferance < 0 && degreeDifferance > -180)) { + return -1; + } else { + return 1; + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("rotation", robot.getRotation()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java index 4056a5d..6e83c82 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java @@ -20,5 +20,8 @@ public class LocalizerTestingEngine extends CyberarmEngine { @Override public void setup() { addState(new IMUDrive(robot,"group", "010_drive")); + addState(new IMUTurn(robot,"group", "020_turn")); + addState(new IMUDrive(robot,"group", "030_drive")); + addState(new IMUTurn(robot,"group", "040_turn")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index 71a8823..2eae4fe 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -29,7 +29,7 @@ public class Robot { double BIAS_LEFT = -1.0; double BIAS_RIGHT = -0.87; - double Circumference_Encoder = Math.PI * 3.8; + double Circumference_Encoder = Math.PI * 4; int Counts_Per_Revolution = 8192; //Robot Localizatoin @@ -37,6 +37,9 @@ public class Robot { private double locationY; private float rotation; + public double traveledLeft; + public double traveledRight; + private int encoderFrontPrevious = 0; private int encoderLeftPrevious = 0; private int encoderBackPrevious = 0; @@ -50,6 +53,11 @@ public class Robot { // encoderBack = hardwareMap.dcMotor.get("encoderBack"); encoderRight = hardwareMap.dcMotor.get("encoderRight"); + encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + encoderRight.setDirection(DcMotorSimple.Direction.REVERSE); encoderLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -72,13 +80,18 @@ public class Robot { } public void updateLocation(){ - rotation = imu.getAngularOrientation().firstAngle; + // IMU orientation is inverted to have clockwise be positive. + rotation = -imu.getAngularOrientation().firstAngle; + float rotationChange = rotation - rotationPrevious; int encoderLeftCurrent = encoderLeft.getCurrentPosition(); int encoderRightCurrent = encoderRight.getCurrentPosition(); double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious; double encoderRightChange = encoderRightCurrent - encoderRightPrevious; + traveledLeft += Math.abs(encoderLeftChange); + traveledRight += Math.abs(encoderRightChange); + encoderLeftPrevious = encoderLeftCurrent; encoderRightPrevious = encoderRightCurrent; rotationPrevious = rotation;