From e46e264a04c108fb53ad0e546ae18b3a750fef48 Mon Sep 17 00:00:00 2001 From: Nathaniel Palme Date: Tue, 15 Dec 2020 20:36:26 -0600 Subject: [PATCH] Reorganized drive motors, added baking, and RUN_USING_ENCODERS. changed robot localization to work with new odometry designs and added OdometryCalibration --- .../Calibration/CalibrationEngine.java | 2 +- .../Calibration/OdometryCalibration.java | 39 +++++++ .../Calibration/StalPowerCalibrator.java | 4 +- .../Calibration/VelocityTest.java | 52 +++++++++ .../timecrafters/UltimateGoal/DriveMotor.java | 45 ++++++++ .../org/timecrafters/UltimateGoal/Robot.java | 100 ++++++++++++------ 6 files changed, 206 insertions(+), 36 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/DriveMotor.java diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java index a8461f8..25918b5 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java @@ -21,6 +21,6 @@ public class CalibrationEngine extends CyberarmEngine { @Override public void setup() { - addState(new MechanumBiasCalibrator(robot)); + addState(new VelocityTest(robot)); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java new file mode 100644 index 0000000..c8784c8 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java @@ -0,0 +1,39 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + +import java.util.ArrayList; + +public class OdometryCalibration extends CyberarmState { + + private Robot robot; + private float rotation; + private int currentTick; + private double ticksPerDegree; + + public OdometryCalibration(Robot robot) { + this.robot = robot; + } + + @Override + public void exec() { + + if (engine.gamepad1.x) { + double power = 0.1; + robot.setDrivePower(power, -power, power, -power); + currentTick = robot.encoderBack.getCurrentPosition(); + rotation = -robot.imu.getAngularOrientation().firstAngle; + ticksPerDegree = currentTick/rotation; + } else { + robot.setDrivePower(0,0,0,0); + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("degrees", rotation); + engine.telemetry.addData("ticks", currentTick); + engine.telemetry.addData("ticks per degree", ticksPerDegree); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java index 55ee1c7..fc8d1b8 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java @@ -28,7 +28,7 @@ public class StalPowerCalibrator extends CyberarmState { for (DcMotor motor : Motors) { motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } } @@ -51,7 +51,7 @@ public class StalPowerCalibrator extends CyberarmState { sleep(1000); for (DcMotor motor : Motors) { motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java new file mode 100644 index 0000000..7d9296a --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java @@ -0,0 +1,52 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.DriveMotor; +import org.timecrafters.UltimateGoal.Robot; + +public class VelocityTest extends CyberarmState { + + private Robot robot; + private double[] Velocities; + private DriveMotor[] Motors; + private long LastTime = 0; + + + + public VelocityTest(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + Motors = new DriveMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight}; + Velocities = new double[] {0,0,0,0}; + + } + + @Override + public void start() { + for (DriveMotor motor : Motors) { + motor.setPower(0.1); + } + } + + @Override + public void exec() { + + for (int i = 0; i < 4; i++) { + Velocities[i] = (Motors[i].motor.getCurrentPosition()* 60000) / runTime(); + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("FrontLeft", Velocities[0]); + engine.telemetry.addData("FrontRight", Velocities[1]); + engine.telemetry.addData("BackLeft", Velocities[2]); + engine.telemetry.addData("BackRight", Velocities[3]); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/DriveMotor.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/DriveMotor.java new file mode 100644 index 0000000..0756e4f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/DriveMotor.java @@ -0,0 +1,45 @@ +package org.timecrafters.UltimateGoal; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +public class DriveMotor { + + public DcMotor motor; + private int brakePosition; + private MotorConfigurationType motorType; + private DcMotorSimple.Direction direction; + static final double FINE_CORRECTION = 0.02; + static final double LARGE_CORRECTION = 0.03; + + public DriveMotor(DcMotor motor, MotorConfigurationType motorType, DcMotorSimple.Direction direction) { + this.motor = motor; + this.motorType = motorType; + this.direction = direction; + } + + public void init() { + motor.setMotorType(motorType); + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motor.setDirection(direction); + } + + public void setPower(double power) { + motor.setPower(power); + } + + public void setBrakePosition() { + brakePosition = motor.getCurrentPosition(); + } + + public void brake() { + double currentPosition = motor.getCurrentPosition(); + double relativePosition = brakePosition - currentPosition; + double breakPower = Math.pow(LARGE_CORRECTION * relativePosition, 3) + FINE_CORRECTION * relativePosition; + + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index 452da1c..5a54a7f 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -7,6 +7,7 @@ import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.ClassFactory; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -47,10 +48,19 @@ public class Robot { //drive system - public DcMotor driveBackLeft; - public DcMotor driveFrontLeft; - public DcMotor driveBackRight; - public DcMotor driveFrontRight; + public DriveMotor driveFrontLeft; + public DriveMotor driveBackLeft; + public DriveMotor driveFrontRight; + public DriveMotor driveBackRight; + + public DcMotor encoderLeft; + public DcMotor encoderRight; + public DcMotor encoderBack; + + private int brakePosFrontLeft; + private int brakePosFrontRight; + private int brakePosBackLeft; + private int brakePosBackRight; static final double BIAS_FRONT_LEFT = 1; static final double BIAS_FRONT_RIGHT = 1; @@ -58,12 +68,14 @@ public class Robot { static final double BIAS_BACK_RIGHT = 1; static final double FINE_CORRECTION = 0.01; - static final double LARGE_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; static final float mmPerInch = 25.4f; + //todo make/run calibration code to find this exact value + static final double TICKS_PER_ROBOT_DEGREE = Math.PI * 1000; // Inches Forward of axis of rotation static final float CAMERA_FORWARD_DISPLACEMENT = 13f; @@ -109,18 +121,21 @@ public class Robot { imu = hardwareMap.get(BNO055IMU.class, "imu"); - driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft"); - driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight"); - driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft"); - driveBackRight = hardwareMap.dcMotor.get("driveBackRight"); + DcMotor dcMotor = hardwareMap.dcMotor.get("driveFrontLeft"); - driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); - driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + MotorConfigurationType motorType = dcMotor.getMotorType(); + motorType.setGearing(5); + motorType.setTicksPerRev(140); + motorType.setMaxRPM(1200); - driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveFrontLeft = new DriveMotor(dcMotor, motorType, DcMotorSimple.Direction.FORWARD); + driveFrontRight = new DriveMotor(hardwareMap.dcMotor.get("driveFrontRight"), motorType, DcMotorSimple.Direction.REVERSE); + driveBackLeft = new DriveMotor(hardwareMap.dcMotor.get("driveBackLeft"), motorType, DcMotorSimple.Direction.FORWARD); + driveBackRight = new DriveMotor(hardwareMap.dcMotor.get("driveBackRight"), motorType, DcMotorSimple.Direction.REVERSE); + + encoderLeft = hardwareMap.dcMotor.get("odoLeft"); + encoderRight = hardwareMap.dcMotor.get("odoRight"); + encoderBack = hardwareMap.dcMotor.get("odoBack"); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); @@ -203,35 +218,40 @@ public class Robot { public void updateLocation(){ // orientation is inverted to have clockwise be positive. float imuAngle = -imu.getAngularOrientation().firstAngle; - double rotationChange = imuAngle - rotationPrevious; + double rotationChange = getRelativeAngle(rotationPrevious, imuAngle); - if (rotationChange > 180) { - rotationChange -= 360; - } - if (rotationChange < -180) { - rotationChange += 360; - } + int encoderLeftCurrent = encoderLeft.getCurrentPosition(); + int encoderRightCurrent = encoderRight.getCurrentPosition(); + int encoderBackCurrent = encoderBack.getCurrentPosition(); - int encoderLeftCurrent = driveFrontLeft.getCurrentPosition(); - int encoderRightCurrent = driveFrontRight.getCurrentPosition(); double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious; double encoderRightChange = encoderRightCurrent - encoderRightPrevious; - - traveledLeft += Math.abs(encoderLeftChange); - traveledRight += Math.abs(encoderRightChange); - rotation += rotationChange; + double encoderBackChange = encoderBackCurrent - encoderBackPrevious; encoderLeftPrevious = encoderLeftCurrent; encoderRightPrevious = encoderRightCurrent; + encoderBackPrevious = encoderBackCurrent; rotationPrevious = imuAngle; - double average = (encoderLeftChange+encoderRightChange)/2; + //The forward Vector has the luxury of having an odometer on both sides of the robot. + //This allows us to eliminate the unwanted influence of turning the robot by averaging + //the two. This meathod doesn't require any prior calibration. + double forwardVector = (encoderLeftChange+encoderRightChange)/2; - double xChange = average * (Math.sin(Math.toRadians(rotation))); - double yChange = average * (Math.cos(Math.toRadians(rotation))); + //Since there isn't a second wheel to remove the influence of robot rotation, we have to + //instead do this by approximating the number of ticks that were removed due to rotation + //based on a separate calibration program. + double sidewaysVector = encoderBackChange + (rotationChange*TICKS_PER_ROBOT_DEGREE); + + double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector)); + double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector); + + double xChange = magnitude * (Math.sin(direction)); + double yChange = magnitude * (Math.cos(direction)); locationX += xChange; locationY += yChange; + rotation += rotationChange; if (rotation > 180) { @@ -349,7 +369,6 @@ public class Robot { 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; @@ -382,7 +401,22 @@ public class Robot { return powers; } - // + public void brakeAll() { + driveFrontLeft.brake(); + driveFrontRight.brake(); + driveBackLeft.brake(); + driveBackRight.brake(); + } + + //sets the position the motors will lock to when braking to the motor's current position. + public void setAllBrakePos() { + driveFrontLeft.setBrakePosition(); + driveFrontRight.setBrakePosition(); + driveBackLeft.setBrakePosition(); + driveBackRight.setBrakePosition(); + } + + //Outputs the power necessary to turn and face a provided direction public double[] getFacePowers(float direction, double power) { double relativeAngle = getRelativeAngle(direction, rotation); double scaler = Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle;