TimeCrafters/UltimateGoal
1
0
Fork 0
mirror of https://github.com/TimeCrafters/UltimateGoal.git synced 2025-12-13 05:02:34 +00:00
Code Issues Projects Releases Wiki Activity

Prepared for switch to mecanum drive train

Browse Source
This commit is contained in:
Nathaniel Palme
2020-11-19 20:05:31 -06:00
parent 167955166e
commit 92e4bd6395
7 changed files with 254 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java Normal file
View File

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

49
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java
View File

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

87
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java Normal file
View File

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

21
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java
View File

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

2
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java
View File

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

7
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java
View File

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

124
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java
View File

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