From 9bef2bec90fb9bf843d417dd3c06646e35433457 Mon Sep 17 00:00:00 2001 From: RobotNRP Date: Sat, 3 Oct 2020 11:41:46 -0500 Subject: [PATCH 1/3] Added unit conversion functions and made robot object functional with minibot modifications. --- .idea/misc.xml | 2 +- .../HardwareTesting/EncoderTest.java | 12 ++++++------ .../org/timecrafters/UltimateGoal/Robot.java | 17 ++++++++++++++--- 3 files changed, 21 insertions(+), 10 deletions(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index 7bfef59..37a7509 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,6 +1,6 @@ - + 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/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index 72f029b..05dc3cf 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -25,8 +25,11 @@ 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 * 3.8; + int Counts_Per_Revolution = 8192; //Robot Localizatoin private double locationX; @@ -46,7 +49,7 @@ public class Robot { // encoderBack = hardwareMap.dcMotor.get("encoderBack"); encoderRight = hardwareMap.dcMotor.get("encoderRight"); - encoderLeft.setDirection(DcMotorSimple.Direction.REVERSE); + encoderRight.setDirection(DcMotorSimple.Direction.REVERSE); encoderLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); encoderRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -100,4 +103,12 @@ 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); + } } From 3556ecf9b8086f58c45dbab3ab1785a5fec93e95 Mon Sep 17 00:00:00 2001 From: RobotNRP Date: Tue, 6 Oct 2020 19:12:54 -0500 Subject: [PATCH 2/3] Added IMU assisted motion states for Localization testing. Fixed bug in ActionConfig --- TeamCode/build.gradle | 1 + .../LocalizerTesting/IMUDrive.java | 64 ++++++++++++++++++ .../LocalizerTesting/IMUTurn.java | 66 +++++++++++++++++++ .../LocalizerTestingEngine.java | 24 +++++++ .../org/timecrafters/UltimateGoal/Robot.java | 18 ++++- .../TimeCraftersConfiguration.java | 2 +- 6 files changed, 172 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java 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/LocalizerTesting/IMUDrive.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java new file mode 100644 index 0000000..f981830 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java @@ -0,0 +1,64 @@ +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; + + 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(); + } + + @Override + public void start() { + angleTarget=robot.getRotation(); + } + + @Override + public void exec() { + + robot.updateLocation(); + + if (Math.abs(robot.encoderRight.getCurrentPosition()) > tickTarget) { + robot.encoderRight.setPower(0); + robot.encoderLeft.setPower(0); + 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.encoderRight.setPower(rightPower * powerAdjust); + robot.encoderLeft.setPower(leftPower * powerAdjust); + } + } +} 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..80451c5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java @@ -0,0 +1,66 @@ +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 double 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 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; + } + + if (!useOptimalDirection && OptimalDirection == turnDirection){ + useOptimalDirection = true; + } + + if (useOptimalDirection){ + turnDirection = OptimalDirection; + } + + robot.encoderLeft.setPower(-power * turnDirection); + robot.encoderRight.setPower(power * turnDirection); + + if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) { + robot.encoderRight.setPower(0); + robot.encoderLeft.setPower(0); + setHasFinished(true); + } + + } +} 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..4056a5d --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java @@ -0,0 +1,24 @@ +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")); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index 05dc3cf..71a8823 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 @@ -92,7 +93,7 @@ public class Robot { } - public double getRotation() { + public float getRotation() { return rotation; } @@ -111,4 +112,17 @@ public class Robot { 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())) { From 67b83d3ace4c5b600d704f984e4346371b359b39 Mon Sep 17 00:00:00 2001 From: RobotNRP Date: Sat, 10 Oct 2020 11:44:18 -0500 Subject: [PATCH 3/3] Fine tuned the diamiter value for unit conversions. Added ability to disable IMUDrive and IMUTurn with the configuration tool. --- .../LocalizerTesting/IMUDrive.java | 32 ++++++++++-- .../LocalizerTesting/IMUTurn.java | 52 +++++++++++++------ .../LocalizerTestingEngine.java | 3 ++ .../org/timecrafters/UltimateGoal/Robot.java | 17 +++++- 4 files changed, 81 insertions(+), 23 deletions(-) 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;