mirror of
https://github.com/TimeCrafters/UltimateGoal.git
synced 2025-12-13 05:02:34 +00:00
Documentation and Mecanum Debugging
This commit is contained in:
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -9,7 +9,7 @@ import java.util.ArrayList;
|
||||
public class MechanumBiasCalibrator extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private ArrayList<double[]> Powers;
|
||||
private ArrayList<double[]> 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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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<Float> angles = new ArrayList<Float>();
|
||||
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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]);
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
// }
|
||||
}
|
||||
|
||||
@@ -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());
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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() {
|
||||
|
||||
Reference in New Issue
Block a user