diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index d487070..86c342f 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -20,4 +20,5 @@ repositories { dependencies { annotationProcessor files('lib/OpModeAnnotationProcessor.jar') + implementation project(path: ':TimeCraftersConfigurationTool') } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java index e3e7739..2fae268 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java @@ -22,7 +22,7 @@ public class EncoderTest extends CyberarmState { robot.setDrivePower(-0.2 * engine.gamepad1.left_stick_y, -0.2 * engine.gamepad1.right_stick_y); - ticksLeft=robot.encoderLeft.getCurrentPosition(); + ticksLeft= robot.encoderLeft.getCurrentPosition(); ticksRight=robot.encoderRight.getCurrentPosition(); @@ -50,16 +50,16 @@ public class EncoderTest extends CyberarmState { @Override public void telemetry() { - engine.telemetry.addData("controler", engine.gamepad1.left_stick_y); + engine.telemetry.addData("controller", engine.gamepad1.left_stick_y); engine.telemetry.addLine("Latency Values"); - engine.telemetry.addData("Y", robot.getLocationY()); - engine.telemetry.addData("X", robot.getLocationX()); + 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("Actual Values"); - engine.telemetry.addData("Left", ticksLeft); - engine.telemetry.addData("Right", ticksRight); + engine.telemetry.addData("Left", robot.ticksToInches(ticksLeft)); + engine.telemetry.addData("Right", robot.ticksToInches(ticksRight)); // engine.telemetry.addLine(""); // engine.telemetry.addData("Front", robot.encoderFront); diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java new file mode 100644 index 0000000..aa004fb --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java @@ -0,0 +1,86 @@ +package org.timecrafters.UltimateGoal.LocalizerTesting; + +import android.util.Log; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.TimeCraftersConfigurationTool.backend.Backend; +import org.timecrafters.UltimateGoal.Robot; + +public class IMUDrive extends CyberarmState { + + private Robot robot; + private String actionName; + private String groupName; + private double power; + 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; + this.actionName = actionName; + this.groupName = groupName; + } + + @Override + public void init() { + Log.i("Config", Backend.instance().gsonForConfig().toJson(robot.stateConfiguration.getConfig())); + + power = robot.stateConfiguration.variable(groupName, actionName, "power").value(); + 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 + public void exec() { + + robot.updateLocation(); + + int ticksTraveled = Math.abs( robot.encoderRight.getCurrentPosition()-tickStart); + if (ticksTraveled > tickTarget) { + robot.setDrivePower(0,0); + sleep(finishDelay); + setHasFinished(true); + } else { + + angleRelative = robot.getRelativeAngle(angleTarget, robot.getRotation()); + + double turnPowerCorrection = Math.pow(0.03 * angleRelative, 3) + 0.01 * angleRelative; + + double rightPower = power + turnPowerCorrection; + double leftPower = power - turnPowerCorrection; + + double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower))); + + 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 new file mode 100644 index 0000000..1784f2b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java @@ -0,0 +1,86 @@ +package org.timecrafters.UltimateGoal.LocalizerTesting; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + +public class IMUTurn extends CyberarmState { + + private Robot robot; + private String actionName; + private String groupName; + private double power; + private float angleTarget; + private int turnDirection; + private float angleAllowance; + private boolean useOptimalDirection; + + public IMUTurn(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.actionName = actionName; + this.groupName = groupName; + } + + @Override + public void init() { + power = robot.stateConfiguration.variable(groupName,actionName, "power").value(); + angleTarget = robot.stateConfiguration.variable(groupName,actionName, "angle").value(); + angleAllowance = robot.stateConfiguration.variable(groupName,actionName, "allowance").value(); + turnDirection = robot.stateConfiguration.variable(groupName, actionName, "direction").value(); + useOptimalDirection = (turnDirection==0); + } + + @Override + public void start() { + if (!robot.stateConfiguration.action(groupName,actionName).enabled) { + setHasFinished(true); + } + } + + @Override + public void exec() { + robot.updateLocation(); + + int OptimalDirection = getOptimalDirection(angleTarget, robot.getRotation()); + + if (!useOptimalDirection && OptimalDirection == turnDirection){ + useOptimalDirection = true; + } + + if (useOptimalDirection){ + turnDirection = OptimalDirection; + } + + robot.setDrivePower(power * turnDirection,-power * turnDirection); + + if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) { + 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 new file mode 100644 index 0000000..6e83c82 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java @@ -0,0 +1,27 @@ +package org.timecrafters.UltimateGoal.LocalizerTesting; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Robot; + +@Autonomous (name = "Localizer Test") +public class LocalizerTestingEngine extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + super.init(); + } + + @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 72f029b..2eae4fe 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import org.cyberarm.NeXT.StateConfiguration; +import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration; public class Robot { @@ -15,7 +16,7 @@ public class Robot { this.hardwareMap = hardwareMap; } - public StateConfiguration stateConfiguration = new StateConfiguration(); + public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration(); public BNO055IMU imu; //drive system @@ -25,14 +26,20 @@ public class Robot { public DcMotor encoderBack; public DcMotor encoderRight; - double BIAS_LEFT = 1.0; - double BIAS_RIGHT = 0.87; + double BIAS_LEFT = -1.0; + double BIAS_RIGHT = -0.87; + + double Circumference_Encoder = Math.PI * 4; + int Counts_Per_Revolution = 8192; //Robot Localizatoin private double locationX; private double locationY; private float rotation; + public double traveledLeft; + public double traveledRight; + private int encoderFrontPrevious = 0; private int encoderLeftPrevious = 0; private int encoderBackPrevious = 0; @@ -46,7 +53,12 @@ public class Robot { // encoderBack = hardwareMap.dcMotor.get("encoderBack"); encoderRight = hardwareMap.dcMotor.get("encoderRight"); - encoderLeft.setDirection(DcMotorSimple.Direction.REVERSE); + 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); encoderRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -68,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; @@ -89,7 +106,7 @@ public class Robot { } - public double getRotation() { + public float getRotation() { return rotation; } @@ -100,4 +117,25 @@ public class Robot { public double getLocationY() { return locationY; } + + public double ticksToInches(double ticks) { + return ticks * (Circumference_Encoder / Counts_Per_Revolution); + } + + public double inchesToTicks(double inches) { + return inches * (Counts_Per_Revolution / Circumference_Encoder); + } + + public float getRelativeAngle(float reference, float current) { + float relative = current - reference; + + if (relative < -180) { + relative += 360; + } + + if (relative > 180) { + relative -= 360; + } + return relative; + } } diff --git a/TimeCraftersConfigurationTool/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/TimeCraftersConfiguration.java b/TimeCraftersConfigurationTool/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/TimeCraftersConfiguration.java index ce41fd5..38ecd1f 100644 --- a/TimeCraftersConfigurationTool/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/TimeCraftersConfiguration.java +++ b/TimeCraftersConfigurationTool/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/TimeCraftersConfiguration.java @@ -70,7 +70,7 @@ public class TimeCraftersConfiguration { } public Variable variable(String groupName, String actionName, String variableName) { - final Action action = action(groupName, groupName); + final Action action = action(groupName, actionName); for (Variable variable : action.getVariables()) { if (variable.name.trim().equals(variableName.trim())) {