Documentation and Mecanum Debugging

This commit is contained in:
Nathaniel Palme
2020-12-08 16:45:46 -06:00
parent 6eb0f4e971
commit 225da7dd95
14 changed files with 346 additions and 61 deletions

View File

@@ -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);
}
}

View File

@@ -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));
}
}

View File

@@ -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);
}
}

View File

@@ -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);
}
}
}

View File

@@ -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]);
}
}

View File

@@ -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);
}
}

View File

@@ -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);
}
}

View File

@@ -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);
}
}

View File

@@ -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();
// }
}

View File

@@ -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());

View File

@@ -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);
}
}

View File

@@ -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);
}

View File

@@ -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;

View File

@@ -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() {