diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/Break.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/Break.java deleted file mode 100644 index f8974cd..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/Break.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.timecrafters.UltimateGoal.Calibration; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; - -import org.cyberarm.engine.V2.CyberarmState; - -public class Break extends CyberarmState { - - public DcMotor driveBackLeft; - public DcMotor driveFrontLeft; - public DcMotor driveBackRight; - public DcMotor driveFrontRight; - - @Override - public void init() { - driveFrontLeft = engine.hardwareMap.dcMotor.get("driveFrontLeft"); - driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight"); - driveBackLeft = engine.hardwareMap.dcMotor.get("driveBackLeft"); - driveBackRight = engine.hardwareMap.dcMotor.get("driveBackRight"); - - driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); - driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE); - - driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } - - @Override - public void start() { - - } - - @Override - public void exec() { - driveFrontLeft.setPower(0); - driveFrontRight.setPower(0); - driveBackLeft.setPower(0); - driveBackRight.setPower(0); - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrateRingBeltLoop.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrateRingBeltLoop.java new file mode 100644 index 0000000..4cbff57 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrateRingBeltLoop.java @@ -0,0 +1,58 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class CalibrateRingBeltLoop extends CyberarmState { + + private Robot robot; + private float startingTick; + private int currentTick; + private double ticksPerLoop; + private boolean limit; + private boolean hasSeenLimit; + private boolean limitPrevious; + + public CalibrateRingBeltLoop(Robot robot) { + this.robot = robot; + } + + @Override + public void exec() { + + currentTick = robot.ringBeltMotor.getCurrentPosition(); + limit = robot.limitSwitch.isPressed(); + + if (engine.gamepad1.x || (engine.gamepad1.a && !limit)) { + robot.ringBeltMotor.setPower(0.5); + } else { + robot.ringBeltMotor.setPower(0); + } + + if (engine.gamepad1.y) { + robot.ringBeltMotor.setPower(-0.1); + } else { + robot.ringBeltMotor.setPower(0); + } + + if (engine.gamepad1.b) { + startingTick = currentTick; + } + + if (limit && !limitPrevious){ + if (hasSeenLimit) { + ticksPerLoop = currentTick - startingTick; + } + startingTick = currentTick; + hasSeenLimit = true; + } + limitPrevious = limit; + } + + @Override + public void telemetry() { + engine.telemetry.addData("ticks", currentTick - startingTick); + engine.telemetry.addData("limit swich", limit ); + engine.telemetry.addData("Clockwise", ticksPerLoop); + } +} 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 75daad4..71939f0 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java @@ -19,6 +19,6 @@ public class CalibrationEngine extends CyberarmEngine { @Override public void setup() { - addState(new VelocityTest(robot)); + addState(new CalibrateRingBeltLoop(robot)); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MotorCheck.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MotorCheck.java new file mode 100644 index 0000000..321534b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MotorCheck.java @@ -0,0 +1,59 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Engagable; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.DriveMotor; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class MotorCheck extends CyberarmState { + + private Robot robot; + private double[] Powers; + private DcMotor[] Motors; + private int currentMotor = 0; + private double currentPower = 0.001; + + private boolean yPrevious; + + + public MotorCheck(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight}; + } + + @Override + public void start() { + Motors[currentMotor].setPower(0.2); + } + + @Override + public void exec() { + + if (currentMotor <= 3) { + + boolean y = engine.gamepad1.y; + if (y && !yPrevious) { + + Motors[currentMotor].setPower(0); + currentMotor += 1; + Motors[currentMotor].setPower(0.2); + sleep(20); + } + yPrevious = y; + } + + } + + @Override + public void telemetry() { + + engine.telemetry.addData("current Motor", Motors[currentMotor].toString()); + + } +} 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 b879417..90d4ee4 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java @@ -6,10 +6,13 @@ import org.timecrafters.UltimateGoal.Competition.Robot; public class OdometryCalibration extends CyberarmState { private Robot robot; - private float rotation; + private float rotation = 0; + private float rotationPrev = 0; private int currentTick; private double ticksPerDegreeClockwise; private double ticksPerDegreeCounterClockwise; + private long timePrevious; + private long timeChange; public OdometryCalibration(Robot robot) { this.robot = robot; @@ -17,21 +20,37 @@ public class OdometryCalibration extends CyberarmState { @Override public void exec() { + long timeCurrent = System.currentTimeMillis(); + timeChange = timeCurrent - timePrevious; - double power = 0.1; - rotation = -robot.imu.getAngularOrientation().firstAngle; - currentTick = robot.encoderBack.getCurrentPosition(); + if (timeChange >= 1200) { + timePrevious = timeCurrent; - if (engine.gamepad1.x) { - robot.setDrivePower(power, -power, power, -power); - ticksPerDegreeClockwise = currentTick/rotation; - } else if(engine.gamepad1.y) { - robot.setDrivePower(-power, power, -power, power); - ticksPerDegreeCounterClockwise = currentTick/rotation; - } else { - robot.setDrivePower(0,0,0,0); + + double power = 0.25; + float imu = -robot.imu.getAngularOrientation().firstAngle; + rotation += robot.getRelativeAngle(imu, rotationPrev); + rotationPrev = imu; + + currentTick = robot.encoderBack.getCurrentPosition(); + + if (engine.gamepad1.x) { + robot.setDrivePower(power, -power, power, -power); + ticksPerDegreeClockwise = currentTick / rotation; + } else if (engine.gamepad1.y) { + robot.setDrivePower(-power, power, -power, power); + ticksPerDegreeCounterClockwise = currentTick / rotation; + } else { + robot.setDrivePower(0, 0, 0, 0); + } } + if (engine.gamepad1.b) { + robot.record("Clockwise : "+ticksPerDegreeClockwise); + robot.record("Counter Clockwise : "+ticksPerDegreeCounterClockwise); + robot.saveRecording(); + setHasFinished(true); + } } @Override 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 95802c4..23ae148 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java @@ -19,7 +19,7 @@ public class PerformanceTest extends CyberarmState { @Override public void exec() { robot.updateLocation(); - robot.record(); +// robot.record(); if (engine.gamepad1.x) { double[] powers = robot.getMecanumPowers(0, 1, 0); diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java deleted file mode 100644 index f99aea0..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java +++ /dev/null @@ -1,70 +0,0 @@ -package org.timecrafters.UltimateGoal.Calibration; - -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Competition.Robot; - -public class StalPowerCalibrator extends CyberarmState { - - private Robot robot; - private double[] Powers; - private DcMotor[] Motors; - private int currentMotor = 0; - private double currentPower = 0.001; - - - - public StalPowerCalibrator(Robot robot) { - this.robot = robot; - } - - @Override - public void init() { -// Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight}; - Powers = new double[] {0,0,0,0}; - - for (DcMotor motor : Motors) { - motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - } - } - - @Override - public void exec() { - - if (currentMotor <= 3) { - - - - if (Math.abs(Motors[currentMotor].getCurrentPosition()) < 10 && currentPower < 1) { - currentPower += 0.001; - Motors[currentMotor].setPower(currentPower); - sleep(20); - } else { - Powers[currentMotor] = currentPower; - Motors[currentMotor].setPower(0); - currentPower = 0.001; - currentMotor += 1; - sleep(1000); - for (DcMotor motor : Motors) { - motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - } - } - } - - } - - @Override - public void telemetry() { - - engine.telemetry.addData("current Power", currentPower); - engine.telemetry.addData("current Motor", currentMotor); - - engine.telemetry.addData("FrontLeft", Powers[0]); - engine.telemetry.addData("FrontRight", Powers[1]); - engine.telemetry.addData("BackLeft", Powers[2]); - engine.telemetry.addData("BackRight", Powers[3]); - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java deleted file mode 100644 index 9727d6f..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.timecrafters.UltimateGoal.Calibration; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Competition.DriveMotor; -import org.timecrafters.UltimateGoal.Competition.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/Competition/Autonomous/AutoEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java index cc7df02..5c4cfa0 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java @@ -1,4 +1,27 @@ package org.timecrafters.UltimateGoal.Competition.Autonomous; -public class AutoEngine { +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; + +@Autonomous (name = "Autonomous") +public class AutoEngine extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + super.init(); + } + + @Override + public void setup() { + //drive to view + addState(new DriveToCoordinates(robot, "auto", "001_0")); + addState(new DriveToCoordinates(robot, "auto", "001_0")); + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java index 32989af..3fab97d 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java @@ -11,11 +11,12 @@ public class DriveToCoordinates extends CyberarmState { private double xTarget; private double yTarget; private float faceAngle; - private double tolerance; + private double tolerancePos; private double power; private boolean braking; private long breakStartTime; private long breakTime; + private boolean autoFace; public DriveToCoordinates(Robot robot, String groupName, String actionName) { this.robot = robot; @@ -23,11 +24,12 @@ public class DriveToCoordinates extends CyberarmState { this.actionName = actionName; } - public DriveToCoordinates(double xTarget, double yTarget, float faceAngle, double tolerance, double power, long breakStartTime) { + public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long breakStartTime) { + this.robot = robot; this.xTarget = xTarget; this.yTarget = yTarget; this.faceAngle = faceAngle; - this.tolerance = tolerance; + this.tolerancePos = tolerance; this.power = power; this.breakTime = breakTime; } @@ -35,18 +37,25 @@ public class DriveToCoordinates extends CyberarmState { @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()); + xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value()); + yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").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(); + tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value()); + breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value(); + + try { + faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value(); + } catch (NullPointerException e) { + autoFace = true; + } } } @Override public void start() { - + if (autoFace) { + faceAngle = robot.getAngleToPosition(xTarget,yTarget); + } } @Override @@ -54,18 +63,17 @@ public class DriveToCoordinates extends CyberarmState { double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY()); - if (distanceToTarget > tolerance) { + if (distanceToTarget > tolerancePos) { double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle); robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]); braking = false; } else { + long currentTime = System.currentTimeMillis(); if (!braking) { - breakStartTime = System.currentTimeMillis(); - robot.setBrakePosAll(); + breakStartTime = currentTime; braking = true; } - robot.brakeAll(); - setHasFinished(System.currentTimeMillis() - breakStartTime > breakTime); + setHasFinished(currentTime - breakStartTime >= breakTime); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToPosition.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToPosition.java deleted file mode 100644 index fb97cea..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToPosition.java +++ /dev/null @@ -1,66 +0,0 @@ -package org.timecrafters.UltimateGoal.Competition.Autonomous; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Competition.Robot; - -public class DriveToPosition extends CyberarmState { - - private Robot robot; - private String groupName; - private String actionName; - private double targetX; - private double targetY; - private double translation; - private double tolerance; - private double power; - - public DriveToPosition(Robot robot, String groupName, String actionName) { - this.robot = robot; - this.groupName = groupName; - this.actionName = actionName; - } - - @Override - public void init() { -// translation = robot.inchesToTicks(4); - targetX = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"x").value()); - targetY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"y").value()); - tolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"tolerance").value()); - power = robot.stateConfiguration.variable(groupName,actionName,"power").value(); - } - - @Override - public void start() { - - } - - @Override - public void exec() { - robot.updateLocation(); -// robot.record(); - - double distanceRemaining = Math.hypot(targetX - robot.getLocationX(), targetY - robot.getLocationY()); - - //Math.pow(3, distanceRemaining - translation) + 0.3; - -// if (power > 0.65) { -// power = 0.65; -// } - - if (distanceRemaining < tolerance) { - robot.setDrivePower(0, 0, 0, 0); - setHasFinished(true); - } else { -// robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power); - } - - } - - @Override - public void telemetry() { - engine.telemetry.addData("Target Visible", robot.trackableVisible); - engine.telemetry.addData("Odo X", robot.ticksToInches(robot.getLocationX())); - engine.telemetry.addData("Odo Y", robot.ticksToInches(robot.getLocationY())); - engine.telemetry.addData(" Rotation", robot.getRotation()); - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java new file mode 100644 index 0000000..5d27137 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java @@ -0,0 +1,73 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class Face extends CyberarmState { + + private Robot robot; + private String groupName = "manual"; + private String actionName; + private float faceAngle; + private double toleranceFace; + private double power; + private boolean braking; + private long breakStartTime; + private long breakTime; + private boolean autoFace; + + public Face(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + public Face(Robot robot, float faceAngle, double toleranceFace, double power, long breakTime) { + this.robot = robot; + this.faceAngle = faceAngle; + this.toleranceFace = toleranceFace; + this.power = power; + this.breakTime = breakTime; + } + + @Override + public void init() { + if (!groupName.equals("manual")) { + power = robot.stateConfiguration.variable(groupName, actionName, "power").value(); + toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value(); + breakTime = robot.stateConfiguration.variable(groupName, actionName, "brake MS").value(); + + try { + faceAngle = robot.stateConfiguration.variable(groupName, actionName, "faceAngle").value(); + } catch (NullPointerException e) { + autoFace = true; + } + } + } + + @Override + public void start() { + if (autoFace) { + double x = robot.stateConfiguration.variable(groupName, actionName, "faceX").value(); + double y = robot.stateConfiguration.variable(groupName, actionName, "faceY").value(); + faceAngle = robot.getAngleToPosition(x,y); + } + } + + @Override + public void exec() { + + if (Math.abs(robot.getRelativeAngle(faceAngle,robot.getRotation())) > toleranceFace) { + double[] powers = robot.getFacePowers(faceAngle,power); + robot.setDrivePower(powers[0], powers[1],powers[0],powers[1]); + braking = false; + } else { + long currentTime = System.currentTimeMillis(); + if (!braking) { + breakStartTime = currentTime; + braking = true; + } + setHasFinished(currentTime - breakStartTime >= breakTime); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java new file mode 100644 index 0000000..803348f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java @@ -0,0 +1,59 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.timecrafters.UltimateGoal.Competition.Robot; + +import java.util.ArrayList; +import java.util.List; + +public class TensorFlowCheck extends CyberarmState { + + private Robot robot; + private String group; + private String action; + private List recognitions; + private int path = 0; + private long checkTime; + public double wobblePosX; + public double wobblePosY; + + + + @Override + public void init() { + checkTime = robot.stateConfiguration.variable(group,action,"time").value(); + } + + @Override + public void start() { + + } + + @Override + public void exec() { + recognitions = robot.tfObjectDetector.getUpdatedRecognitions(); + + if (recognitions != null) { + + if (recognitions.size() == 1) { + Recognition recognition = recognitions.get(0); + String label = recognition.getLabel(); + if (label.equals("Single")) { + path = 1; + } else if (label.equals("Quad")) { + path = 2; + } + } else if (recognitions.size() > 1) { + path = 1; + } + } + + if (runTime() >= checkTime) { + if (checkTime == 0) { + wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos1","x").value(); + } + } + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/CamServo.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/CamServo.java new file mode 100644 index 0000000..c381a2c --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/CamServo.java @@ -0,0 +1,47 @@ +package org.timecrafters.UltimateGoal.Competition; + +import org.cyberarm.engine.V2.CyberarmState; + +public class CamServo extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private boolean open; + private long waitTime = -1; + + public CamServo(Robot robot, String groupName, String actionName, boolean open) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + this.open = open; + } + + public CamServo(Robot robot, boolean armUp, long waitTime) { + this.robot = robot; + this.open = armUp; + this.waitTime = waitTime; + } + + @Override + public void init() { + if (waitTime == -1) { + waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value(); + } + } + + @Override + public void start() { + if (open) { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX); + } else { + robot.wobbleGrabServo.setPosition(0); + } + } + + @Override + public void exec() { + setHasFinished(runTime() > waitTime); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java index 192066c..43db37b 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java @@ -10,7 +10,7 @@ public class DriveMotor { private int brakePosition; private MotorConfigurationType motorType; private DcMotorSimple.Direction direction; - static final double FINE_CORRECTION = 0.02; + static final double FINE_CORRECTION = 0.05; static final double LARGE_CORRECTION = 0.03; public DriveMotor(DcMotor motor, MotorConfigurationType motorType, DcMotorSimple.Direction direction) { @@ -20,9 +20,9 @@ public class DriveMotor { } public void init() { - motor.setMotorType(motorType); - motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// motor.setMotorType(motorType); +// motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); motor.setDirection(direction); } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java index 9ba35b3..1b114e1 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java @@ -1,4 +1,39 @@ package org.timecrafters.UltimateGoal.Competition; -public class Launch { +import org.cyberarm.engine.V2.CyberarmState; + +public class Launch extends CyberarmState { + + private Robot robot; + boolean hasCycled; + boolean detectedPass; + + public Launch(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + robot.ringBeltOn(); + } + + @Override + public void exec() { + //detect when limit switch is initially triggered + boolean detectingPass = robot.limitSwitch.isPressed(); + if (detectingPass && !detectedPass) { + //finish once the ring belt has cycled all the way through and then returned to + //the first receiving position. + if (hasCycled) { + robot.ringBeltMotor.setPower(0); + robot.ringBeltStage = 0; + setHasFinished(true); + } else { + hasCycled = true; + } + } + detectedPass = detectingPass; + +// if (robot.getBeltPos() > robot.loopPos(Robot.RING_BELT_GAP * 3) && hasCycled); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java new file mode 100644 index 0000000..d72229e --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java @@ -0,0 +1,26 @@ +package org.timecrafters.UltimateGoal.Competition.PreInit; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class FindLimitSwitch extends CyberarmState { + + private Robot robot; + + public FindLimitSwitch(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + robot.ringBeltOn(); + } + + @Override + public void exec() { + if (robot.limitSwitch.isPressed()) { + robot.ringBeltMotor.setPower(0); + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java new file mode 100644 index 0000000..d9fcbb5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java @@ -0,0 +1,30 @@ +package org.timecrafters.UltimateGoal.Competition.PreInit; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class LoadRings extends CyberarmState { + + private Robot robot; + private ProgressRingBelt ringBeltState; + private int ringCount = 0; + + public LoadRings(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + robot.collectionMotor.setPower(1); + } + + @Override + public void exec() { + ringBeltState = new ProgressRingBelt(robot); + if (engine.gamepad1.x && childrenHaveFinished()) { + addParallelState(ringBeltState); + + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java new file mode 100644 index 0000000..8693100 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java @@ -0,0 +1,25 @@ +package org.timecrafters.UltimateGoal.Competition.PreInit; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; + +@Autonomous (name = "Load Rings") +public class PreInitEngine extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + super.init(); + } + + @Override + public void setup() { + addState(new FindLimitSwitch(robot)); + addState(new LoadRings(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java new file mode 100644 index 0000000..13813c3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java @@ -0,0 +1,49 @@ +package org.timecrafters.UltimateGoal.Competition; + +import org.cyberarm.engine.V2.CyberarmState; + +public class ProgressRingBelt extends CyberarmState { + + private Robot robot; + private int targetPos; + private boolean useLaunchPrep; + + public ProgressRingBelt(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + useLaunchPrep = (robot.launchMotor.getPower() == 0); + } + + @Override + public void start() { + int currentPos = robot.getBeltPos(); + if (robot.ringBeltStage < 2) { + targetPos = robot.loopPos(currentPos + Robot.RING_BELT_GAP); + robot.ringBeltOn(); + robot.ringBeltStage += 1; + } else if (robot.ringBeltStage == 2) { + targetPos = robot.loopPos(currentPos + 160); + robot.ringBeltOn(); + robot.ringBeltStage += 1; + } + + } + + @Override + public void exec() { + int currentPos = robot.getBeltPos(); + if (currentPos >= targetPos && currentPos < targetPos + Robot.RING_BELT_GAP) { + robot.ringBeltMotor.setPower(0); + + if(useLaunchPrep) { + robot.launchMotor.setPower(Robot.LAUNCH_POWER); + } + + setHasFinished(true); + } + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java index 8dcd989..121877b 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -4,10 +4,12 @@ import android.os.Environment; import android.util.Log; import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.hardware.rev.RevTouchSensor; 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.Servo; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.ClassFactory; @@ -35,7 +37,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ; import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX; import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; -public class Robot { +public class Robot { private HardwareMap hardwareMap; @@ -46,34 +48,29 @@ public class Robot { public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration(); public BNO055IMU imu; - private WebcamName webcam; - private VuforiaLocalizer vuforia; - //drive system - - public DriveMotor driveFrontLeft; - public DriveMotor driveBackLeft; - public DriveMotor driveFrontRight; - public DriveMotor driveBackRight; + public DcMotor driveFrontLeft; + public DcMotor driveBackLeft; + public DcMotor driveFrontRight; + public DcMotor driveBackRight; public DcMotor encoderLeft; public DcMotor encoderRight; public DcMotor encoderBack; //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 FINE_CORRECTION = 0.055 ; + static final double LARGE_CORRECTION = 0.025 ; + static final double MOMENTUM_CORRECTION = 1.02; + static final double MOMENTUM_MAX_CORRECTION = 1.3; 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 double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622; 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_CLOCKWISE = Math.PI * 1000; - static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = Math.PI * 1000; + static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4; + static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6; // Inches Forward of axis of rotation static final float CAMERA_FORWARD_DISPLACEMENT = 13f; @@ -95,85 +92,117 @@ public class Robot { //Launcher public DcMotor launchMotor; + public static final double LAUNCH_POWER = 0.75; - 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); + private static final long LAUNCH_ACCEL_TIME = 500; + public static final double LAUNCH_POSITION_X = 36 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); + public static final double LAUNCH_POSITION_Y = -8 * (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_POS = 0.5 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); 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_GAP = 200; - public static final int RING_BELT_RECEIVE_POS = 100; + + //Ring Intake + public DcMotor collectionMotor; + public Rev2mDistanceSensor ringIntakeSensor; public static final double RING_DETECT_DISTANCE = 100; public static final double RING_DETECT_DELAY = 1000; + //Ring Belt + public DcMotor ringBeltMotor; + public RevTouchSensor limitSwitch; + public int ringBeltStage; + public static final int RING_BELT_LOOP_TICKS = 2544; + public static final int RING_BELT_GAP = 670; - //Debugging - public double visionX; - public double visionY; - public float rawAngle; -// private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight"; - private String TestingRecord = "AngularVelocity"; + public static final double RING_BELT_POWER = 0.2; - public double traveledLeft; - public double traveledRight; + //Wobble Goal Arm + public DcMotor wobbleArmMotor; + public Servo wobbleGrabServo; + public static final int WOBBLE_ARM_DOWN = -710; + public static final double WOBBLE_SERVO_MAX = 0.3; //vuforia navigation + private WebcamName webcam; + private VuforiaLocalizer vuforia; + public Servo webCamServo; + public static final double CAM_SERVO_UP = 0.2; + public static final double CAM_SERVO_Down = 0.2; + 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; + private long minCheckDurationMs = 500; + private int minCheckVelocity = 1; //TensorFlow Object Detection public TFObjectDetector tfObjectDetector; private static final float MINIMUM_CONFIDENCE = 0.8f; + //Debugging + public double totalV; + public double visionX; + public double visionY; + public float rawAngle; +// private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight"; + private String TestingRecord = "TicksPerDegree"; + + public double traveledLeft; + public double traveledRight; public void initHardware() { -// webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); - imu = hardwareMap.get(BNO055IMU.class, "imu"); -// limitSwitch.setMode(DigitalChannel.Mode.INPUT); + limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim"); - DcMotor dcMotor = hardwareMap.dcMotor.get("driveFrontLeft"); + driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft"); + driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight"); + driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft"); + driveBackRight = hardwareMap.dcMotor.get("driveBackRight"); - MotorConfigurationType motorType = dcMotor.getMotorType(); - motorType.setGearing(5); - motorType.setTicksPerRev(140); - motorType.setMaxRPM(800); + driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD); + driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD); + driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE); - 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); + //Localization encoders + encoderRight = hardwareMap.dcMotor.get("driveFrontRight"); + encoderBack = hardwareMap.dcMotor.get("driveFrontLeft"); + encoderLeft = hardwareMap.dcMotor.get("collect"); - driveFrontLeft.init(); - driveFrontRight.init(); - driveBackLeft.init(); - driveBackRight.init(); + encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + encoderBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + encoderBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); -// encoderLeft = hardwareMap.dcMotor.get("odoLeft"); -// encoderRight = hardwareMap.dcMotor.get("odoRight"); -// encoderBack = hardwareMap.dcMotor.get("odoBack"); + //init wobble arm + wobbleArmMotor = hardwareMap.dcMotor.get("wobbleArm"); + wobbleArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + wobbleArmMotor.setTargetPosition(0); + wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - launchMotor = hardwareMap.dcMotor.get("launcher"); + wobbleGrabServo = hardwareMap.servo.get("wobbleGrab"); + + //init ring belt collectionMotor = hardwareMap.dcMotor.get("collect"); - collectionMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE); + + //init ring belt ringBeltMotor = hardwareMap.dcMotor.get("belt"); + ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + + //init IMU + imu = hardwareMap.get(BNO055IMU.class, "imu"); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); @@ -196,18 +225,39 @@ public class Robot { imu.startAccelerationIntegration(startPosition,startVelocity, 10); -// initVuforia(); + //Init Localization + webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + initVuforia(); + webCamServo = hardwareMap.servo.get("look"); + webCamServo.setDirection(Servo.Direction.REVERSE ); 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); -// } + minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value(); + minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "minCheckMS").value(); + + //Init Launch Motor + DcMotor launcher = hardwareMap.dcMotor.get("launcher"); + + MotorConfigurationType launchMotorType = launcher.getMotorType(); + launchMotorType.setGearing(3); + launchMotorType.setTicksPerRev(84); + launchMotorType.setMaxRPM(2400); + + launchMotor = launcher; + launchMotor.setMotorType(launchMotorType); + launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + if (stateConfiguration.action("system","initLauncher").enabled) { + double launcherPower = 0; + long launchAccelStart = System.currentTimeMillis(); + while (launcherPower < LAUNCH_POWER) { + launcherPower = (double) ((System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME) * LAUNCH_POWER; + launchMotor.setPower(launcherPower); + } + } } @@ -267,7 +317,7 @@ public class Robot { int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName()); TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId); - parameters.minResultConfidence = MINIMUM_CONFIDENCE; + parameters.minResultConfidence = stateConfiguration.variable("system", "tensorFlow", "minConfidence").value(); tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia); tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single"); } @@ -279,9 +329,8 @@ public class Robot { 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 encoderLeftCurrent = -encoderLeft.getCurrentPosition(); + int encoderRightCurrent = -encoderRight.getCurrentPosition(); int encoderBackCurrent = encoderBack.getCurrentPosition(); double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious; @@ -309,8 +358,8 @@ public class Robot { } else { ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE; } - //todo: check if direction of the back encoder gives expected values - double sidewaysVector = encoderBackChange - (rotationChange* ticksPerDegree); + + double sidewaysVector = encoderBackChange + (rotationChange* ticksPerDegree); double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector)); double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector); @@ -324,14 +373,15 @@ public class Robot { rotation += rotationChange; - double totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightCurrent) + Math.abs(encoderBackChange); + totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange); - if (totalV < MIN_CHECK_VELOCITY) { + + if (totalV < minCheckVelocity) { long timeCurrent = System.currentTimeMillis(); if (timeStartZeroVelocity == 0) { timeStartZeroVelocity = timeCurrent; - } else if (timeCurrent - timeStartZeroVelocity >= MIN_CHECK_DURATION_MS) { + } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs) { syncWithVuforia(); } @@ -349,15 +399,6 @@ 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) { @@ -480,23 +521,23 @@ public class Robot { 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; +// //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); + double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); + double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential); powerForwardRight *= momentumCorrection; powerForwardLeft *= momentumCorrection; powerBackRight *= momentumCorrection; @@ -532,21 +573,6 @@ 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 setBrakePosAll() { - 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) { angularVelocity = imu.getAngularVelocity().xRotationRate; @@ -568,20 +594,21 @@ public class Robot { return powers; } + public void ringBeltOn() { + ringBeltMotor.setPower(RING_BELT_POWER); + } + public int getBeltPos(){ return loopPos(ringBeltMotor.getCurrentPosition()); } 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; + public void record(String record) { + TestingRecord+="\n"+record; } public void saveRecording() { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java index 61b313a..e102201 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java @@ -1,10 +1,32 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; -import org.cyberarm.engine.V2.CyberarmEngine; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; + +@TeleOp (name = "TeleOp",group = "comp") public class TeleOpEngine extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + robot.wobbleGrabServo.setPosition(0); + robot.webCamServo.setPosition(0); + super.init(); + } + @Override public void setup() { + addState(new TeleOpState(robot)); + } + @Override + public void stop() { + robot.deactivateVuforia(); + super.stop(); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java index 16daf0e..9f37b0f 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java @@ -1,8 +1,11 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; +import org.timecrafters.UltimateGoal.Competition.Launch; +import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.WobbleArm; +import org.timecrafters.UltimateGoal.Competition.WobbleGrab; public class TeleOpState extends CyberarmState { @@ -12,14 +15,24 @@ public class TeleOpState extends CyberarmState { private float rightJoystickDegrees; private double rightJoystickMagnitude; - private double POWER_SPRINT = 0.4; - private double POWER_NORMAL = 0.2; - private double powerScale = 0.2 ; + private double[] powers = {0,0,0,0}; + private double drivePower = 1; + private static final double TURN_POWER = 1; private boolean toggleSpeedInput = false; + private Launch launchState; + private boolean launching; + private ProgressRingBelt ringBeltState; + private boolean CollectorOn; + private boolean xPrev; + private boolean yPrev; + private boolean aPrev; + private boolean bPrev; + private boolean rbPrev; + private boolean wobbleArmUp = true; + private boolean wobbleGrabOpen = 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) { @@ -47,46 +60,117 @@ public class TeleOpState extends CyberarmState { rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)); rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); + if (leftJoystickMagnitude > 0.66) { + drivePower = 1 ; + } else { + drivePower = 0.6; + } + double[] powers = {0,0,0,0}; - 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); + boolean y = engine.gamepad1.y; + if (y) { - } else if (robot.getRelativeAngle(robot.LAUNCH_ROTATION, robot.getRotation()) > robot.LAUNCH_TOLERANCE_FACE) { - //todo add launch sequence + //Launch Sequence + + 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), drivePower, Robot.LAUNCH_ROTATION); + + } else if (robot.getRelativeAngle(Robot.LAUNCH_ROTATION, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) { + + launchState = new Launch(robot); + addParallelState(launchState); + + } else { + + double[] facePowers = robot.getFacePowers(Robot.LAUNCH_ROTATION, TURN_POWER); + powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; } - } 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 (!y || ( launchState != null && launchState.getHasFinished())) { + //Normal Driver Controls if (rightJoystickMagnitude == 0) { if (leftJoystickMagnitude !=0) { - powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, leftJoystickDegrees); + powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, leftJoystickDegrees); } } else { if (leftJoystickMagnitude == 0) { - double[] facePowers = robot.getFacePowers(rightJoystickDegrees, powerScale * rightJoystickMagnitude); + double[] facePowers = robot.getFacePowers(rightJoystickDegrees, TURN_POWER * rightJoystickMagnitude); powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; } else { - powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, rightJoystickDegrees); + powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, rightJoystickDegrees); } } } robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]); + this.powers = powers; + + + boolean x = engine.gamepad2.x; + if (x && !xPrev && childrenHaveFinished()) { + if (CollectorOn) { + robot.collectionMotor.setPower(0); + CollectorOn = false; + } else { + robot.collectionMotor.setPower(1); + CollectorOn = true; + } + } + xPrev = x; + + boolean y2 = engine.gamepad2.y; + if (y2 && !yPrev && childrenHaveFinished()) { + launchState = new Launch(robot); + addParallelState(launchState); + } + yPrev = y2; + + boolean a = engine.gamepad2.a; + if (a && !aPrev && childrenHaveFinished()) { + addParallelState(new ProgressRingBelt(robot)); + } + aPrev = a; + + boolean b = engine.gamepad2.b; + if (b && !bPrev) { + wobbleArmUp = !wobbleArmUp; + addParallelState(new WobbleArm(robot, wobbleArmUp, 100)); + } + bPrev = b; + + boolean rb = engine.gamepad2.right_bumper; + if (rb && !rbPrev) { + wobbleGrabOpen = !wobbleGrabOpen ; + addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100)); + } + rbPrev = rb; } + + @Override + public void telemetry() { + engine.telemetry.addLine("Powers"); + for (double power : powers) { + engine.telemetry.addData(" ", power); + } + + + engine.telemetry.addLine("Location"); + engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()))+","+round(robot.ticksToInches(robot.getLocationY()))+")"); + engine.telemetry.addData("Rotation ", robot.getRotation()); + engine.telemetry.addData("totalV", robot.totalV); + } + + private float round(double number) { + return ((float) Math.floor(number*100)) / 100; + } + + } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java new file mode 100644 index 0000000..96618a3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java @@ -0,0 +1,48 @@ +package org.timecrafters.UltimateGoal.Competition; + +import org.cyberarm.engine.V2.CyberarmState; + +public class WobbleArm extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private boolean armUp; + private long waitTime = -1; + + public WobbleArm(Robot robot, String groupName, String actionName, boolean armUp) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + this.armUp = armUp; + } + + public WobbleArm(Robot robot, boolean armUp, long waitTime) { + this.robot = robot; + this.armUp = armUp; + this.waitTime = waitTime; + } + + @Override + public void init() { + if (waitTime == -1) { + waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value(); + } + } + + @Override + public void start() { + robot.wobbleArmMotor.setPower(0.3); + if (armUp) { + robot.wobbleArmMotor.setTargetPosition(0); + } else { + robot.wobbleArmMotor.setTargetPosition(Robot.WOBBLE_ARM_DOWN); + } + } + + @Override + public void exec() { + setHasFinished(runTime() > waitTime); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java new file mode 100644 index 0000000..5ee929c --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java @@ -0,0 +1,47 @@ +package org.timecrafters.UltimateGoal.Competition; + +import org.cyberarm.engine.V2.CyberarmState; + +public class WobbleGrab extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private boolean open; + private long waitTime = -1; + + public WobbleGrab(Robot robot, String groupName, String actionName, boolean open) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + this.open = open; + } + + public WobbleGrab(Robot robot, boolean armUp, long waitTime) { + this.robot = robot; + this.open = armUp; + this.waitTime = waitTime; + } + + @Override + public void init() { + if (waitTime == -1) { + waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value(); + } + } + + @Override + public void start() { + if (open) { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX); + } else { + robot.wobbleGrabServo.setPosition(0); + } + } + + @Override + public void exec() { + setHasFinished(runTime() > waitTime); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/FullTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/FullTest.java deleted file mode 100644 index dc38892..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/FullTest.java +++ /dev/null @@ -1,39 +0,0 @@ -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 9247355..eaeaf3c 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java @@ -49,7 +49,7 @@ public class MecanumFunctionTest extends CyberarmState { @Override public void exec() { robot.updateLocation(); - robot.record(); +// robot.record();///////////// AngularVelocity angularVelocity = robot.imu.getAngularVelocity(); @@ -129,9 +129,9 @@ public class MecanumFunctionTest extends CyberarmState { 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()); + engine.telemetry.addData("FL", robot.driveFrontLeft.getPower()); + engine.telemetry.addData("FR", robot.driveFrontRight.getPower()); + engine.telemetry.addData("BL", robot.driveBackLeft.getPower()); + engine.telemetry.addData("BR", robot.driveBackRight.getPower()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java new file mode 100644 index 0000000..f00af01 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java @@ -0,0 +1,36 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.robotcore.hardware.Servo; + +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.Competition.Robot; + + +public class ServoPosTest extends CyberarmState { + + private Servo servo; + private double servoPos; + + public ServoPosTest() { + } + + @Override + public void init() { + servo = engine.hardwareMap.servo.get("look"); + servo.setDirection(Servo.Direction.REVERSE ); + } + + @Override + public void exec() { + servoPos = engine.gamepad1.right_stick_y * 0.2; + servo.setPosition(servoPos); + } + + @Override + public void telemetry() { + engine.telemetry.addData("Position", servoPos); + } +} 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 139f9a7..cc27030 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -8,19 +8,19 @@ import org.timecrafters.UltimateGoal.Competition.Robot; @TeleOp (name = "Hardware test", group = "test") public class TestingEngine extends CyberarmEngine { - private Robot robot; - @Override - public void init() { - robot = new Robot(hardwareMap); - robot.initHardware(); - super.init(); - } +// private Robot robot; +// @Override +// public void init() { +// robot = new Robot(hardwareMap); +// robot.initHardware(); +// super.init(); +// } @Override public void setup() { - addState(new FullTest(robot)); + addState(new ServoPosTest()); } -// + // @Override // public void stop() { // robot.saveRecording(); diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WobbleArmTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WobbleArmTest.java new file mode 100644 index 0000000..2c07e08 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WobbleArmTest.java @@ -0,0 +1,30 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +import org.cyberarm.engine.V2.CyberarmState; + + +public class WobbleArmTest extends CyberarmState { + + private int currentPos; + private DcMotor motor; + + @Override + public void init() { + motor = engine.hardwareMap.dcMotor.get("wobbleArm"); + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + @Override + public void exec() { + currentPos = motor.getCurrentPosition(); + } + + @Override + public void telemetry() { + engine.telemetry.addData("Position", currentPos); + } +}