diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java new file mode 100644 index 0000000..131c675 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java @@ -0,0 +1,62 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + +import java.lang.reflect.Array; +import java.util.ArrayList; + +public class MechanumBiasCalibrator extends CyberarmState { + + private Robot robot; + private ArrayList Powers; + private double BiasFR; + private double BiasFL; + private double BiasBR; + private double BiasBL; + private boolean hasCalculated; + + + public MechanumBiasCalibrator(Robot robot) { + this.robot = robot; + } + + @Override + public void exec() { + robot.updateLocation(); + + if (engine.gamepad1.x) { + double[] mecanumPowers = robot.getMecanumPowers(0, 1, 0); + + //TODO: add motors when they exist. + + Powers.add(mecanumPowers); + } + + if (engine.gamepad1.y && !hasCalculated) { + hasCalculated = true; + + double sumFR = 0; + double sumFL = 0; + double sumBR = 0; + double sumBL = 0; + + for (double[] powers : Powers) { + sumFR+= powers[0]; + sumFL+= powers[1]; + sumBR+= powers[2]; + sumBL+= powers[3]; + } + + int length = Powers.size(); + BiasFR = sumFR / length; + BiasFL = sumFL / length; + BiasBR = sumBR / length; + BiasBL = sumBL / length; + + } else if (!engine.gamepad1.y) { + hasCalculated = false; + } + + } +} 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 2fae268..d137818 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java @@ -7,10 +7,6 @@ import org.timecrafters.UltimateGoal.Robot; public class EncoderTest extends CyberarmState { private Robot robot; - private int ticksLeft; - private int ticksRight; - private double biasLeft = 0; - private double biasRight = 0; public EncoderTest(Robot robot) { this.robot = robot; @@ -18,51 +14,8 @@ public class EncoderTest extends CyberarmState { @Override public void exec() { - robot.updateLocation(); - robot.setDrivePower(-0.2 * engine.gamepad1.left_stick_y, -0.2 * engine.gamepad1.right_stick_y); + robot.setDrivePower(1, 1,1,1); - ticksLeft= robot.encoderLeft.getCurrentPosition(); - ticksRight=robot.encoderRight.getCurrentPosition(); - - -// if (runTime() < 3000) { -// -// -// -// -// } else { -// robot.encoderLeft.setPower(0.0); -// robot.encoderRight.setPower(0.0); -// -// double ticksExtreme; -// if (Math.abs(ticksLeft) < Math.abs(ticksRight)) { -// ticksExtreme = ticksLeft; -// } else { -// ticksExtreme = ticksRight; -// } -// biasLeft = ticksExtreme/ticksLeft; -// biasRight = ticksExtreme/ticksRight; -// } - - } - - @Override - public void telemetry() { - - engine.telemetry.addData("controller", engine.gamepad1.left_stick_y); - engine.telemetry.addLine("Latency 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("Actual Values"); - engine.telemetry.addData("Left", robot.ticksToInches(ticksLeft)); - engine.telemetry.addData("Right", robot.ticksToInches(ticksRight)); - -// 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/MecanumFunctionTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java new file mode 100644 index 0000000..3dc46de --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java @@ -0,0 +1,87 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + + +public class MecanumFunctionTest extends CyberarmState { + + private Robot robot; + private float leftJoystickDegrees; + private double leftJoystickMagnitude; + private float rightJoystickDegrees; + private double rightJoystickMagnitude; + private double powerFrontLeft; + private double powerFrontRight; + private double powerBackLeft; + private double powerBackRight; + + private static double TURN_POWER_SCALE = 0.5; + + public MecanumFunctionTest(Robot robot) { + this.robot = robot; + } + + @Override + public void exec() { + robot.updateLocation(); + + double leftJoystickX = engine.gamepad1.left_stick_x; + double leftJoystickY = engine.gamepad1.left_stick_y; + + 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; + + 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]; + } else { + if (leftJoystickMagnitude == 0) { + double[] powers = robot.getFacePowers(rightJoystickDegrees, rightJoystickMagnitude); + powerFrontLeft = TURN_POWER_SCALE * powers[0]; + powerFrontRight = TURN_POWER_SCALE * powers[1]; + powerBackLeft = TURN_POWER_SCALE * powers[0]; + powerBackRight = TURN_POWER_SCALE * powers[1]; + } else { + double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, rightJoystickDegrees); + powerFrontLeft = powers[0]; + powerFrontRight = powers[1]; + powerBackLeft = powers[2]; + powerBackRight = powers[3]; + } + } + + robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight); + + } + + @Override + public void telemetry() { + + engine.telemetry.addLine("Left Joystick"); + engine.telemetry.addData("Angle", leftJoystickDegrees); + engine.telemetry.addData("Mag", leftJoystickMagnitude); + + engine.telemetry.addLine("Right Joystick"); + engine.telemetry.addData("Angle", leftJoystickDegrees); + engine.telemetry.addData("Mag", leftJoystickMagnitude); + +// 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 db6769d..842d851 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -5,24 +5,27 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.UltimateGoal.Robot; -@TeleOp (name = "Encoder test", group = "test") +@TeleOp (name = "Mecanum test", group = "test") public class TestingEngine extends CyberarmEngine { private Robot robot; -// @Override -// public void init() { -// robot = new Robot(hardwareMap); -// robot.initHardware(); -// super.init(); -// } + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + super.init(); + } @Override public void setup() { - addState(new WelcomeToJankyTown()); + addState(new EncoderTest(robot)); } - + @Override + public void stop() { + robot.deactivateVuforia(); + } } 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 afa44a4..472afa6 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java @@ -48,7 +48,7 @@ public class DriveToPosition extends CyberarmState { // } if (distanceRemaining < tolerance) { - robot.setDrivePower(0, 0); + robot.setDrivePower(0, 0, 0, 0); setHasFinished(true); } else { robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power); 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 7961f89..cc2e8be 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java @@ -1,9 +1,6 @@ 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 { @@ -41,7 +38,7 @@ public class IMUDrive extends CyberarmState { } angleTarget=robot.getRotation(); - tickStart = robot.encoderRight.getCurrentPosition(); + tickStart = robot.driveFrontRight.getCurrentPosition(); } @Override @@ -49,7 +46,7 @@ public class IMUDrive extends CyberarmState { robot.updateLocation(); - int ticksTraveled = Math.abs( robot.encoderRight.getCurrentPosition()-tickStart); + int ticksTraveled = Math.abs( robot.driveFrontRight.getCurrentPosition()-tickStart); if (ticksTraveled > tickTarget) { robot.setDrivePower(0,0); sleep(finishDelay); diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index 5cc0e40..3a575bf 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -1,6 +1,5 @@ package org.timecrafters.UltimateGoal; -import android.app.Activity; import android.os.Environment; import android.util.Log; @@ -14,15 +13,12 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.robotcore.external.navigation.Orientation; -import org.firstinspires.ftc.robotcore.external.navigation.VuforiaBase; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; 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; @@ -50,13 +46,16 @@ public class Robot { private VuforiaLocalizer vuforia; //drive system - public DcMotor encoderFront; - public DcMotor encoderLeft; - public DcMotor encoderBack; - public DcMotor encoderRight; - static final double BIAS_LEFT = -1.0; - static final double BIAS_RIGHT = -0.87; + public DcMotor driveBackLeft; + public DcMotor driveFrontLeft; + public DcMotor driveBackRight; + public DcMotor driveFrontRight; + + static final double BIAS_FRONT_LEFT = 1; + static final double BIAS_FRONT_RIGHT = 1; + static final double BIAS_BACK_LEFT = 1; + static final double BIAS_BACK_RIGHT = 1; //Conversion Constants static final double ENCODER_CIRCUMFERENCE = Math.PI * 4; @@ -84,7 +83,7 @@ public class Robot { public double traveledLeft; public double traveledRight; - private int encoderFrontPrevious = 0; + private int encoderLeftPrevious = 0; private int encoderBackPrevious = 0; private int encoderRightPrevious = 0; @@ -105,20 +104,21 @@ public class Robot { webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); imu = hardwareMap.get(BNO055IMU.class, "imu"); -// encoderFront = hardwareMap.dcMotor.get("encoderFront"); - encoderLeft = hardwareMap.dcMotor.get("encoderLeft"); -// 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); + driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft"); + driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight"); + driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft"); + driveBackRight = hardwareMap.dcMotor.get("driveBackRight"); - encoderRight.setDirection(DcMotorSimple.Direction.REVERSE); + 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); - encoderLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - encoderRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); @@ -184,7 +184,9 @@ public class Robot { targetsUltimateGoal.activate(); } - //TODO : Test range of Tensor Object identification. + public void deactivateVuforia() { + targetsUltimateGoal.deactivate(); + } private void initTensorFlow() { int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( @@ -195,12 +197,6 @@ public class Robot { tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single"); } - public void setDrivePower(double powerLeft, double powerRight){ - encoderLeft.setPower(powerLeft * BIAS_LEFT); - encoderRight.setPower(powerRight * BIAS_RIGHT); - - } - public void updateLocation(){ // orientation is inverted to have clockwise be positive. float imuAngle = -imu.getAngularOrientation().firstAngle; @@ -213,8 +209,8 @@ public class Robot { rotationChange += 360; } - int encoderLeftCurrent = encoderLeft.getCurrentPosition(); - int encoderRightCurrent = encoderRight.getCurrentPosition(); + int encoderLeftCurrent = driveFrontLeft.getCurrentPosition(); + int encoderRightCurrent = driveFrontRight.getCurrentPosition(); double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious; double encoderRightChange = encoderRightCurrent - encoderRightPrevious; @@ -327,6 +323,65 @@ public class Robot { return relative; } + //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); + driveBackLeft.setPower(powerBackLeft * BIAS_BACK_LEFT); + driveBackLeft.setPower(powerBackRight * BIAS_BACK_RIGHT); + } + + public double[] getMecanumPowers(float degreesDirectionMotion, double scalar, float degreesDirectionFace) { + double rad = Math.toRadians(degreesDirectionMotion); + double y = scalar * Math.cos(rad); + double x = scalar * Math.sin(rad); + + //TODO: Try swapping p and q if left and right seam incorrect + double p = y + x; + double q = y - x; + + float relativeRotation = getRelativeAngle(degreesDirectionFace, rotation); + double turnCorrection = Math.pow(0.03 * relativeRotation, 3) + 0.02 * relativeRotation; + + 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 + // -1 to 1 power range, dividing the powers by the "extreme" scales everything back into the + // workable range without altering the final motion vector; + + double extreme = Math.max( + Math.max(Math.abs(powerForwardRight),Math.abs(powerForwardLeft)), + Math.max(Math.abs(powerBackRight),Math.abs(powerBackLeft))); + + if (extreme > 1) { + powerForwardRight = powerForwardRight/extreme; + powerForwardLeft = powerForwardLeft/extreme; + powerBackRight = powerBackRight/extreme; + powerBackLeft = powerBackLeft/extreme; + } + + double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight}; + + return powers; + } + + public double[] getFacePowers(float direction, double power) { + double left = power; + double right = -power; + + if (getRelativeAngle(direction, rotation) > 0) { + left *= -1; + right *= -1; + } + double[] powers = {left,right}; + return powers; + } + + //This function should not be used public void driveAtAngle(float angle, double power) { double relativeAngle = getRelativeAngle(angle, getRotation()); @@ -347,9 +402,10 @@ public class Robot { // maintaining the power ratio nesesary to execute the turn. double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower))); - setDrivePower(leftPower * powerAdjust, rightPower * powerAdjust); +// setDrivePower(leftPower * powerAdjust, rightPower * powerAdjust); } + //Data Recording public void record() { TestingRecord+="\n"+locationX+","+locationY+","+rotation; } @@ -373,7 +429,5 @@ public class Robot { } } - public void deactivateVuforia() { - targetsUltimateGoal.deactivate(); - } + }