diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java index 98f1784..52ac74f 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -106,6 +106,8 @@ public abstract class CyberarmEngine extends OpMode { for (CyberarmState state: cyberarmStates) { stopState(state); } + + } /** diff --git a/TeamCode/src/main/java/org/samples/ConceptTensorFlowObjectDetectionWebcam.java b/TeamCode/src/main/java/org/samples/ConceptTensorFlowObjectDetectionWebcam.java index 6e1cdf8..490db04 100644 --- a/TeamCode/src/main/java/org/samples/ConceptTensorFlowObjectDetectionWebcam.java +++ b/TeamCode/src/main/java/org/samples/ConceptTensorFlowObjectDetectionWebcam.java @@ -145,18 +145,17 @@ public class ConceptTensorFlowObjectDetectionWebcam extends LinearOpMode { * Initialize the Vuforia localization engine. */ private void initVuforia() { - /* - * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. - */ - VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); - parameters.vuforiaLicenseKey = VUFORIA_KEY; - parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1"); + parameters.vuforiaLicenseKey = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah"; + parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");; + parameters.useExtendedTracking = false; - // Instantiate the Vuforia engine vuforia = ClassFactory.getInstance().createVuforia(parameters); + // Loading trackables is not necessary for the TensorFlow Object Detection engine. } @@ -170,5 +169,6 @@ public class ConceptTensorFlowObjectDetectionWebcam extends LinearOpMode { tfodParameters.minResultConfidence = 0.8f; tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java index 6ef9a23..db6769d 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -21,7 +21,7 @@ public class TestingEngine extends CyberarmEngine { @Override public void setup() { - addState(new LauncherTest()); + addState(new WelcomeToJankyTown()); } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WelcomeToJankyTown.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WelcomeToJankyTown.java new file mode 100644 index 0000000..c74a7fa --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WelcomeToJankyTown.java @@ -0,0 +1,30 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; + + +public class WelcomeToJankyTown extends CyberarmState { + + DcMotor left; + DcMotor right; + + @Override + public void init() { + left = engine.hardwareMap.dcMotor.get("left"); + right = engine.hardwareMap.dcMotor.get("right"); + } + + @Override + public void exec() { + left.setPower(-engine.gamepad1.left_stick_y); + right.setPower(engine.gamepad1.right_stick_y); + } + + @Override + public void telemetry() { + engine.telemetry.addLine("Welcome to Janky Town!"); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java new file mode 100644 index 0000000..afa44a4 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java @@ -0,0 +1,66 @@ +package org.timecrafters.UltimateGoal.LocalizerTesting; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + +public class DriveToPosition extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private double targetX; + private double targetY; + private double translation; + private double tolerance; + private double power; + + public DriveToPosition(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void init() { +// translation = robot.inchesToTicks(4); + targetX = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"x").value()); + targetY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"y").value()); + tolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"tolerance").value()); + power = robot.stateConfiguration.variable(groupName,actionName,"power").value(); + } + + @Override + public void start() { + + } + + @Override + public void exec() { + robot.updateLocation(); + robot.record(); + + double distanceRemaining = Math.hypot(targetX - robot.getLocationX(), targetY - robot.getLocationY()); + + //Math.pow(3, distanceRemaining - translation) + 0.3; + +// if (power > 0.65) { +// power = 0.65; +// } + + if (distanceRemaining < tolerance) { + robot.setDrivePower(0, 0); + setHasFinished(true); + } else { + robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power); + } + + } + + @Override + public void telemetry() { + engine.telemetry.addData("Target Visible", robot.trackableVisible); + engine.telemetry.addData("Odo X", robot.ticksToInches(robot.getLocationX())); + engine.telemetry.addData("Odo Y", robot.ticksToInches(robot.getLocationY())); + 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 c1f9486..3096b11 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java @@ -21,6 +21,15 @@ public class LocalizerTestingEngine extends CyberarmEngine { @Override public void setup() { addState(new VuforiaNavTesting(robot)); +// addState(new DriveToPosition(robot, "DtoP", "d1")); +// addState(new DriveToPosition(robot, "DtoP", "d2")); } + + @Override + public void stop() { + super.stop(); + robot.saveRecording(); + robot.deactivateVuforia(); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java index ec93568..4593937 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java @@ -10,28 +10,36 @@ public class VuforiaNavTesting extends CyberarmState { private float angle; private double X; private double Y; - private double targetX; - private double targetY; + private float joystickDegrees; + private double power; public VuforiaNavTesting(Robot robot) { this.robot = robot; } - @Override - public void init() { - targetX = robot.inchesToTicks(24); - targetY = robot.inchesToTicks(24); - } - @Override public void exec() { - robot.setDrivePower(-0.5 * engine.gamepad1.left_stick_y, -0.5 * engine.gamepad1.right_stick_y); + if (engine.gamepad1.x) { + robot.syncWithVuforia(); + } + robot.updateLocation(); + robot.record(); + + double joystickX = engine.gamepad1.right_stick_x; + double joystickY = engine.gamepad1.right_stick_y; + + joystickDegrees = (float) Math.toDegrees(Math.atan2(joystickX, -joystickY)); + power = 0.3 * Math.hypot(joystickX, joystickY); + + robot.driveAtAngle(joystickDegrees, power); + X = robot.getLocationX(); Y = robot.getLocationY(); angle = robot.getRotation(); + } @Override @@ -40,8 +48,11 @@ public class VuforiaNavTesting extends CyberarmState { engine.telemetry.addData("Odo X", robot.ticksToInches(X)); engine.telemetry.addData("Odo Y", robot.ticksToInches(Y)); + engine.telemetry.addData("Joystick", joystickDegrees); +// engine.telemetry.addData("Vis X", robot.ticksToInches(robot.visionX)); +// engine.telemetry.addData("Vis Y", robot.ticksToInches(robot.visionY)); + engine.telemetry.addData("Robot Angle", angle); - engine.telemetry.addData("Raw Angle", robot.rawAngle); - engine.telemetry.addData("Angle to Target", robot.getAngleToPosition(targetX,targetY)); } + } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index d778ac3..5cc0e40 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -1,6 +1,8 @@ package org.timecrafters.UltimateGoal; import android.app.Activity; +import android.os.Environment; +import android.util.Log; import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.DcMotor; @@ -19,7 +21,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefau import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration; +import org.timecrafters.TimeCraftersConfigurationTool.backend.Backend; +import org.timecrafters.TimeCraftersConfigurationTool.backend.TAC; +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; import java.util.ArrayList; import java.util.List; @@ -57,20 +64,21 @@ public class Robot { static final float mmPerInch = 25.4f; // Inches Forward of axis of rotation - static final float CAMERA_FORWARD_DISPLACEMENT = 4.25f; + static final float CAMERA_FORWARD_DISPLACEMENT = 13f; // Inches above Ground static final float CAMERA_VERTICAL_DISPLACEMENT = 4.5f; // Inches Left of axis of rotation static final float CAMERA_LEFT_DISPLACEMENT = 2f; //Robot Localization - private double locationX; - private double locationY; + public double locationX; + public double locationY; private float rotation; public double visionX; public double visionY; public float rawAngle; + private String TestingRecord = "X,Y,Angle"; public double traveledLeft; @@ -122,6 +130,10 @@ public class Robot { imu.initialize(parameters); initVuforia(); + + rotation = stateConfiguration.variable("system", "startPos", "direction").value(); + locationX = stateConfiguration.variable("system", "startPos", "x").value(); + locationY = stateConfiguration.variable("system", "startPos", "y").value(); } private void initVuforia() { @@ -192,7 +204,7 @@ public class Robot { public void updateLocation(){ // orientation is inverted to have clockwise be positive. float imuAngle = -imu.getAngularOrientation().firstAngle; - float rotationChange = imuAngle - rotationPrevious; + double rotationChange = imuAngle - rotationPrevious; if (rotationChange > 180) { rotationChange -= 360; @@ -222,17 +234,22 @@ public class Robot { locationX += xChange; locationY += yChange; - //TODO : add separate odometer and vision coordinates. - //TODO : make Odometer Coordinates set to vision coordinates on button push. + if (rotation > 180) { + rotation -= 360; + } + if (rotation < -180) { + rotation += 360; + } + } + + public void syncWithVuforia() { trackableVisible = false; for (VuforiaTrackable trackable : trackables) { if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) { OpenGLMatrix robotLocation = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); - - //this is used for debugging purposes. trackableVisible = true; @@ -244,8 +261,7 @@ public class Robot { VectorF translation = lastConfirmendLocation.getTranslation(); locationX = -inchesToTicks(translation.get(1) / mmPerInch); locationY = inchesToTicks( translation.get(0) / mmPerInch); -// visionX = -translation.get(1) / mmPerInch; -// visionY = translation.get(0) / mmPerInch; + //For our tournament, it makes sense to make zero degrees towards the goal. @@ -260,14 +276,6 @@ public class Robot { break; } } - - if (rotation > 180) { - rotation -= 360; - } - if (rotation < -180) { - rotation += 360; - } - } public float getRotation() { @@ -282,6 +290,12 @@ public class Robot { return locationY; } + public void setCurrentPosition(float rotation, double x, double y) { + this.rotation = rotation; + locationX = x; + locationY = y; + } + public float getAngleToPosition (double x, double y) { double differenceX = x- getLocationX(); double differenceY = y- getLocationY(); @@ -313,7 +327,53 @@ public class Robot { return relative; } - public void deactivateVuforia() { + public void driveAtAngle(float angle, double power) { + double relativeAngle = getRelativeAngle(angle, getRotation()); + + //calculate how the power of each motor should be adjusted to make the robot curve + //towards the target angle + //-------------------------------------------------------------------------------------- + + double turnPowerCorrection = Math.pow(0.03 * relativeAngle, 3) + 0.02 * relativeAngle; + + //Adjusts power based on degrees off from target. + double leftPower = power - turnPowerCorrection; + double rightPower = power + turnPowerCorrection; + //-------------------------------------------------------------------------------------- + + + //calculates speed adjuster that slows the motors to be closer to the BasePower while + // maintaining the power ratio nesesary to execute the turn. + double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower))); + + setDrivePower(leftPower * powerAdjust, rightPower * powerAdjust); + } + + public void record() { + TestingRecord+="\n"+locationX+","+locationY+","+rotation; + } + + public void saveRecording() { + writeToFile(Environment.getExternalStorageDirectory().getAbsolutePath()+File.separator+"TimeCrafters_TestingRecord"+File.separator+"RobotTestingRecord.txt", TestingRecord); + } + + public boolean writeToFile(String filePath, String content) { + try { + + FileWriter writer = new FileWriter(filePath); + writer.write(content); + writer.close(); + + return true; + + } catch (IOException e) { + Log.e("RecordTest", e.getLocalizedMessage()); + return false; + } + } + + public void deactivateVuforia() { + targetsUltimateGoal.deactivate(); } }