diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/Break.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/Break.java new file mode 100644 index 0000000..f8974cd --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/Break.java @@ -0,0 +1,43 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.cyberarm.engine.V2.CyberarmState; + +public class Break extends CyberarmState { + + public DcMotor driveBackLeft; + public DcMotor driveFrontLeft; + public DcMotor driveBackRight; + public DcMotor driveFrontRight; + + @Override + public void init() { + driveFrontLeft = engine.hardwareMap.dcMotor.get("driveFrontLeft"); + driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight"); + driveBackLeft = engine.hardwareMap.dcMotor.get("driveBackLeft"); + driveBackRight = engine.hardwareMap.dcMotor.get("driveBackRight"); + + driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + @Override + public void start() { + + } + + @Override + public void exec() { + driveFrontLeft.setPower(0); + driveFrontRight.setPower(0); + driveBackLeft.setPower(0); + driveBackRight.setPower(0); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java new file mode 100644 index 0000000..a8461f8 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java @@ -0,0 +1,26 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.HardwareTesting.MecanumFunctionTest; +import org.timecrafters.UltimateGoal.Robot; + +@TeleOp (name = "Calibration", group = "test") +public class CalibrationEngine extends CyberarmEngine { + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + super.init(); + } + + + @Override + public void setup() { + addState(new MechanumBiasCalibrator(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java index 131c675..32f5933 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java @@ -9,7 +9,7 @@ import java.util.ArrayList; public class MechanumBiasCalibrator extends CyberarmState { private Robot robot; - private ArrayList Powers; + private ArrayList Powers = new ArrayList<>(); private double BiasFR; private double BiasFL; private double BiasBR; @@ -26,11 +26,17 @@ public class MechanumBiasCalibrator extends CyberarmState { robot.updateLocation(); if (engine.gamepad1.x) { - double[] mecanumPowers = robot.getMecanumPowers(0, 1, 0); + double[] mecanumPowers = robot.getMecanumPowers(-45, 0.5, 0); - //TODO: add motors when they exist. + + robot.driveFrontLeft.setPower(mecanumPowers[0]); + robot.driveFrontRight.setPower(mecanumPowers[1]); + robot.driveBackLeft.setPower(mecanumPowers[2]); + robot.driveBackRight.setPower(mecanumPowers[3]); Powers.add(mecanumPowers); + } else { + robot.setDrivePower(0,0,0,0 ); } if (engine.gamepad1.y && !hasCalculated) { @@ -42,10 +48,10 @@ public class MechanumBiasCalibrator extends CyberarmState { double sumBL = 0; for (double[] powers : Powers) { - sumFR+= powers[0]; - sumFL+= powers[1]; - sumBR+= powers[2]; - sumBL+= powers[3]; + sumFL+= Math.abs(powers[0]); + sumFR+= Math.abs(powers[1]); + sumBL+= Math.abs(powers[2]); + sumBR+= Math.abs(powers[3]); } int length = Powers.size(); @@ -54,9 +60,24 @@ public class MechanumBiasCalibrator extends CyberarmState { BiasBR = sumBR / length; BiasBL = sumBL / length; + double max = Math.max(Math.max(BiasFL,BiasFR),Math.max(BiasBL,BiasBR)); + + BiasFL = BiasFL /max; + BiasFR = BiasFR /max; + BiasBL = BiasBL /max; + BiasBR = BiasBR /max; + } else if (!engine.gamepad1.y) { hasCalculated = false; } } + + @Override + public void telemetry() { + engine.telemetry.addData("FrontLeft", BiasFL); + engine.telemetry.addData("FrontRight", BiasFR); + engine.telemetry.addData("BackLeft", BiasBL); + engine.telemetry.addData("BackRight", BiasBR); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java new file mode 100644 index 0000000..47f3cc5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java @@ -0,0 +1,34 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + +import java.util.ArrayList; + +public class PerformanceTest extends CyberarmState { + + private Robot robot; + private ArrayList angles = new ArrayList(); + + + public PerformanceTest(Robot robot) { + this.robot = robot; + } + + + @Override + public void exec() { + robot.updateLocation(); + robot.record(); + + if (engine.gamepad1.x) { + double[] powers = robot.getMecanumPowers(0, 1, 0); + robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]); + } else { + robot.setDrivePower(0,0,0,0); + } + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java new file mode 100644 index 0000000..55ee1c7 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java @@ -0,0 +1,72 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + +import java.util.ArrayList; + +public class StalPowerCalibrator extends CyberarmState { + + private Robot robot; + private double[] Powers; + private DcMotor[] Motors; + private int currentMotor = 0; + private double currentPower = 0.001; + + + + public StalPowerCalibrator(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight}; + Powers = new double[] {0,0,0,0}; + + for (DcMotor motor : Motors) { + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + } + + @Override + public void exec() { + + if (currentMotor <= 3) { + + + + if (Math.abs(Motors[currentMotor].getCurrentPosition()) < 10 && currentPower < 1) { + currentPower += 0.001; + Motors[currentMotor].setPower(currentPower); + sleep(20); + } else { + Powers[currentMotor] = currentPower; + Motors[currentMotor].setPower(0); + currentPower = 0.001; + currentMotor += 1; + sleep(1000); + for (DcMotor motor : Motors) { + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + } + } + + } + + @Override + public void telemetry() { + + engine.telemetry.addData("current Power", currentPower); + engine.telemetry.addData("current Motor", currentMotor); + + engine.telemetry.addData("FrontLeft", Powers[0]); + engine.telemetry.addData("FrontRight", Powers[1]); + engine.telemetry.addData("BackLeft", Powers[2]); + engine.telemetry.addData("BackRight", Powers[3]); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlerTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlerTest.java new file mode 100644 index 0000000..388316d --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlerTest.java @@ -0,0 +1,66 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + + +public class ControlerTest extends CyberarmState { + + private float leftJoystickDegrees; + private double leftJoystickMagnitude; + private float rightJoystickDegrees; + private double rightJoystickMagnitude; + private double leftJoystickX; + private double leftJoystickY; + private double rightJoystickX; + private double rightJoystickY; + + + + + @Override + public void exec() { + + leftJoystickX = engine.gamepad1.left_stick_x; + leftJoystickY = engine.gamepad1.left_stick_y; + + leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)); + leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY); + + rightJoystickX = engine.gamepad1.left_stick_x; + rightJoystickY = engine.gamepad1.left_stick_y; + + rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)); + rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); + + + + } + + @Override + public void telemetry() { + + engine.telemetry.addLine("Left Joystick"); + engine.telemetry.addData("Pos", "("+leftJoystickX+","+leftJoystickY+")"); + engine.telemetry.addData("Angle", leftJoystickDegrees); + engine.telemetry.addData("Mag", leftJoystickMagnitude); + + engine.telemetry.addLine("Right Joystick"); + engine.telemetry.addData("Pos", "("+rightJoystickX+","+rightJoystickY+")"); + engine.telemetry.addData("Angle", leftJoystickDegrees); + engine.telemetry.addData("Mag", leftJoystickMagnitude); + + engine.telemetry.addLine("Buttons"); + engine.telemetry.addData("a", engine.gamepad1.a); + engine.telemetry.addData("b", engine.gamepad1.b); + engine.telemetry.addData("x", engine.gamepad1.x); + engine.telemetry.addData("y", engine.gamepad1.y); + + engine.telemetry.addLine("Top"); + engine.telemetry.addData("Left Bump", engine.gamepad1.left_bumper); + engine.telemetry.addData("Right Bump", engine.gamepad1.right_bumper); + engine.telemetry.addData("Left Trigger", engine.gamepad1.left_trigger); + engine.telemetry.addData("Right Trigger", engine.gamepad1.right_trigger); + + } +} 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 d137818..1fd562e 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java @@ -12,10 +12,11 @@ public class EncoderTest extends CyberarmState { this.robot = robot; } + @Override public void exec() { - robot.setDrivePower(1, 1,1,1); + robot.setDrivePower(0.1, 0.1,0.1,0.1); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java index d3d599c..f689c29 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java @@ -11,10 +11,10 @@ public class MecanumFunctionTest extends CyberarmState { private double leftJoystickMagnitude; private float rightJoystickDegrees; private double rightJoystickMagnitude; - private double powerFrontLeft; - private double powerFrontRight; - private double powerBackLeft; - private double powerBackRight; + private double powerFrontLeft=0; + private double powerFrontRight=0; + private double powerBackLeft=0; + private double powerBackRight=0; private static double TURN_POWER_SCALE = 0.5; @@ -32,23 +32,25 @@ public class MecanumFunctionTest extends CyberarmState { leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)); leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY); - double rightJoystickX = engine.gamepad1.left_stick_x; - double rightJoystickY = engine.gamepad1.left_stick_y; + double rightJoystickX = engine.gamepad1.right_stick_x; + double rightJoystickY = engine.gamepad1.right_stick_y; rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)); rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); - +// powerFrontLeft = 0; powerFrontRight = 0; powerBackLeft = 0; powerBackRight = 0; if (rightJoystickMagnitude == 0) { - double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, leftJoystickDegrees); - powerFrontLeft = powers[0]; - powerFrontRight = powers[1]; - powerBackLeft = powers[2]; - powerBackRight = powers[3]; + if (leftJoystickMagnitude !=0) { + double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, leftJoystickDegrees); + powerFrontLeft = powers[0]; + powerFrontRight = powers[1]; + powerBackLeft = powers[2]; + powerBackRight = powers[3]; + } } else { if (leftJoystickMagnitude == 0) { double[] powers = robot.getFacePowers(rightJoystickDegrees, rightJoystickMagnitude); @@ -65,6 +67,8 @@ public class MecanumFunctionTest extends CyberarmState { } } +// robot.record(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight); + robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight); } @@ -77,11 +81,8 @@ public class MecanumFunctionTest extends CyberarmState { engine.telemetry.addData("Mag", leftJoystickMagnitude); engine.telemetry.addLine("Right Joystick"); - engine.telemetry.addData("Angle", leftJoystickDegrees); - engine.telemetry.addData("Mag", leftJoystickMagnitude); + engine.telemetry.addData("Angle", rightJoystickDegrees); + engine.telemetry.addData("Mag", rightJoystickMagnitude); -// engine.telemetry.addLine(""); -// engine.telemetry.addData("Front", robot.encoderFront); -// engine.telemetry.addData("Back", robot.encoderBack); } } 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 842d851..7da6410 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -5,7 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.UltimateGoal.Robot; -@TeleOp (name = "Mecanum test", group = "test") +@TeleOp (name = "Encoder test", group = "test") public class TestingEngine extends CyberarmEngine { private Robot robot; @@ -21,11 +21,12 @@ public class TestingEngine extends CyberarmEngine { @Override public void setup() { - addState(new EncoderTest(robot)); + addState(new MecanumFunctionTest(robot)); } - @Override - public void stop() { - robot.deactivateVuforia(); - } +// @Override +// public void stop() { +// robot.saveRecording(); +// super.stop(); +// } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java index 472afa6..267c764 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java @@ -37,7 +37,7 @@ public class DriveToPosition extends CyberarmState { @Override public void exec() { robot.updateLocation(); - robot.record(); +// robot.record(); double distanceRemaining = Math.hypot(targetX - robot.getLocationX(), targetY - robot.getLocationY()); 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 cc2e8be..8fa54b6 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java @@ -48,7 +48,7 @@ public class IMUDrive extends CyberarmState { int ticksTraveled = Math.abs( robot.driveFrontRight.getCurrentPosition()-tickStart); if (ticksTraveled > tickTarget) { - robot.setDrivePower(0,0); +// robot.setDrivePower(0,0); sleep(finishDelay); setHasFinished(true); } else { @@ -62,7 +62,7 @@ public class IMUDrive extends CyberarmState { double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower))); - robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower); +// robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower); } } 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 e00e1be..f86b3cb 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java @@ -54,10 +54,10 @@ public class IMUTurn extends CyberarmState { turnDirection = OptimalDirection; } - robot.setDrivePower(power * turnDirection,-power * turnDirection); +// robot.setDrivePower(power * turnDirection,-power * turnDirection); if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) { - robot.setDrivePower(0,0); +// robot.setDrivePower(0,0); setHasFinished(true); } 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 4593937..23ba2c4 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java @@ -24,7 +24,7 @@ public class VuforiaNavTesting extends CyberarmState { } robot.updateLocation(); - robot.record(); +// robot.record(); double joystickX = engine.gamepad1.right_stick_x; double joystickY = engine.gamepad1.right_stick_y; diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index 05e27d5..0f4b0bc 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -57,6 +57,9 @@ public class Robot { static final double BIAS_BACK_LEFT = 1; static final double BIAS_BACK_RIGHT = 1; + static final double FINE_CORRECTION = 0.01; + static final double LARGE_CORRECTION = 0.01 ; + //Conversion Constants static final double ENCODER_CIRCUMFERENCE = Math.PI * 4; static final int COUNTS_PER_REVOLUTION = 8192; @@ -78,7 +81,8 @@ public class Robot { public double visionX; public double visionY; public float rawAngle; - private String TestingRecord = "X,Y,Angle"; +// private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight"; + private String TestingRecord = "Rotation"; public double traveledLeft; public double traveledRight; @@ -101,7 +105,7 @@ public class Robot { public void initHardware() { - webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); +// webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); imu = hardwareMap.get(BNO055IMU.class, "imu"); @@ -110,15 +114,13 @@ public class Robot { driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft"); driveBackRight = hardwareMap.dcMotor.get("driveBackRight"); - driveFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - driveFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - driveFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - driveFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); - + driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); @@ -129,7 +131,7 @@ public class Robot { imu.initialize(parameters); - initVuforia(); +// initVuforia(); rotation = stateConfiguration.variable("system", "startPos", "direction").value(); locationX = stateConfiguration.variable("system", "startPos", "x").value(); @@ -197,6 +199,7 @@ public class Robot { tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single"); } + //run this in every exec to track the robot's location. public void updateLocation(){ // orientation is inverted to have clockwise be positive. float imuAngle = -imu.getAngularOrientation().firstAngle; @@ -286,12 +289,14 @@ public class Robot { return locationY; } + //Manually set the position of the robot on the field. public void setCurrentPosition(float rotation, double x, double y) { this.rotation = rotation; locationX = x; locationY = y; } + //returns the angle from the robot's current position to the given target position. public float getAngleToPosition (double x, double y) { double differenceX = x- getLocationX(); double differenceY = y- getLocationY(); @@ -301,6 +306,7 @@ public class Robot { } + //Unit conversion public double ticksToInches(double ticks) { return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION); } @@ -310,6 +316,9 @@ public class Robot { } + //Returns the angle between two angles, with positive angles indicating that the reference is + //to the right (clockwise) of the current. Negative angles indicate that the reference is to the + //left. public float getRelativeAngle(float reference, float current) { float relative = current - reference; @@ -326,13 +335,15 @@ public class Robot { //Drive Functions public void setDrivePower(double powerFrontLeft, double powerFrontRight, double powerBackLeft, double powerBackRight){ driveFrontLeft.setPower(powerFrontLeft * BIAS_FRONT_LEFT); - driveFrontRight.setPower(powerFrontLeft * BIAS_FRONT_RIGHT); + driveFrontRight.setPower(powerFrontRight * BIAS_FRONT_RIGHT); driveBackLeft.setPower(powerBackLeft * BIAS_BACK_LEFT); - driveBackLeft.setPower(powerBackRight * BIAS_BACK_RIGHT); + driveBackRight.setPower(powerBackRight * BIAS_BACK_RIGHT); } + //returns an array of the powers necessary to execute the provided motion. The order of the motors + //is ForwardLeft, ForwardRight, BackLeft, BackRight public double[] getMecanumPowers(float degreesDirectionMotion, double scalar, float degreesDirectionFace) { - double rad = Math.toRadians(degreesDirectionMotion); + double rad = Math.toRadians(getRelativeAngle(degreesDirectionFace,degreesDirectionMotion)); double y = scalar * Math.cos(rad); double x = scalar * Math.sin(rad); @@ -341,12 +352,12 @@ public class Robot { double q = y - x; float relativeRotation = getRelativeAngle(degreesDirectionFace, rotation); - double turnCorrection = Math.pow(0.03 * relativeRotation, 3) + 0.02 * relativeRotation; + double turnCorrection = Math.pow(LARGE_CORRECTION * relativeRotation, 3) + FINE_CORRECTION * relativeRotation; - double powerForwardRight = q - turnCorrection; - double powerForwardLeft = p + turnCorrection; - double powerBackRight = p - turnCorrection; - double powerBackLeft = q + turnCorrection; + double powerForwardRight = q + turnCorrection; + double powerForwardLeft = p - turnCorrection; + double powerBackRight = p + turnCorrection; + double powerBackLeft = q - turnCorrection; // The "extreme" is the power value that is furthest from zero. When this values exceed the @@ -369,14 +380,19 @@ public class Robot { return powers; } + // public double[] getFacePowers(float direction, double power) { - double left = power; - double right = -power; + double relativeAngle = getRelativeAngle(direction, rotation); + double scaler = Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle; - if (getRelativeAngle(direction, rotation) > 0) { - left *= -1; - right *= -1; - } + double left = -power * scaler; + double right = power *scaler; + + +// if (relativeAngle > 0) { +// left *= -1; +// right *= -1; +// } double[] powers = {left,right}; return powers; } @@ -406,8 +422,12 @@ public class Robot { } //Data Recording +// public void record(double frontLeft, double frontRight, double backLeft, double backRight) { +// TestingRecord+="\n"+frontLeft+","+frontRight+","+backLeft+","+backRight; +// } + public void record() { - TestingRecord+="\n"+locationX+","+locationY+","+rotation; + TestingRecord+="\n"+rotation; } public void saveRecording() {