diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/DriveToCoordinates.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/DriveToCoordinates.java new file mode 100644 index 0000000..cba6ba9 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/DriveToCoordinates.java @@ -0,0 +1,72 @@ +package org.timecrafters.UltimateGoal.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + +public class DriveToCoordinates extends CyberarmState { + + private Robot robot; + private String groupName = "manual"; + private String actionName; + private double xTarget; + private double yTarget; + private float faceAngle; + private double tolerance; + private double power; + private boolean braking; + private long breakStartTime; + private long breakTime; + + public DriveToCoordinates(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + public DriveToCoordinates(double xTarget, double yTarget, float faceAngle, double tolerance, double power, long breakStartTime) { + this.xTarget = xTarget; + this.yTarget = yTarget; + this.faceAngle = faceAngle; + this.tolerance = tolerance; + this.power = power; + this.breakTime = breakTime; + } + + @Override + public void init() { + if (!groupName.equals("manual")) { + xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "x").value()); + yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "y").value()); + power = robot.stateConfiguration.variable(groupName, actionName, "power").value(); + faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value(); + tolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tol").value()); + breakTime = robot.stateConfiguration.variable(groupName, actionName, "breakMS").value(); + } + } + + @Override + public void start() { + + } + + @Override + public void exec() { + + double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY()); + + if (distanceToTarget > tolerance) { + double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle); + robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]); + braking = false; + } else { + if (!braking) { + breakStartTime = System.currentTimeMillis(); + robot.setBrakePosAll(); + braking = true; + } + robot.brakeAll(); + setHasFinished(System.currentTimeMillis() - breakStartTime > breakTime); + } + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/ThreadStates.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/ThreadStates.java new file mode 100644 index 0000000..77149af --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Autonomous/ThreadStates.java @@ -0,0 +1,33 @@ +package org.timecrafters.UltimateGoal.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; + +import java.util.ArrayList; + +public class ThreadStates extends CyberarmState { + + private ArrayList states = new ArrayList<>(); + + public ThreadStates(ArrayList states) { + this.states = states; + } + + @Override + public void start() { + for (CyberarmState state : states) { + addParallelState(state); + } + } + + @Override + public void exec() { + int finishedStates = 0; + for (CyberarmState state : states) { + if (state.getHasFinished()) { + finishedStates += 1; + } + } + setHasFinished(finishedStates == states.size()); + + } +} 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 c8784c8..221bc28 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java @@ -3,14 +3,13 @@ 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; + private double ticksPerDegreeClockwise; + private double ticksPerDegreeCounterClockwise; public OdometryCalibration(Robot robot) { this.robot = robot; @@ -19,21 +18,27 @@ public class OdometryCalibration extends CyberarmState { @Override public void exec() { + double power = 0.1; + rotation = -robot.imu.getAngularOrientation().firstAngle; + currentTick = robot.encoderBack.getCurrentPosition(); + 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; + ticksPerDegreeClockwise = currentTick/rotation; + } else if(engine.gamepad1.y) { + robot.setDrivePower(-power, power, -power, power); + ticksPerDegreeCounterClockwise = 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); + engine.telemetry.addData("Clockwise", ticksPerDegreeClockwise); + engine.telemetry.addData("CounterClockwise", ticksPerDegreeCounterClockwise); } } 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 fc8d1b8..fd72e36 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java @@ -23,7 +23,7 @@ public class StalPowerCalibrator extends CyberarmState { @Override public void init() { - Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight}; +// Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight}; Powers = new double[] {0,0,0,0}; for (DcMotor motor : Motors) { 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 442a8b5..1faf240 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java @@ -1,27 +1,78 @@ 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; public class ControlHubTest extends CyberarmState { private Robot robot; - private float angle = 0; + + private Acceleration acceleration; + private double Vx = 0; + private double Vy = 0; + private double Vz = 0; + private double aVx = 0; + private double aVy = 0; + private double aVz = 0; + public ControlHubTest(Robot robot) { this.robot = robot; } + @Override + public void init() { + + Velocity startVelocity = new Velocity(); + startVelocity.xVeloc = 0; + startVelocity.yVeloc = 0; + startVelocity.zVeloc = 0; + + Position startPosition = new Position(); + startPosition.x = 0; + startPosition.y = 0; + startPosition.z = 0; + + robot.imu.startAccelerationIntegration(startPosition,startVelocity, 200); + } + @Override public void exec() { - angle = robot.imu.getAngularOrientation().firstAngle; + acceleration = robot.imu.getAcceleration(); + + Vx = robot.imu.getVelocity().xVeloc; + Vy = robot.imu.getVelocity().yVeloc; + Vz = robot.imu.getVelocity().zVeloc; + + aVx = robot.imu.getAngularVelocity().xRotationRate; + aVy = robot.imu.getAngularVelocity().yRotationRate; + aVz = robot.imu.getAngularVelocity().zRotationRate; + + } @Override public void telemetry() { - engine.telemetry.addLine("Greetings"); - engine.telemetry.addData("Angle", angle); + engine.telemetry.addLine("Acceleration"); + engine.telemetry.addData("x", acceleration.xAccel); + engine.telemetry.addData("y", acceleration.yAccel); + engine.telemetry.addData("z", acceleration.zAccel); + + engine.telemetry.addLine("Velocity"); + engine.telemetry.addData("X", Vx); + engine.telemetry.addData("Y", Vy); + engine.telemetry.addData("Z", Vz); + + engine.telemetry.addLine("Angular Velocity"); + engine.telemetry.addData("X", aVx); + engine.telemetry.addData("Y", aVy); + engine.telemetry.addData("Z", aVz); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlerTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlerTest.java deleted file mode 100644 index 388316d..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlerTest.java +++ /dev/null @@ -1,66 +0,0 @@ -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); - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java index 1fd562e..1e6540b 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java @@ -16,7 +16,5 @@ public class EncoderTest extends CyberarmState { @Override public void exec() { - robot.setDrivePower(0.1, 0.1,0.1,0.1); - } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java deleted file mode 100644 index 55b0dff..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.timecrafters.UltimateGoal.HardwareTesting; - -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Robot; - - -public class LauncherTest extends CyberarmState { - - DcMotor LaunchMotor; - DcMotor SuckMotor; - - @Override - public void init() { - LaunchMotor = engine.hardwareMap.dcMotor.get("launcher"); - SuckMotor = engine.hardwareMap.dcMotor.get("collector"); - } - - @Override - public void exec() { - LaunchMotor.setPower(1); - SuckMotor.setPower(1); - } - - @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 f689c29..a4b564e 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java @@ -1,6 +1,9 @@ package org.timecrafters.UltimateGoal.HardwareTesting; 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; @@ -16,15 +19,43 @@ public class MecanumFunctionTest extends CyberarmState { private double powerBackLeft=0; private double powerBackRight=0; - private static double TURN_POWER_SCALE = 0.5; + private double aVx = 0; + private double aVy = 0; + private double aVz = 0; + + private int powerStep = 5; + private double POWER_SCALE = 0.5 ; + private boolean toggleSpeedInput = false; public MecanumFunctionTest(Robot robot) { this.robot = robot; } + @Override + public void init() { + Velocity startVelocity = new Velocity(); + startVelocity.xVeloc = 0; + startVelocity.yVeloc = 0; + startVelocity.zVeloc = 0; + + Position startPosition = new Position(); + startPosition.x = 0; + startPosition.y = 0; + startPosition.z = 0; + + robot.imu.startAccelerationIntegration(startPosition,startVelocity, 10); + } + @Override public void exec() { robot.updateLocation(); + robot.record(); + + AngularVelocity angularVelocity = robot.imu.getAngularVelocity(); + + aVx = angularVelocity.xRotationRate; + aVy = angularVelocity.yRotationRate; + aVz = angularVelocity.zRotationRate; double leftJoystickX = engine.gamepad1.left_stick_x; double leftJoystickY = engine.gamepad1.left_stick_y; @@ -37,29 +68,45 @@ public class MecanumFunctionTest extends CyberarmState { rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)); rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); -// + powerFrontLeft = 0; powerFrontRight = 0; powerBackLeft = 0; powerBackRight = 0; + boolean a = engine.gamepad1.a; + boolean b = engine.gamepad1.b; + + if (a && !toggleSpeedInput && POWER_SCALE < 1) { + powerStep += 1; + POWER_SCALE = powerStep * 0.1; + } + + if (b && !toggleSpeedInput && POWER_SCALE > 0.1) { + powerStep -= 1; + POWER_SCALE = powerStep * 0.1; + } + + toggleSpeedInput = a || b; + if (rightJoystickMagnitude == 0) { if (leftJoystickMagnitude !=0) { - double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, leftJoystickDegrees); + double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * 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]; + double[] powers = robot.getFacePowers(rightJoystickDegrees, POWER_SCALE * rightJoystickMagnitude); + powerFrontLeft = powers[0]; + powerFrontRight = powers[1]; + powerBackLeft = powers[0]; + powerBackRight = powers[1]; } else { - double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, rightJoystickDegrees); + double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, rightJoystickDegrees); powerFrontLeft = powers[0]; powerFrontRight = powers[1]; powerBackLeft = powers[2]; @@ -67,22 +114,24 @@ public class MecanumFunctionTest extends CyberarmState { } } -// robot.record(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight); - robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight); + } @Override public void telemetry() { + engine.telemetry.addData("Scale", POWER_SCALE); - engine.telemetry.addLine("Left Joystick"); - engine.telemetry.addData("Angle", leftJoystickDegrees); - engine.telemetry.addData("Mag", leftJoystickMagnitude); - - engine.telemetry.addLine("Right Joystick"); - engine.telemetry.addData("Angle", rightJoystickDegrees); - engine.telemetry.addData("Mag", rightJoystickMagnitude); + engine.telemetry.addLine("Angular Velocity"); + engine.telemetry.addData("X", aVx); + engine.telemetry.addData("Y", aVy); + engine.telemetry.addData("Z", aVz); + engine.telemetry.addLine("Powers"); + engine.telemetry.addData("FL", robot.driveFrontLeft.motor.getPower()); + engine.telemetry.addData("FR", robot.driveFrontRight.motor.getPower()); + engine.telemetry.addData("BL", robot.driveBackLeft.motor.getPower()); + engine.telemetry.addData("BR", robot.driveBackRight.motor.getPower()); } } 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 7da6410..2e40d12 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -5,7 +5,7 @@ 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 = "Performance test", group = "test") public class TestingEngine extends CyberarmEngine { private Robot robot; @@ -24,9 +24,9 @@ public class TestingEngine extends CyberarmEngine { addState(new MecanumFunctionTest(robot)); } -// @Override -// public void stop() { -// robot.saveRecording(); -// super.stop(); -// } + @Override + public void stop() { + robot.saveRecording(); + super.stop(); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WelcomeToJankyTown.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WelcomeToJankyTown.java deleted file mode 100644 index c74a7fa..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WelcomeToJankyTown.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.timecrafters.UltimateGoal.HardwareTesting; - -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.cyberarm.engine.V2.CyberarmState; - - -public class WelcomeToJankyTown extends CyberarmState { - - DcMotor left; - DcMotor right; - - @Override - public void init() { - left = engine.hardwareMap.dcMotor.get("left"); - right = engine.hardwareMap.dcMotor.get("right"); - } - - @Override - public void exec() { - left.setPower(-engine.gamepad1.left_stick_y); - right.setPower(engine.gamepad1.right_stick_y); - } - - @Override - public void telemetry() { - engine.telemetry.addLine("Welcome to Janky Town!"); - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java index 8fa54b6..5bec742 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java @@ -38,7 +38,7 @@ public class IMUDrive extends CyberarmState { } angleTarget=robot.getRotation(); - tickStart = robot.driveFrontRight.getCurrentPosition(); + tickStart = robot.driveFrontRight.motor.getCurrentPosition(); } @Override @@ -46,7 +46,7 @@ public class IMUDrive extends CyberarmState { robot.updateLocation(); - int ticksTraveled = Math.abs( robot.driveFrontRight.getCurrentPosition()-tickStart); + int ticksTraveled = Math.abs( robot.driveFrontRight.motor.getCurrentPosition()-tickStart); if (ticksTraveled > tickTarget) { // robot.setDrivePower(0,0); sleep(finishDelay); diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index 5a54a7f..3e18a1d 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -14,6 +14,8 @@ 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.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; @@ -57,25 +59,22 @@ public class Robot { public DcMotor encoderRight; public DcMotor encoderBack; - private int brakePosFrontLeft; - private int brakePosFrontRight; - private int brakePosBackLeft; - private int brakePosBackRight; + public DcMotor launcher; - 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; - - static final double FINE_CORRECTION = 0.01; - static final double LARGE_CORRECTION = 0.01; + //Steering Constants + static final double FINE_CORRECTION = 0.05 ; + static final double LARGE_CORRECTION = 0.002; + static final double MOMENTUM_CORRECTION = 1.015; + static final double MOMENTUM_MAX_CORRECTION = 1.35; + static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION)); //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; + static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = Math.PI * 1000; + static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = Math.PI * 1000; // Inches Forward of axis of rotation static final float CAMERA_FORWARD_DISPLACEMENT = 13f; @@ -89,28 +88,48 @@ public class Robot { public double locationY; private float rotation; + private int encoderLeftPrevious = 0; + private int encoderBackPrevious = 0; + private int encoderRightPrevious = 0; + private float rotationPrevious = 0; + public float angularVelocity; + + //Launcher + public DcMotor launchMotor; + + 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; + public static final double LAUNCH_TOLERANCE_POS = 50; + public static final double LAUNCH_TOLERANCE_FACE = 0.5; + + //Ring Belt + 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; + //Debugging public double visionX; public double visionY; public float rawAngle; // private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight"; - private String TestingRecord = "Rotation"; + private String TestingRecord = "AngularVelocity"; public double traveledLeft; public double traveledRight; - private int encoderLeftPrevious = 0; - private int encoderBackPrevious = 0; - private int encoderRightPrevious = 0; - private float rotationPrevious = 0; - //vuforia navigation public boolean trackableVisible; private VuforiaTrackables targetsUltimateGoal; private List trackables = new ArrayList(); private OpenGLMatrix lastConfirmendLocation; + private long timeStartZeroVelocity = 0; + private static final long MIN_CHECK_DURATION_MS = 500; + private static final double MIN_CHECK_VELOCITY = 0.005; + //TensorFlow Object Detection public TFObjectDetector tfObjectDetector; private static final float MINIMUM_CONFIDENCE = 0.8f; @@ -126,16 +145,24 @@ public class Robot { MotorConfigurationType motorType = dcMotor.getMotorType(); motorType.setGearing(5); motorType.setTicksPerRev(140); - motorType.setMaxRPM(1200); + motorType.setMaxRPM(800); 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); + driveFrontLeft.init(); + driveFrontRight.init(); + driveBackLeft.init(); + driveBackRight.init(); + + //todo add these when they exist encoderLeft = hardwareMap.dcMotor.get("odoLeft"); - encoderRight = hardwareMap.dcMotor.get("odoRight"); - encoderBack = hardwareMap.dcMotor.get("odoBack"); +// encoderRight = hardwareMap.dcMotor.get("odoRight"); +// encoderBack = hardwareMap.dcMotor.get("odoBack"); +// +// launcher = hardwareMap.dcMotor.get("launcher"); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); @@ -146,6 +173,18 @@ public class Robot { imu.initialize(parameters); + Velocity startVelocity = new Velocity(); + startVelocity.xVeloc = 0; + startVelocity.yVeloc = 0; + startVelocity.zVeloc = 0; + + Position startPosition = new Position(); + startPosition.x = 0; + startPosition.y = 0; + startPosition.z = 0; + + imu.startAccelerationIntegration(startPosition,startVelocity, 10); + // initVuforia(); rotation = stateConfiguration.variable("system", "startPos", "direction").value(); @@ -216,10 +255,12 @@ public class Robot { //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; double rotationChange = getRelativeAngle(rotationPrevious, imuAngle); + //todo add this when encoders exist. int encoderLeftCurrent = encoderLeft.getCurrentPosition(); int encoderRightCurrent = encoderRight.getCurrentPosition(); int encoderBackCurrent = encoderBack.getCurrentPosition(); @@ -241,7 +282,16 @@ public class Robot { //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 ticksPerDegree; + + if(rotationChange < 0) { + ticksPerDegree = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE; + } else { + ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE; + } + //todo: check if direction of the back encoder gives expected values + double sidewaysVector = encoderBackChange - (rotationChange* ticksPerDegree); double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector)); double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector); @@ -251,9 +301,26 @@ public class Robot { locationX += xChange; locationY += yChange; + rotation += rotationChange; + double totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightCurrent) + Math.abs(encoderBackChange); + + if (totalV < MIN_CHECK_VELOCITY) { + long timeCurrent = System.currentTimeMillis(); + + if (timeStartZeroVelocity == 0) { + timeStartZeroVelocity = timeCurrent; + } else if (timeCurrent - timeStartZeroVelocity >= MIN_CHECK_DURATION_MS) { + syncWithVuforia(); + } + + } else { + timeStartZeroVelocity = 0; + } + + if (rotation > 180) { rotation -= 360; } @@ -263,6 +330,15 @@ public class Robot { } + public void checkVuforiaSync() { + + double V1 = driveFrontLeft.motor.getPower(); + double V2 = driveFrontRight.motor.getPower(); + double V3 = driveBackLeft.motor.getPower(); + double V4 = driveBackRight.motor.getPower(); + + } + public void syncWithVuforia() { trackableVisible = false; for (VuforiaTrackable trackable : trackables) { @@ -354,10 +430,10 @@ public class Robot { //Drive Functions public void setDrivePower(double powerFrontLeft, double powerFrontRight, double powerBackLeft, double powerBackRight){ - driveFrontLeft.setPower(powerFrontLeft * BIAS_FRONT_LEFT); - driveFrontRight.setPower(powerFrontRight * BIAS_FRONT_RIGHT); - driveBackLeft.setPower(powerBackLeft * BIAS_BACK_LEFT); - driveBackRight.setPower(powerBackRight * BIAS_BACK_RIGHT); + driveFrontLeft.setPower(powerFrontLeft); + driveFrontRight.setPower(powerFrontRight); + driveBackLeft.setPower(powerBackLeft); + driveBackRight.setPower(powerBackRight); } //returns an array of the powers necessary to execute the provided motion. "degreesDirectionMotion" @@ -365,25 +441,52 @@ public class Robot { //the angle the robot should face relative to the field. The order of the output powers is //is ForwardLeft, ForwardRight, BackLeft, BackRight public double[] getMecanumPowers(float degreesDirectionMotion, double scalar, float degreesDirectionFace) { + angularVelocity = imu.getAngularVelocity().xRotationRate; + + //calculating the base mecanum powers so that the robot drives along the degreesDirectionMotion + //once it is pointed towards the degreesDirectionFace double rad = Math.toRadians(getRelativeAngle(degreesDirectionFace,degreesDirectionMotion)); - double y = scalar * Math.cos(rad); - double x = scalar * Math.sin(rad); + double y = Math.cos(rad); + double x = Math.sin(rad); double p = y + x; double q = y - x; + //calculating correction needed to steer the robot towards the degreesDirectionFace float relativeRotation = getRelativeAngle(degreesDirectionFace, rotation); 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 = scalar * (q + turnCorrection); + double powerForwardLeft = scalar * (p - turnCorrection); + double powerBackRight = scalar * (p + turnCorrection); + double powerBackLeft = scalar * (q - turnCorrection); + //the turnCorrection often results in powers with magnitudes significantly larger than the + //scalar. The scaleRatio mitigates this without altering the quality of the motion by making + //it so that the average of the four magnitudes is equal to the scalar magnitude. + double powerSum = Math.abs(powerForwardRight) + Math.abs(powerForwardLeft) + + Math.abs(powerBackRight) + Math.abs(powerBackLeft); + double scaleRatio = (4 * Math.abs(scalar))/powerSum; + + powerForwardRight *= scaleRatio; + powerForwardLeft *= scaleRatio; + powerBackRight *= scaleRatio; + powerBackLeft *= scaleRatio; + + + if (relativeRotation != 0) { + double momentumRelative = angularVelocity * (relativeRotation / Math.abs(relativeRotation)); + double exponential = Math.pow(MOMENTUM_CORRECTION, -momentumRelative); + double momentumCorrection = (2*exponential)/(1+exponential); + powerForwardRight *= momentumCorrection; + powerForwardLeft *= momentumCorrection; + powerBackRight *= momentumCorrection; + powerBackLeft *= momentumCorrection; + } // 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; + // workable range without altering the final motion vector. double extreme = Math.max( Math.max(Math.abs(powerForwardRight),Math.abs(powerForwardLeft)), @@ -396,6 +499,15 @@ public class Robot { powerBackLeft = powerBackLeft/extreme; } +// double powerControlThreshold = 0.6 * ; +// +// if (Math.min(extreme, 1) > powerControlThreshold) { +// powerForwardLeft *= powerControlThreshold; +// powerForwardRight *= powerControlThreshold; +// powerBackLeft *= powerControlThreshold; +// powerBackRight *= powerControlThreshold; +// } + double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight}; return powers; @@ -409,7 +521,7 @@ public class Robot { } //sets the position the motors will lock to when braking to the motor's current position. - public void setAllBrakePos() { + public void setBrakePosAll() { driveFrontLeft.setBrakePosition(); driveFrontRight.setBrakePosition(); driveBackLeft.setBrakePosition(); @@ -418,17 +530,21 @@ public class Robot { //Outputs the power necessary to turn and face a provided direction public double[] getFacePowers(float direction, double power) { + angularVelocity = imu.getAngularVelocity().xRotationRate; double relativeAngle = getRelativeAngle(direction, rotation); double scaler = Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle; + if (relativeAngle != 0) { + double momentumRelative = angularVelocity * (relativeAngle / Math.abs(relativeAngle)); + double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); + double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential); + + scaler *= momentumCorrection; + } + double left = -power * scaler; double right = power *scaler; - -// if (relativeAngle > 0) { -// left *= -1; -// right *= -1; -// } double[] powers = {left,right}; return powers; } @@ -442,7 +558,7 @@ public class Robot { //towards the target angle //-------------------------------------------------------------------------------------- - double turnPowerCorrection = Math.pow(0.03 * relativeAngle, 3) + 0.02 * relativeAngle; + 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; @@ -463,7 +579,7 @@ public class Robot { // } public void record() { - TestingRecord+="\n"+rotation; + TestingRecord+="\n"+angularVelocity; } public void saveRecording() { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpEngine.java index 2549857..ba81c07 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpEngine.java @@ -1,4 +1,10 @@ package org.timecrafters.UltimateGoal; -public class TeleOpEngine { +import org.cyberarm.engine.V2.CyberarmEngine; + +public class TeleOpEngine extends CyberarmEngine { + @Override + public void setup() { + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpState.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpState.java index 892949d..d3053f6 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/TeleOpState.java @@ -1,4 +1,83 @@ package org.timecrafters.UltimateGoal; -public class TeleOpState { +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Autonomous.DriveToCoordinates; + +public class TeleOpState extends CyberarmState { + + private Robot robot; + private float leftJoystickDegrees; + private double leftJoystickMagnitude; + private float rightJoystickDegrees; + private double rightJoystickMagnitude; + + private double POWER_SPRINT = 0.4; + private double POWER_NORMAL = 0.2; + 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 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.right_stick_x; + double rightJoystickY = engine.gamepad1.right_stick_y; + + rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)); + rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); + + 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) { + + } + } + + } }