From 91d443d84fc40b9fe72287f81acac12d16376899 Mon Sep 17 00:00:00 2001 From: Nathaniel Palme Date: Sat, 16 Jan 2021 11:46:05 -0600 Subject: [PATCH] So many things --- .../timecrafters/UltimateGoal/AutoEngine.java | 4 - .../Calibration/CalibrationEngine.java | 4 +- .../Calibration/MechanumBiasCalibrator.java | 3 +- .../Calibration/OdometryCalibration.java | 2 +- .../Calibration/PerformanceTest.java | 4 +- .../Calibration/StalPowerCalibrator.java | 4 +- .../Calibration/VelocityTest.java | 7 +- .../Competition/Autonomous/AutoEngine.java | 4 + .../Autonomous/DriveToCoordinates.java | 4 +- .../Autonomous}/DriveToPosition.java | 6 +- .../Autonomous/ThreadStates.java | 2 +- .../{ => Competition}/DriveMotor.java | 2 +- .../UltimateGoal/Competition/Launch.java | 4 + .../UltimateGoal/{ => Competition}/Robot.java | 74 +++++++-------- .../TeleOp}/TeleOpEngine.java | 2 +- .../{ => Competition/TeleOp}/TeleOpState.java | 69 +++++++------- .../HardwareTesting/ControlHubTest.java | 4 +- .../HardwareTesting/DistanceSensorTest.java | 37 ++++++++ .../HardwareTesting/EncoderTest.java | 20 ----- .../HardwareTesting/FullTest.java | 39 ++++++++ .../HardwareTesting/MecanumFunctionTest.java | 2 +- .../HardwareTesting/TestingEngine.java | 21 ++--- .../LocalizerTesting/IMUDrive.java | 82 ----------------- .../LocalizerTesting/IMUTurn.java | 90 ------------------- .../LocalizerTestingEngine.java | 35 -------- .../LocalizerTesting/VuforiaNavTesting.java | 58 ------------ 26 files changed, 187 insertions(+), 396 deletions(-) delete mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/AutoEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java rename TeamCode/src/main/java/org/timecrafters/UltimateGoal/{ => Competition}/Autonomous/DriveToCoordinates.java (95%) rename TeamCode/src/main/java/org/timecrafters/UltimateGoal/{LocalizerTesting => Competition/Autonomous}/DriveToPosition.java (90%) rename TeamCode/src/main/java/org/timecrafters/UltimateGoal/{ => Competition}/Autonomous/ThreadStates.java (92%) rename TeamCode/src/main/java/org/timecrafters/UltimateGoal/{ => Competition}/DriveMotor.java (96%) create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java rename TeamCode/src/main/java/org/timecrafters/UltimateGoal/{ => Competition}/Robot.java (93%) rename TeamCode/src/main/java/org/timecrafters/UltimateGoal/{ => Competition/TeleOp}/TeleOpEngine.java (71%) rename TeamCode/src/main/java/org/timecrafters/UltimateGoal/{ => Competition/TeleOp}/TeleOpState.java (61%) create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/FullTest.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/AutoEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/AutoEngine.java deleted file mode 100644 index 7fa87d2..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/AutoEngine.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.timecrafters.UltimateGoal; - -public class AutoEngine { -} 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 25918b5..75daad4 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java @@ -1,11 +1,9 @@ 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; +import org.timecrafters.UltimateGoal.Competition.Robot; @TeleOp (name = "Calibration", group = "test") public class CalibrationEngine extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java index 32f5933..5723ea8 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java @@ -1,9 +1,8 @@ package org.timecrafters.UltimateGoal.Calibration; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; +import org.timecrafters.UltimateGoal.Competition.Robot; -import java.lang.reflect.Array; import java.util.ArrayList; public class MechanumBiasCalibrator extends CyberarmState { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java index 221bc28..b879417 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java @@ -1,7 +1,7 @@ package org.timecrafters.UltimateGoal.Calibration; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; +import org.timecrafters.UltimateGoal.Competition.Robot; public class OdometryCalibration extends CyberarmState { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java index 47f3cc5..95802c4 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java @@ -1,9 +1,7 @@ package org.timecrafters.UltimateGoal.Calibration; -import com.qualcomm.robotcore.hardware.DcMotor; - import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; +import org.timecrafters.UltimateGoal.Competition.Robot; import java.util.ArrayList; 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 fd72e36..f99aea0 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java @@ -3,9 +3,7 @@ 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; +import org.timecrafters.UltimateGoal.Competition.Robot; public class StalPowerCalibrator extends CyberarmState { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java index 7d9296a..9727d6f 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java @@ -1,11 +1,8 @@ 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; +import org.timecrafters.UltimateGoal.Competition.DriveMotor; +import org.timecrafters.UltimateGoal.Competition.Robot; public class VelocityTest extends CyberarmState { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java new file mode 100644 index 0000000..cc7df02 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java @@ -0,0 +1,4 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +public class AutoEngine { +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/DriveToCoordinates.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java similarity index 95% rename from TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/DriveToCoordinates.java rename to TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java index cba6ba9..32989af 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/DriveToCoordinates.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java @@ -1,7 +1,7 @@ -package org.timecrafters.UltimateGoal.Autonomous; +package org.timecrafters.UltimateGoal.Competition.Autonomous; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; +import org.timecrafters.UltimateGoal.Competition.Robot; public class DriveToCoordinates extends CyberarmState { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToPosition.java similarity index 90% rename from TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java rename to TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToPosition.java index 267c764..fb97cea 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToPosition.java @@ -1,7 +1,7 @@ -package org.timecrafters.UltimateGoal.LocalizerTesting; +package org.timecrafters.UltimateGoal.Competition.Autonomous; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; +import org.timecrafters.UltimateGoal.Competition.Robot; public class DriveToPosition extends CyberarmState { @@ -51,7 +51,7 @@ public class DriveToPosition extends CyberarmState { robot.setDrivePower(0, 0, 0, 0); setHasFinished(true); } else { - robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power); +// robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/ThreadStates.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/ThreadStates.java similarity index 92% rename from TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/ThreadStates.java rename to TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/ThreadStates.java index 77149af..617282a 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/ThreadStates.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/ThreadStates.java @@ -1,4 +1,4 @@ -package org.timecrafters.UltimateGoal.Autonomous; +package org.timecrafters.UltimateGoal.Competition.Autonomous; import org.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/DriveMotor.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java similarity index 96% rename from TeamCode/src/main/java/org/timecrafters/UltimateGoal/DriveMotor.java rename to TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java index 0756e4f..192066c 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/DriveMotor.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java @@ -1,4 +1,4 @@ -package org.timecrafters.UltimateGoal; +package org.timecrafters.UltimateGoal.Competition; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java new file mode 100644 index 0000000..9ba35b3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java @@ -0,0 +1,4 @@ +package org.timecrafters.UltimateGoal.Competition; + +public class Launch { +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java similarity index 93% rename from TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java rename to TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java index 3e18a1d..8dcd989 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -1,4 +1,4 @@ -package org.timecrafters.UltimateGoal; +package org.timecrafters.UltimateGoal.Competition; import android.os.Environment; import android.util.Log; @@ -6,6 +6,7 @@ import android.util.Log; import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; @@ -59,8 +60,6 @@ public class Robot { public DcMotor encoderRight; public DcMotor encoderBack; - public DcMotor launcher; - //Steering Constants static final double FINE_CORRECTION = 0.05 ; static final double LARGE_CORRECTION = 0.002; @@ -97,6 +96,7 @@ public class Robot { //Launcher public DcMotor launchMotor; + private static final long LAUNCH_ACCEL_TIME = 3000; public static final double LAUNCH_POSITION_X = 28 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); public static final double LAUNCH_POSITION_Y = 0 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); public static final float LAUNCH_ROTATION = 0; @@ -104,10 +104,18 @@ public class Robot { public static final double LAUNCH_TOLERANCE_FACE = 0.5; //Ring Belt + public DcMotor collectionMotor; + public DcMotor ringBeltMotor; + private DigitalChannel limitSwitch; + + //todo: tune these when they exist public static final int RING_BELT_LOOP_TICKS = 1000; - public static final int RING_BELT_POS_1 = 200; - public static final int RING_BELT_POS_2 = 400; - public static final int RING_BELT_POS_3 = 600; + public static final int RING_BELT_GAP = 200; + public static final int RING_BELT_RECEIVE_POS = 100; + + public static final double RING_DETECT_DISTANCE = 100; + public static final double RING_DETECT_DELAY = 1000; + //Debugging public double visionX; @@ -139,6 +147,7 @@ public class Robot { // webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); imu = hardwareMap.get(BNO055IMU.class, "imu"); +// limitSwitch.setMode(DigitalChannel.Mode.INPUT); DcMotor dcMotor = hardwareMap.dcMotor.get("driveFrontLeft"); @@ -157,12 +166,14 @@ public class Robot { driveBackLeft.init(); driveBackRight.init(); - //todo add these when they exist - encoderLeft = hardwareMap.dcMotor.get("odoLeft"); +// encoderLeft = hardwareMap.dcMotor.get("odoLeft"); // encoderRight = hardwareMap.dcMotor.get("odoRight"); // encoderBack = hardwareMap.dcMotor.get("odoBack"); -// -// launcher = hardwareMap.dcMotor.get("launcher"); + + launchMotor = hardwareMap.dcMotor.get("launcher"); + collectionMotor = hardwareMap.dcMotor.get("collect"); + collectionMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ringBeltMotor = hardwareMap.dcMotor.get("belt"); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); @@ -190,6 +201,14 @@ public class Robot { rotation = stateConfiguration.variable("system", "startPos", "direction").value(); locationX = stateConfiguration.variable("system", "startPos", "x").value(); locationY = stateConfiguration.variable("system", "startPos", "y").value(); + +// double launcherPower = 0; +// long launchAccelStart = System.currentTimeMillis(); +// while (launcherPower < 1) { +// launcherPower = (double) (System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME; +// launchMotor.setPower(launcherPower); +// } + } private void initVuforia() { @@ -549,34 +568,17 @@ public class Robot { return powers; } - //This function should not be used - public void driveAtAngle(float angle, double power) { - - double relativeAngle = getRelativeAngle(angle, getRotation()); - - //calculate how the power of each motor should be adjusted to make the robot curve - //towards the target angle - //-------------------------------------------------------------------------------------- - - double turnPowerCorrection = Math.abs(power) * (Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle); - - //Adjusts power based on degrees off from target. - double leftPower = power - turnPowerCorrection; - double rightPower = power + turnPowerCorrection; - //-------------------------------------------------------------------------------------- - - - //calculates speed adjuster that slows the motors to be closer to the BasePower while - // maintaining the power ratio nesesary to execute the turn. - double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower))); - -// setDrivePower(leftPower * powerAdjust, rightPower * powerAdjust); + public int getBeltPos(){ + return loopPos(ringBeltMotor.getCurrentPosition()); } - //Data Recording -// public void record(double frontLeft, double frontRight, double backLeft, double backRight) { -// TestingRecord+="\n"+frontLeft+","+frontRight+","+backLeft+","+backRight; -// } + public int loopPos(int pos) { + if (pos < 0) { + pos += RING_BELT_LOOP_TICKS; + } + pos %= RING_BELT_LOOP_TICKS; + return pos; + } public void record() { TestingRecord+="\n"+angularVelocity; diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java similarity index 71% rename from TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpEngine.java rename to TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java index ba81c07..61b313a 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java @@ -1,4 +1,4 @@ -package org.timecrafters.UltimateGoal; +package org.timecrafters.UltimateGoal.Competition.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpState.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java similarity index 61% rename from TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpState.java rename to TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java index d3053f6..16daf0e 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java @@ -1,7 +1,8 @@ -package org.timecrafters.UltimateGoal; +package org.timecrafters.UltimateGoal.Competition.TeleOp; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Autonomous.DriveToCoordinates; +import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; +import org.timecrafters.UltimateGoal.Competition.Robot; public class TeleOpState extends CyberarmState { @@ -16,13 +17,20 @@ public class TeleOpState extends CyberarmState { private double powerScale = 0.2 ; private boolean toggleSpeedInput = false; + private boolean launchInput = false; private CyberarmState launchDriveState = new DriveToCoordinates(robot.LAUNCH_POSITION_X, robot.LAUNCH_POSITION_Y, robot.LAUNCH_ROTATION, 10, POWER_NORMAL, 50); + public TeleOpState(Robot robot) { this.robot = robot; } + @Override + public void start() { + + } + @Override public void exec() { robot.updateLocation(); @@ -41,43 +49,44 @@ public class TeleOpState extends CyberarmState { double[] powers = {0,0,0,0}; - boolean joystickButton = engine.gamepad1.left_stick_button; - - if (joystickButton && !toggleSpeedInput) { - if (powerScale == POWER_NORMAL) { - powerScale = POWER_SPRINT; - } else { - powerScale = POWER_NORMAL; - } - } - - toggleSpeedInput = joystickButton; - - - if (rightJoystickMagnitude == 0) { - if (leftJoystickMagnitude !=0) { - powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, leftJoystickDegrees); - } - } else { - if (leftJoystickMagnitude == 0) { - double[] facePowers = robot.getFacePowers(rightJoystickDegrees, powerScale * rightJoystickMagnitude); - powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; - } else { - powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, rightJoystickDegrees); - } - } - - robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]); - if (engine.gamepad1.y) { double distanceToTarget = Math.hypot(robot.LAUNCH_POSITION_X - robot.getLocationX(), robot.LAUNCH_POSITION_Y - robot.getLocationY()); if (distanceToTarget > robot.LAUNCH_TOLERANCE_POS) { powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.LAUNCH_POSITION_X, robot.LAUNCH_POSITION_Y), POWER_NORMAL, robot.LAUNCH_ROTATION); } else if (robot.getRelativeAngle(robot.LAUNCH_ROTATION, robot.getRotation()) > robot.LAUNCH_TOLERANCE_FACE) { + //todo add launch sequence } + } else { + boolean joystickButton = engine.gamepad1.left_stick_button; + + if (joystickButton && !toggleSpeedInput) { + if (powerScale == POWER_NORMAL) { + powerScale = POWER_SPRINT; + } else { + powerScale = POWER_NORMAL; + } + } + + toggleSpeedInput = joystickButton; + + + if (rightJoystickMagnitude == 0) { + if (leftJoystickMagnitude !=0) { + powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, leftJoystickDegrees); + } + } else { + if (leftJoystickMagnitude == 0) { + double[] facePowers = robot.getFacePowers(rightJoystickDegrees, powerScale * rightJoystickMagnitude); + powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; + } else { + powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, rightJoystickDegrees); + } + } } + robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java index 1faf240..b8b2b37 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java @@ -1,12 +1,10 @@ package org.timecrafters.UltimateGoal.HardwareTesting; -import com.qualcomm.hardware.bosch.BNO055IMU; - import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.navigation.Acceleration; import org.firstinspires.ftc.robotcore.external.navigation.Position; import org.firstinspires.ftc.robotcore.external.navigation.Velocity; -import org.timecrafters.UltimateGoal.Robot; +import org.timecrafters.UltimateGoal.Competition.Robot; public class ControlHubTest extends CyberarmState { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java new file mode 100644 index 0000000..664c945 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java @@ -0,0 +1,37 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.robotcore.hardware.ColorRangeSensor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + + +public class DistanceSensorTest extends CyberarmState { + + + private Rev2mDistanceSensor distanceSensor; +// private RevColorSensorV3 colorSensor; + private ColorRangeSensor colorSensor; + private double distance = 0; + private double color = 0; + + @Override + public void init() { + distanceSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "distance"); + colorSensor = engine.hardwareMap.get(ColorRangeSensor.class, "color"); +// colorSensor = engine.hardwareMap.get(RevColorSensorV3.class, "color"); + } + + @Override + public void exec() { + distance = distanceSensor.getDistance(DistanceUnit.MM); + color = colorSensor.getDistance(DistanceUnit.MM); + } + + @Override + public void telemetry() { + engine.telemetry.addData("distance", distance); + engine.telemetry.addData("color", color); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java deleted file mode 100644 index 1e6540b..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.timecrafters.UltimateGoal.HardwareTesting; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; - - -public class EncoderTest extends CyberarmState { - - private Robot robot; - - public EncoderTest(Robot robot) { - this.robot = robot; - } - - - @Override - public void exec() { - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/FullTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/FullTest.java new file mode 100644 index 0000000..dc38892 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/FullTest.java @@ -0,0 +1,39 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + + +public class FullTest extends CyberarmState { + + private Robot robot; + + public FullTest(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + + } + + @Override + public void exec() { + double p = 0; + + if (engine.gamepad1.x) { + p = 0.1; + } + + + robot.setDrivePower(p,p,p,p); + robot.collectionMotor.setPower(0.5); + robot.ringBeltMotor.setPower(p); + robot.launchMotor.setPower(p); + } + + @Override + public void telemetry() { + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java index a4b564e..9247355 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java @@ -4,7 +4,7 @@ import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; import org.firstinspires.ftc.robotcore.external.navigation.Position; import org.firstinspires.ftc.robotcore.external.navigation.Velocity; -import org.timecrafters.UltimateGoal.Robot; +import org.timecrafters.UltimateGoal.Competition.Robot; public class MecanumFunctionTest extends CyberarmState { 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 2e40d12..139f9a7 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -3,13 +3,12 @@ package org.timecrafters.UltimateGoal.HardwareTesting; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.UltimateGoal.Robot; +import org.timecrafters.UltimateGoal.Competition.Robot; -@TeleOp (name = "Performance test", group = "test") +@TeleOp (name = "Hardware test", group = "test") public class TestingEngine extends CyberarmEngine { private Robot robot; - @Override public void init() { robot = new Robot(hardwareMap); @@ -17,16 +16,14 @@ public class TestingEngine extends CyberarmEngine { super.init(); } - - @Override public void setup() { - addState(new MecanumFunctionTest(robot)); - } - - @Override - public void stop() { - robot.saveRecording(); - super.stop(); + addState(new FullTest(robot)); } +// +// @Override +// public void stop() { +// robot.saveRecording(); +// super.stop(); +// } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java deleted file mode 100644 index 5bec742..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java +++ /dev/null @@ -1,82 +0,0 @@ -package org.timecrafters.UltimateGoal.LocalizerTesting; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; - -public class IMUDrive extends CyberarmState { - - private Robot robot; - private String actionName; - private String groupName; - private double power; - private int tickTarget; - private float angleRelative; - private float angleTarget; - private int tickStart; - private long finishDelay; - - public IMUDrive(Robot robot, String groupName, String actionName) { - this.robot = robot; - this.actionName = actionName; - this.groupName = groupName; - } - - @Override - public void init() { - - power = robot.stateConfiguration.variable(groupName, actionName, "power").value(); - double inchesTarget = robot.stateConfiguration.variable(groupName, actionName, "inches").value(); - tickTarget = (int) robot.inchesToTicks(inchesTarget); - angleTarget = robot.stateConfiguration.variable(groupName, actionName, "angle").value(); - finishDelay = robot.stateConfiguration.variable(groupName, actionName, "delay").value(); - } - - @Override - public void start() { - if (!robot.stateConfiguration.action(groupName,actionName).enabled) { - setHasFinished(true); - } - - angleTarget=robot.getRotation(); - tickStart = robot.driveFrontRight.motor.getCurrentPosition(); - } - - @Override - public void exec() { - - robot.updateLocation(); - - int ticksTraveled = Math.abs( robot.driveFrontRight.motor.getCurrentPosition()-tickStart); - if (ticksTraveled > tickTarget) { -// robot.setDrivePower(0,0); - sleep(finishDelay); - setHasFinished(true); - } else { - - angleRelative = robot.getRelativeAngle(angleTarget, robot.getRotation()); - - double turnPowerCorrection = Math.pow(0.03 * angleRelative, 3) + 0.01 * angleRelative; - - double rightPower = power + turnPowerCorrection; - double leftPower = power - turnPowerCorrection; - - double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower))); - -// robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower); - } - } - - @Override - public void telemetry() { - engine.telemetry.addLine("Measured 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("Total Travel"); - engine.telemetry.addData("Left", robot.ticksToInches(robot.traveledLeft)); - engine.telemetry.addData("Right", robot.ticksToInches(robot.traveledRight)); - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java deleted file mode 100644 index f86b3cb..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java +++ /dev/null @@ -1,90 +0,0 @@ -package org.timecrafters.UltimateGoal.LocalizerTesting; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; - -public class IMUTurn extends CyberarmState { - - private Robot robot; - private String actionName; - private String groupName; - private double power; - private float angleTarget; - private int turnDirection; - private float angleAllowance; - private boolean useOptimalDirection; - - public IMUTurn(Robot robot, String groupName, String actionName) { - this.robot = robot; - this.actionName = actionName; - this.groupName = groupName; - } - - @Override - public void init() { - power = robot.stateConfiguration.variable(groupName,actionName, "power").value(); - angleTarget = robot.stateConfiguration.variable(groupName,actionName, "angle").value(); - angleAllowance = robot.stateConfiguration.variable(groupName,actionName, "allowance").value(); - - // turnDirection = robot.stateConfiguration.variable(groupName, actionName, "direction").value(); - - turnDirection = 0; - - useOptimalDirection = (turnDirection == 0); - } - - @Override - public void start() { - if (!robot.stateConfiguration.action(groupName,actionName).enabled) { - setHasFinished(true); - } - } - - @Override - public void exec() { - robot.updateLocation(); - - int OptimalDirection = getOptimalDirection(angleTarget, robot.getRotation()); - - if (!useOptimalDirection && OptimalDirection == turnDirection){ - useOptimalDirection = true; - } - - if (useOptimalDirection){ - turnDirection = OptimalDirection; - } - -// robot.setDrivePower(power * turnDirection,-power * turnDirection); - - if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) { -// robot.setDrivePower(0,0); - setHasFinished(true); - } - - - - } - - private int getOptimalDirection(float angleTarget, float angleCurrent){ - - if (angleCurrent < 0) { - angleCurrent += 360; - } - - if (angleTarget < 0) { - angleTarget += 360; - } - - float degreeDifferance = angleTarget - angleCurrent; - if (degreeDifferance > 180 || (degreeDifferance < 0 && degreeDifferance > -180)) { - return -1; - } else { - return 1; - } - } - - @Override - public void telemetry() { - engine.telemetry.addData("rotation", robot.getRotation()); - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java deleted file mode 100644 index 3096b11..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.timecrafters.UltimateGoal.LocalizerTesting; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.UltimateGoal.Robot; - -@TeleOp(name = "Localizer Test") -public class LocalizerTestingEngine extends CyberarmEngine { - - private Robot robot; - - @Override - public void init() { - robot = new Robot(hardwareMap); - robot.initHardware(); - super.init(); - } - - @Override - public void setup() { - addState(new VuforiaNavTesting(robot)); -// addState(new DriveToPosition(robot, "DtoP", "d1")); -// addState(new DriveToPosition(robot, "DtoP", "d2")); - - } - - @Override - public void stop() { - super.stop(); - robot.saveRecording(); - robot.deactivateVuforia(); - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java deleted file mode 100644 index 23ba2c4..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java +++ /dev/null @@ -1,58 +0,0 @@ -package org.timecrafters.UltimateGoal.LocalizerTesting; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; - - -public class VuforiaNavTesting extends CyberarmState { - - private Robot robot; - private float angle; - private double X; - private double Y; - private float joystickDegrees; - private double power; - - public VuforiaNavTesting(Robot robot) { - this.robot = robot; - } - - @Override - public void exec() { - if (engine.gamepad1.x) { - robot.syncWithVuforia(); - } - - robot.updateLocation(); -// robot.record(); - - double joystickX = engine.gamepad1.right_stick_x; - double joystickY = engine.gamepad1.right_stick_y; - - joystickDegrees = (float) Math.toDegrees(Math.atan2(joystickX, -joystickY)); - power = 0.3 * Math.hypot(joystickX, joystickY); - - robot.driveAtAngle(joystickDegrees, power); - - X = robot.getLocationX(); - Y = robot.getLocationY(); - angle = robot.getRotation(); - - - - } - - @Override - public void telemetry() { - engine.telemetry.addData("Target Visible", robot.trackableVisible); - engine.telemetry.addData("Odo X", robot.ticksToInches(X)); - engine.telemetry.addData("Odo Y", robot.ticksToInches(Y)); - - engine.telemetry.addData("Joystick", joystickDegrees); -// engine.telemetry.addData("Vis X", robot.ticksToInches(robot.visionX)); -// engine.telemetry.addData("Vis Y", robot.ticksToInches(robot.visionY)); - - engine.telemetry.addData("Robot Angle", angle); - } - -}