diff --git a/.idea/compiler.xml b/.idea/compiler.xml index fb7f4a8..61a9130 100644 --- a/.idea/compiler.xml +++ b/.idea/compiler.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml index 860da66..d5d35ec 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,6 +1,6 @@ - + diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java index 4a03a27..1ac173a 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -150,7 +150,7 @@ public abstract class CyberarmEngine extends OpMode { * Recursively stop states * @param state State to stop */ - private void stopState(CyberarmState state) { + public void stopState(CyberarmState state) { state.setHasFinished(true); state.stop(); 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 d24cf37..7f1ed4d 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java @@ -1,10 +1,11 @@ package org.timecrafters.UltimateGoal.Calibration; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.UltimateGoal.Competition.Robot; - +@Disabled @TeleOp (name = "Calibration", group = "test") public class CalibrationEngine extends CyberarmEngine { private Robot robot; 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 0a26eb4..d1e542f 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 @@ -35,7 +35,7 @@ public class AutoEngine extends CyberarmEngine { // since we've preloaded three rings, the ring belt stage is set to account for this; robot.ringBeltStage = 3; - //Autonomous specific Variables + //Autonomous specific Configuration Variables float rotation = robot.stateConfiguration.variable("system", "startPos", "direction").value(); double locationX = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "x").value()); double locationY = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "y").value()); diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java index c5bd4f6..5cc594a 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java @@ -1,5 +1,9 @@ package org.timecrafters.UltimateGoal.Competition.Autonomous; +/* +The FindWobbleGoal state is used in teleOp and Autonomous to aid in capturing the wobble goal. +*/ + import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.UltimateGoal.Competition.Robot; @@ -47,6 +51,7 @@ public class FindWobbleGoal extends CyberarmState { robot.updateLocation(); double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM); + //Stage 1: scan back and forth untile the sensor is in line with the wobble goal. if (sensorValue > turnCheck) { float rotation = robot.getRelativeAngle(startRotation,robot.getRotation()); @@ -65,18 +70,23 @@ public class FindWobbleGoal extends CyberarmState { ccCheckPrev = ccCheck; } else { + //Stage 2: drive toward wobble goal until it's close enough to grab if (sensorValue > driveCheck) { if (!foundGoalRotation) { foundGoalRotation = true; wobbleGoalRotation = robot.getRotation(); } - double[] powers = robot.getMecanumPowers(wobbleGoalRotation - 90, power*2 , wobbleGoalRotation); + double[] powers = robot.getMecanumPowers( + wobbleGoalRotation - 90, + power*2 , wobbleGoalRotation); robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]); } else { + //stage 3: grab the wobble goal && finish the state endSearch(); } } + //if the search takes too long, the robot grabs and finishes the state if (runTime() > timeLimit) { endSearch(); } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoControl.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoControl.java deleted file mode 100644 index 82a1ed6..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoControl.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.timecrafters.UltimateGoal.Competition.Demo; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.UltimateGoal.Competition.Robot; -import org.timecrafters.UltimateGoal.Competition.TeleOp.Player1; -import org.timecrafters.UltimateGoal.Competition.TeleOp.Player2; - -public class DemoControl extends CyberarmState { - - private Robot robot; - - public DemoControl(Robot robot) { - this.robot = robot; - } - - @Override - public void start() { -// addParallelState(new Demo1(robot)); -// addParallelState(new Demo2(robot)); - } - - @Override - public void exec() { - - } - - @Override - public void telemetry() { - - engine.telemetry.addLine("Location"); - engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")"); - engine.telemetry.addData("Rotation ", robot.getRotation()); - } - - private float round(double number,double unit) { - return (float) (Math.floor(number/unit) * unit); - } - -} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java index 3145699..c7c2b3b 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java @@ -1,14 +1,12 @@ package org.timecrafters.UltimateGoal.Competition.Demo; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.UltimateGoal.Competition.Robot; import org.timecrafters.UltimateGoal.Competition.TeleOp.TeleOpState; -@Disabled -@TeleOp (name = "Demo") +@TeleOp (name = "Demo: Game",group = "demo") public class DemoEngine extends CyberarmEngine { private Robot robot; @@ -17,14 +15,14 @@ public class DemoEngine extends CyberarmEngine { public void init() { robot = new Robot(hardwareMap); robot.initHardware(); - robot.wobbleGrabServo.setPosition(0); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); robot.webCamServo.setPosition(0); super.init(); } @Override public void setup() { - addState(new DemoControl(robot)); + addState(new DemoState(robot)); } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngineTank.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngineTank.java new file mode 100644 index 0000000..3652ed3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngineTank.java @@ -0,0 +1,33 @@ +package org.timecrafters.UltimateGoal.Competition.Demo; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; + +@TeleOp (name = "Demo: Tank",group = "demo") +public class DemoEngineTank extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + robot.webCamServo.setPosition(0); + super.init(); + } + + @Override + public void setup() { + addState(new DemoStateTank(robot)); + } + + @Override + public void stop() { + robot.deactivateVuforia(); + robot.saveRecording(); + super.stop(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoState.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoState.java new file mode 100644 index 0000000..e3007c1 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoState.java @@ -0,0 +1,194 @@ +package org.timecrafters.UltimateGoal.Competition.Demo; + +/* +The Player1 state has all the controls for player one. The Player One and Player Two controls are +kept in separate states so that the childrenHaveFinished() method can be used to easily stop things +from running at the same time that shouldn't be. +*/ + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl; + +public class DemoState extends CyberarmState { + private Robot robot; + + //normal drive control + private float leftJoystickDegrees; + private double leftJoystickMagnitude; + private float rightJoystickDegrees; + private double rightJoystickMagnitude; + private double rightJoystickMagnitudePrevious; + + private double faceControlThreshold; + private float cardinalSnapping; + private float pairSnapping; + + private float faceDirection = 0; + private double[] powers = {0,0,0,0}; + private double drivePower = 1; + private boolean lbPrev; + + private double refinePower; + + public DemoState(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + cardinalSnapping = robot.stateConfiguration.variable( + "tele","control", "cardinalSnapping").value(); + pairSnapping = robot.stateConfiguration.variable( + "tele","control", "pairSnapping").value(); + faceControlThreshold = robot.stateConfiguration.variable( + "tele","control", "faceControlT").value(); + refinePower = robot.stateConfiguration.variable( + "tele","control", "refPower").value(); + + } + + @Override + public void start() { + faceDirection = robot.getRotation(); + } + + @Override + public void exec() { + robot.updateLocation(); + + Gamepad gamepad = engine.gamepad1; + if (engine.gamepad2.right_trigger != 0) { + gamepad = engine.gamepad2; + } + + //toggle for drive speed + boolean lb = engine.gamepad2.left_stick_button; + if (lb && !lbPrev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + lbPrev = lb; + + //Calculate Joystick Vector + double leftJoystickX = gamepad.left_stick_x; + double leftJoystickY = gamepad.left_stick_y; + + leftJoystickDegrees = robot.getRelativeAngle(0, + (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY))); + leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY); + + double rightJoystickX = gamepad.right_stick_x; + double rightJoystickY = gamepad.right_stick_y; + + rightJoystickDegrees = robot.getRelativeAngle(0, + (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY))); + rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); + + //allows the the driver to indicate which direction the robot is currently looking + //so that the controller and robot can be re-synced in the event of a bad initial + //rotation. + if (gamepad.back) { + float newRotation = 0; + + if (rightJoystickMagnitude != 0) { + newRotation = rightJoystickDegrees; + } + + robot.setLocalization(newRotation, robot.getLocationX(), robot.getLocationY()); + faceDirection = newRotation; + } + + //if the driver is letting go of the face joystick, the robot should maintain it's + //current face direction. + if (rightJoystickMagnitude > faceControlThreshold || + rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) { + + //if the joystick is close to one of the cardinal directions, it is set to exactly + // the cardinal direction + faceDirection = snapToCardinal(rightJoystickDegrees, cardinalSnapping, 0); + } + rightJoystickMagnitudePrevious = rightJoystickMagnitude; + + //The D-pad is used if the drivers need to get a more precise angle than they can get + //using the face joystick + if (gamepad.dpad_right) { + powers = new double[]{refinePower, -refinePower, refinePower, -refinePower}; + faceDirection = robot.getRotation(); + } else if (gamepad.dpad_left) { + powers = new double[]{-refinePower, refinePower, -refinePower, refinePower}; + faceDirection = robot.getRotation(); + } else if (leftJoystickMagnitude == 0) { + double[] facePowers = robot.getFacePowers(faceDirection, 0.4); + powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; + } else { + //drives the robot in the direction of the move joystick while facing the direction + //of the look joystick. if the move direction is almost aligned/perpendicular to the + //look joystick, + powers = robot.getMecanumPowers( + snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection), + drivePower, faceDirection); + } + + robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]); + + + //LED feedback control + double ringBeltPower = robot.ringBeltMotor.getPower(); + if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - + robot.ringBeltMotor.getCurrentPosition()) > 10) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE ); + } else if (ringBeltPower < 0) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED); + } else { + if (drivePower == 1) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN); + } else { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GOLD); + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine("Controler Directions"); + engine.telemetry.addData("Right", rightJoystickDegrees); + engine.telemetry.addData("Left", leftJoystickDegrees); + + engine.telemetry.addData("face", faceDirection); + + engine.telemetry.addData("Player 1 children", childrenHaveFinished()); + for (CyberarmState state : children) { + if (!state.getHasFinished()) { + engine.telemetry.addLine("" + state.getClass()); + } + } + } + + //This just checks if the wobble arm runmode is already the desired mode before setting it. + //I don't know if this is actually helpful + private void setArmMode(DcMotor.RunMode runMode) { + if (robot.wobbleArmMotor.getMode() != runMode) { + robot.wobbleArmMotor.setMode(runMode); + } + } + + private float snapToCardinal(float angle, float snapping, float offset) { + int o = (int) offset + 180; + o %= 90; + for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) { + if (angle >= cardinal-snapping && angle <= cardinal+snapping) { + angle = cardinal; + } + } + return angle; + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoStateTank.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoStateTank.java new file mode 100644 index 0000000..ecd65e0 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoStateTank.java @@ -0,0 +1,55 @@ +package org.timecrafters.UltimateGoal.Competition.Demo; + +/* +The Player1 state has all the controls for player one. The Player One and Player Two controls are +kept in separate states so that the childrenHaveFinished() method can be used to easily stop things +from running at the same time that shouldn't be. +*/ + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl; + +public class DemoStateTank extends CyberarmState { + private Robot robot; + + private double drivePower = 1; + private boolean lbPrev; + + + public DemoStateTank(Robot robot) { + this.robot = robot; + } + + @Override + public void exec() { + robot.updateLocation(); + + Gamepad gamepad = engine.gamepad1; + if (engine.gamepad2.right_bumper) { + gamepad = engine.gamepad2; + } + + //toggle for drive speed + boolean lb = engine.gamepad2.left_stick_button; + if (lb && !lbPrev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + lbPrev = lb; + + double left = -drivePower * gamepad.left_stick_y; + double right = -drivePower * gamepad.right_stick_y; + robot.setDrivePower(left,right,left,right); + + } + +} 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 1d003c7..9b7d500 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java @@ -1,6 +1,10 @@ package org.timecrafters.UltimateGoal.Competition; -import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +/* +The Launch state is used in teleOp and Autonomous. In addition to launching any rings by cycling the +ring belt, this state returns the ring belt to the starting position +*/ + import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; @@ -45,10 +49,7 @@ public class Launch extends CyberarmState { @Override public void exec() { - //detect when limit switch is initially triggered - boolean detectingPass = robot.limitSwitch.isPressed(); - int beltPos = robot.ringBeltMotor.getCurrentPosition(); - + //jam counter measures if (robot.beltIsStuck() && childrenHaveFinished()) { long currentTime = System.currentTimeMillis(); if (stuckStartTime == 0) { @@ -60,6 +61,9 @@ public class Launch extends CyberarmState { stuckStartTime = 0; } + //detect when limit switch is initially triggered + boolean detectingPass = robot.limitSwitch.isPressed(); + int beltPos = robot.ringBeltMotor.getCurrentPosition(); if (detectingPass && !detectedPass) { //finish once the ring belt has cycled all the way through and then returned to //the first receiving position. diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java index 102dcf0..5199b93 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java @@ -16,7 +16,7 @@ public class Pause extends CyberarmState { this.actionName = actionName; } - public Pause(Robot robot, boolean open, long waitTime) { + public Pause(Robot robot, long waitTime) { this.robot = robot; this.waitTime = waitTime; } 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 index 8693100..64ba7e9 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java @@ -1,10 +1,11 @@ package org.timecrafters.UltimateGoal.Competition.PreInit; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.UltimateGoal.Competition.Robot; - +@Disabled @Autonomous (name = "Load Rings") public class PreInitEngine extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java index 4df0635..283f72d 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java @@ -1,8 +1,11 @@ package org.timecrafters.UltimateGoal.Competition; +/* +The ProgressRingBelt state is used in teleOp to automatically move the ring belt. +*/ + import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.robotcore.hardware.DcMotor; - import org.cyberarm.engine.V2.CyberarmState; public class ProgressRingBelt extends CyberarmState { @@ -27,13 +30,20 @@ public class ProgressRingBelt extends CyberarmState { @Override public void start() { int currentPos = robot.ringBeltMotor.getCurrentPosition(); + + //The first two progressions should move to preprare for another ring. if (robot.ringBeltStage < 2) { targetPos = currentPos + robot.ringBeltGap; prep(); + + //The third progression needs to only move forward enought to block the ring from + //falling out } else if (robot.ringBeltStage == 2) { targetPos = currentPos + 150; prep(); prepLaunch = !robot.initLauncher; + + //If the ring belt is already full, It does allow another progression } else if (robot.ringBeltStage > 2) { setHasFinished(true); } @@ -44,6 +54,7 @@ public class ProgressRingBelt extends CyberarmState { @Override public void exec() { + //finished state when the target position is reached int currentPos = robot.ringBeltMotor.getCurrentPosition(); if (currentPos >= targetPos) { if(prepLaunch) { @@ -53,6 +64,7 @@ public class ProgressRingBelt extends CyberarmState { setHasFinished(true); } + //belt jam mitigation if (robot.beltIsStuck() && childrenHaveFinished()) { long currentTime = System.currentTimeMillis(); if (stuckStartTime == 0) { 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 89d619a..359665e 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -1,4 +1,4 @@ -package org.timecrafters.UltimateGoal.Competition; + package org.timecrafters.UltimateGoal.Competition; /* The robot object contains all the hardware and functions that are used in both teleOp and @@ -14,6 +14,7 @@ import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -53,7 +54,8 @@ public class Robot { } //The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit - //variables saved on the phone, without having to re-download the whole program. + //variables saved on the phone, without having to re-download the whole program. This is + //especially useful for autonomous route tuning public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration(); //We use the IMU to get reliable rotation and angular velocity information. Experimentation has @@ -74,6 +76,12 @@ public class Robot { public DcMotor encoderBack; //Motion Constants + + //related graphs + //https://www.desmos.com/calculator/gndnkjndu9 + //https://www.desmos.com/calculator/w0rebnftvg + //https://www.desmos.com/calculator/qxa1rq8hrv + static final double CUBIC_CORRECTION = 0.035; static final double FACE_CUBIC_CORRECTION = 0.025; static final double LINEAR_CORRECTION = 0.055; @@ -81,10 +89,12 @@ public class Robot { static final double FACE_LINEAR_CORRECTION = 0.025; static final double MOMENTUM_CORRECTION = 1.05; static final double MOMENTUM_MAX_CORRECTION = 1.4; - static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION)); + static final double MOMENTUM_HORIZONTAL_CORRECTION = + -(Math.log10(MOMENTUM_MAX_CORRECTION-1) / Math.log10(MOMENTUM_CORRECTION)); static final double FACE_MOMENTUM_MAX_CORRECTION = 1.1; static final double FACE_MOMENTUM_CORRECTION = 1.06; - static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1)/Math.log10(FACE_MOMENTUM_CORRECTION)); + static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION = + -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1) / Math.log10(FACE_MOMENTUM_CORRECTION)); static final double ZERO_POWER_THRESHOLD = 0.25; //Unit Conversion Constants @@ -107,7 +117,7 @@ public class Robot { private float rotationPrevious = 0; public float angularVelocity; - //vuforia navigation + //vuforia && tensorFlow Stuff private WebcamName webcam; private VuforiaLocalizer vuforia; @@ -118,8 +128,10 @@ public class Robot { // Inches Left of axis of rotation static final float CAMERA_LEFT_DISPLACEMENT = 4f; - static final double CAMERA_DISPLACEMENT_MAG = Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT); - static final float CAMERA_DISPLACEMENT_DIRECTION = (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT); + static final double CAMERA_DISPLACEMENT_MAG = + Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT); + static final float CAMERA_DISPLACEMENT_DIRECTION = + (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT); public boolean trackableVisible; private VuforiaTrackables targetsUltimateGoal; @@ -145,6 +157,7 @@ public class Robot { public static final double LAUNCH_POWER = 0.715; private static final long LAUNCH_ACCEL_TIME = 500; + //These variables were originally going to be used in both autonomous and teleOp public double launchPositionX; public double launchPositionY; public float launchRotation; @@ -182,22 +195,17 @@ public class Robot { //Debugging public double totalV; - public double visionX; - public double visionY; - public double visionZ; - public float rawAngle; - private String TestingRecord = "x,y"; - + private String TestingRecord = "Raw IMU, Delta, Saved"; public double forwardVector; public double sidewaysVector; - public double traveledForward = 0; - public double traveledRight; + public DcMotorEx motorAmpsTest; + //All our hardware initialization in one place, for everything that is the same in TeleOp and + //Autonomous public void initHardware() { - limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim"); - + //drive motors driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft"); driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight"); driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft"); @@ -226,9 +234,12 @@ public class Robot { wobbleArmMotor.setTargetPosition(0); wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - wobbleUpPos = stateConfiguration.variable("system","arm", "up").value(); - wobbleDownPos = stateConfiguration.variable("system","arm", "down").value(); - wobbleDropPos = stateConfiguration.variable("system","arm", "drop").value(); + wobbleUpPos = stateConfiguration.variable( + "system","arm", "up").value(); + wobbleDownPos = stateConfiguration.variable( + "system","arm", "down").value(); + wobbleDropPos = stateConfiguration.variable( + "system","arm", "drop").value(); wobbleGrabServo = hardwareMap.servo.get("wobbleGrab"); @@ -236,7 +247,7 @@ public class Robot { wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch"); - //init ring belt + //init collection motor collectionMotor = hardwareMap.dcMotor.get("collect"); collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE); @@ -244,10 +255,17 @@ public class Robot { ringBeltMotor = hardwareMap.dcMotor.get("belt"); ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER); - beltMaxStopTime = stateConfiguration.variable("system","belt", "maxStopTime").value(); - beltMaxStopTicks = stateConfiguration.variable("system","belt", "maxStopTicks").value(); - beltReverseTicks = stateConfiguration.variable("system","belt", "reverseTicks").value(); - ringBeltGap = stateConfiguration.variable("system","belt","gap").value(); + + limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim"); + + beltMaxStopTime = stateConfiguration.variable( + "system","belt", "maxStopTime").value(); + beltMaxStopTicks = stateConfiguration.variable( + "system","belt", "maxStopTicks").value(); + beltReverseTicks = stateConfiguration.variable( + "system","belt", "reverseTicks").value(); + ringBeltGap = stateConfiguration.variable( + "system","belt","gap").value(); //init IMU imu = hardwareMap.get(BNO055IMU.class, "imu"); @@ -279,9 +297,12 @@ public class Robot { webCamServo = hardwareMap.servo.get("look"); webCamServo.setDirection(Servo.Direction.REVERSE ); - minCheckVelocity =stateConfiguration.variable("system", "camera", "minCheckV").value(); - vuforiaRotationCull = stateConfiguration.variable("system", "camera", "rCull").value(); - minCheckDurationMs =stateConfiguration.variable("system", "camera", "minCheckMS").value(); + minCheckVelocity =stateConfiguration.variable( + "system", "camera", "minCheckV").value(); + vuforiaRotationCull = stateConfiguration.variable( + "system", "camera", "rCull").value(); + minCheckDurationMs =stateConfiguration.variable( + "system", "camera", "minCheckMS").value(); //Init Launch Motor DcMotor launcher = hardwareMap.dcMotor.get("launcher"); @@ -296,8 +317,12 @@ public class Robot { launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - initLauncher = stateConfiguration.action("system","initLauncher").enabled; - reduceLaunchPos = stateConfiguration.variable("system", "launchPos", "reducePower").value(); + //This is a debugging option that automatically turns on the launch wheel during init. + //This can be disabled using a variable in the TimeCraftersConfiguration + initLauncher = stateConfiguration.action( + "system","initLauncher").enabled; + reduceLaunchPos = stateConfiguration.variable( + "system", "launchPos", "reducePower").value(); if (initLauncher) { double launcherPower = 0; @@ -307,10 +332,13 @@ public class Robot { launchMotor.setPower(launcherPower); } } - // - launchPositionX = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","x").value()); - launchPositionY = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","y").value()); - launchRotation = stateConfiguration.variable("system", "launchPos","rot").value(); + + launchPositionX = inchesToTicks((double) stateConfiguration.variable( + "system", "launchPos","x").value()); + launchPositionY = inchesToTicks((double) stateConfiguration.variable( + "system", "launchPos","y").value()); + launchRotation = stateConfiguration.variable( + "system", "launchPos","rot").value(); initTensorFlow(); @@ -319,8 +347,10 @@ public class Robot { private void initVuforia() { - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = + new VuforiaLocalizer.Parameters(cameraMonitorViewId); parameters.vuforiaLicenseKey = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah"; parameters.cameraName = webcam; @@ -346,20 +376,27 @@ public class Robot { redAllianceTarget.setLocation(OpenGLMatrix .translation(0, -halfField, mmTargetHeight) - .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180))); + .multiplied(Orientation.getRotationMatrix( + EXTRINSIC, XYZ, DEGREES, 90, 0, 180))); frontWallTarget.setLocation(OpenGLMatrix .translation(-halfField, 0, mmTargetHeight) - .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90))); + .multiplied(Orientation.getRotationMatrix( + EXTRINSIC, XYZ, DEGREES, 90, 0 , 90))); redTowerGoalTarget.setLocation(OpenGLMatrix .translation(halfField, -quadField, mmTargetHeight) - .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90))); + .multiplied(Orientation.getRotationMatrix( + EXTRINSIC, XYZ, DEGREES, 90, 0, -90))); OpenGLMatrix robotFromCamera = OpenGLMatrix - .translation(CAMERA_FORWARD_DISPLACEMENT * mmPerInch, CAMERA_LEFT_DISPLACEMENT * mmPerInch, CAMERA_VERTICAL_DISPLACEMENT * mmPerInch) - .multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, -90, 0, 0)); + .translation(CAMERA_FORWARD_DISPLACEMENT * mmPerInch, + CAMERA_LEFT_DISPLACEMENT * mmPerInch, + CAMERA_VERTICAL_DISPLACEMENT * mmPerInch) + .multiplied(Orientation.getRotationMatrix( + EXTRINSIC, YZX, DEGREES, -90, 0, 0)); for (VuforiaTrackable trackable : trackables) { - ((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(robotFromCamera, parameters.cameraDirection); + ((VuforiaTrackableDefaultListener) trackable.getListener()) + .setPhoneInformation(robotFromCamera, parameters.cameraDirection); } targetsUltimateGoal.activate(); @@ -373,11 +410,15 @@ public class Robot { int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName()); TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId); - parameters.minResultConfidence = stateConfiguration.variable("system", "camera", "minConfidence").value(); + parameters.minResultConfidence = stateConfiguration.variable( + "system", "camera", "minConfidence").value(); tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia); - tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single"); + tfObjectDetector.loadModelFromAsset( + "UltimateGoal.tflite", "Quad", "Single"); } + //Localization Function! This function is represented in a flow diagram, earlier in the + //software section //run this in every exec to track the robot's location. public void updateLocation(){ @@ -417,13 +458,15 @@ public class Robot { ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD; } - forwardVector = ((encoderLeftChange+encoderRightChange)/2) - (rotationChange* ticksPerDegreeForward); + forwardVector = ((encoderLeftChange+encoderRightChange)/2) - + (rotationChange* ticksPerDegreeForward); traveledForward += forwardVector; sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways); double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector)); - double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector); + double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) + + Math.atan2(sidewaysVector,forwardVector); double xChange = magnitude * (Math.sin(direction)); double yChange = magnitude * (Math.cos(direction)); @@ -434,11 +477,15 @@ public class Robot { Robot.rotation += rotationChange; Robot.rotation = scaleAngleRange(Robot.rotation); - totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange); + totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + + Math.abs(encoderBackChange); + +// record(""+System.currentTimeMillis()+", "+imuAngle); } - - + //Experimentation has demonstrated that Vuforia dosen't provide good data while the camera + //is moving or rotating. This function detects if conditions are appropriate to run vuforia and + //get more accurate results public void syncIfStationary() { if (totalV < minCheckVelocity) { long timeCurrent = System.currentTimeMillis(); @@ -459,7 +506,9 @@ public class Robot { trackableVisible = false; for (VuforiaTrackable trackable : trackables) { if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) { - OpenGLMatrix robotLocation = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + OpenGLMatrix robotLocation = + ((VuforiaTrackableDefaultListener)trackable.getListener()) + .getUpdatedRobotLocation(); //this is used for debugging purposes. trackableVisible = true; @@ -470,7 +519,8 @@ public class Robot { //For our tournament, it makes sense to make zero degrees towards the goal. //Orientation is inverted to have clockwise be positive. - Orientation orientation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES); + Orientation orientation = + Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES); float vuforiaRotation = 90-orientation.thirdAngle; if (vuforiaRotation > 180) { @@ -484,8 +534,10 @@ public class Robot { double camX = -translation.get(1) / mmPerInch; double camY = translation.get(0) / mmPerInch; - double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION); - double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION); + double displaceX = CAMERA_DISPLACEMENT_MAG * + Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION); + double displaceY = CAMERA_DISPLACEMENT_MAG * + Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION); locationX = inchesToTicks(camX - displaceX); locationY = inchesToTicks(camY - displaceY); @@ -508,18 +560,13 @@ public class Robot { return Robot.locationY; } + //This is meant to only be used to indicate starting positions and to reorient the robot. public void setLocalization(float rotation, double x, double y) { Robot.rotation = rotation; Robot.locationX = x; Robot.locationY = y; } - //Manually set the position of the robot on the field. - public void setCurrentPosition(float rotation, double x, double y) { - Robot.rotation = rotation; - Robot.locationX = x; - Robot.locationY = y; - } //returns the angle from the robot's current position to the given target position. public float getAngleToPosition (double x, double y) { @@ -531,7 +578,7 @@ public class Robot { } - //Unit conversion + //Unit conversions public double ticksToInches(double ticks) { return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION); } @@ -541,26 +588,28 @@ public class Robot { } - //Returns the angle between two angles, with positive angles indicating that the reference is - //to the right (clockwise) of the current. Negative angles indicate that the reference is to the - //left. + //Returns the shortest angle between two directions, with positive angles indicating that the + // reference is to the right (clockwise) of the current. Negative angles indicate that the + // reference is to the left. public float getRelativeAngle(float reference, float current) { return scaleAngleRange(current - reference); } //Drive Functions - public void setDrivePower(double powerFrontLeft, double powerFrontRight, double powerBackLeft, double powerBackRight){ + public void setDrivePower(double powerFrontLeft, double powerFrontRight, + double powerBackLeft, double powerBackRight){ 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" - //is the angle relative to the field that the robot should drive at. "degreesDirectionFace" is - //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) { + //returns an array of the powers necessary to execute the provided motion. + //"degreesDirectionMotion" is the angle relative to the field that the robot should drive at. + //"degreesDirectionFace" is the angle the robot should face relative to the field. The order of + //the output powers 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 @@ -582,12 +631,14 @@ public class Robot { LINEAR_CORRECTION * relativeRotation; if (relativeRotation != 0) { - double momentumRelative = angularVelocity * (relativeRotation / Math.abs(relativeRotation)); - double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); + double momentumRelative = angularVelocity * + (relativeRotation / Math.abs(relativeRotation)); + double exponential = + Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential); //reduces concern for momentum when the angle is far away from target - turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) * (1 - momentumCorrection)) / 180 ); -// turnCorrection *= momentumCorrection; + turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) * + (1 - momentumCorrection)) / 180 ); } double powerForwardRight = scalar * (q + turnCorrection); @@ -660,6 +711,7 @@ public class Robot { return powers; } + //Used to for automated jam countermeasures public boolean beltIsStuck() { int ringBeltPos = ringBeltMotor.getCurrentPosition(); boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks); @@ -677,6 +729,7 @@ public class Robot { return angle; } + //Debugging tools public void record(String record) { TestingRecord+="\n"+record; } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java index 689f396..180f648 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java @@ -17,7 +17,6 @@ public class LaunchControl extends CyberarmState { @Override public void start() { - robot.launchMotor.setPower(Robot.LAUNCH_POWER); if (robot.ringBeltStage > 2) { if (robot.ringBeltStage > 4) { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java index 543621b..d25a9d9 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java @@ -1,5 +1,11 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; +/* +The Player1 state has all the controls for player one. The Player One and Player Two controls are +kept in separate states so that the childrenHaveFinished() method can be used to easily stop things +from running at the same time that shouldn't be. +*/ + import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import org.cyberarm.engine.V2.CyberarmState; @@ -40,6 +46,7 @@ public class Player1 extends CyberarmState { private double endGameX; private double endGameY; private float endGameRot; + private int loopCount; private double refinePower; @@ -49,14 +56,21 @@ public class Player1 extends CyberarmState { @Override public void init() { - cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value(); - pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value(); - faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value(); - refinePower = robot.stateConfiguration.variable("tele","control", "refPower").value(); + cardinalSnapping = robot.stateConfiguration.variable( + "tele","control", "cardinalSnapping").value(); + pairSnapping = robot.stateConfiguration.variable( + "tele","control", "pairSnapping").value(); + faceControlThreshold = robot.stateConfiguration.variable( + "tele","control", "faceControlT").value(); + refinePower = robot.stateConfiguration.variable( + "tele","control", "refPower").value(); - endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value(); - endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value(); - endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value(); + endGameX = robot.stateConfiguration.variable( + "tele","_endGameStart","x").value(); + endGameY = robot.stateConfiguration.variable( + "tele","_endGameStart","y").value(); + endGameRot = robot.stateConfiguration.variable( + "tele","_endGameStart", "r").value(); } @Override @@ -66,8 +80,11 @@ public class Player1 extends CyberarmState { @Override public void exec() { + loopCount+=1; + robot.record(""+runTime()+", "+loopCount+", "+robot.getRotation()); robot.updateLocation(); + //toggle for drive speed boolean lb = engine.gamepad1.left_stick_button; if (lb && !lbPrev) { if (drivePower == 1) { @@ -78,7 +95,9 @@ public class Player1 extends CyberarmState { } lbPrev = lb; - + //This Runs the FindWobbleGoal state as long as the button is pressed. When it's released, + //the state is interrupted. If the state finishes while the button is still pressed, + //it waits until the button is released before running the state again runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished()); boolean findWobbleInput = engine.gamepad1.x; @@ -89,7 +108,8 @@ public class Player1 extends CyberarmState { faceDirection = robot.getRotation(); if (runNextFindWobble && !findWobbleInputPrev) { - findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0"); + findWobbleGoal = + new FindWobbleGoal(robot, "auto", "08_0"); addParallelState(findWobbleGoal); } //if the claw is closed, open the claw. @@ -113,6 +133,8 @@ public class Player1 extends CyberarmState { } aPrev = a; + //This logic works the same as the stuff above, accept instead of autonomous wobble grab, + //it runNextDriveToLaunch = (powerShots == null || powerShots.getHasFinished()); boolean driveToLaunchInput = engine.gamepad1.y && !findWobbleInput; @@ -127,29 +149,27 @@ public class Player1 extends CyberarmState { } driveToLaunchInputPrev = driveToLaunchInput; -// if (engine.gamepad1.y) { -// robot.setLocalization(endGameRot,endGameX,endGameY); -// setHasFinished(true); -// } - + //Normal Driver Controls if (childrenHaveFinished()) { - //Normal Driver Controls + double leftJoystickX = engine.gamepad1.left_stick_x; double leftJoystickY = engine.gamepad1.left_stick_y; - leftJoystickDegrees = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY))); + leftJoystickDegrees = robot.getRelativeAngle(90, + (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 = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY))); + rightJoystickDegrees = robot.getRelativeAngle(90, + (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY))); rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); //allows the the driver to indicate which direction the robot is currently looking //so that the controller and robot can be re-synced in the event of a bad initial - //position. + //rotation. if (engine.gamepad1.back) { float newRotation = 0; @@ -161,14 +181,19 @@ public class Player1 extends CyberarmState { faceDirection = newRotation; } - //if the driver is letting go of the face joystick, the robot should maintain it's current face direction. - if (rightJoystickMagnitude > faceControlThreshold || rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) { + //if the driver is letting go of the face joystick, the robot should maintain it's + //current face direction. + if (rightJoystickMagnitude > faceControlThreshold || + rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) { - //if the joystick is close to one of the cardinal directions, it is set to exactly the cardinal direction + //if the joystick is close to one of the cardinal directions, it is set to exactly + // the cardinal direction faceDirection = snapToCardinal(rightJoystickDegrees, cardinalSnapping, 0); } rightJoystickMagnitudePrevious = rightJoystickMagnitude; + //The D-pad is used if the drivers need to get a more precise angle than they can get + //using the face joystick if (engine.gamepad1.dpad_right) { powers = new double[]{refinePower, -refinePower, refinePower, -refinePower}; faceDirection = robot.getRotation(); @@ -182,7 +207,9 @@ public class Player1 extends CyberarmState { //drives the robot in the direction of the move joystick while facing the direction //of the look joystick. if the move direction is almost aligned/perpendicular to the //look joystick, - powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection), drivePower, faceDirection); + powers = robot.getMecanumPowers( + snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection), + drivePower, faceDirection); } robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]); @@ -190,7 +217,8 @@ public class Player1 extends CyberarmState { //LED feedback control double ringBeltPower = robot.ringBeltMotor.getPower(); - if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - robot.ringBeltMotor.getCurrentPosition()) > 10) { + if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - + robot.ringBeltMotor.getCurrentPosition()) > 10) { robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE ); } else if (ringBeltPower < 0) { robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED); diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java index 624665e..70123f7 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java @@ -1,9 +1,15 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; +/* +The Player2 state has all the controls for player two. This includes a lot of automation and +emergency features, for if the driver wants to take control unexpectedly +*/ + import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.timecrafters.UltimateGoal.Competition.Launch; import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; import org.timecrafters.UltimateGoal.Competition.Robot; @@ -22,6 +28,7 @@ public class Player2 extends CyberarmState { private DcMotor.RunMode runModePrev; private boolean lbPrev; private boolean manualArmHold; + private int loops = 0; @@ -45,9 +52,10 @@ public class Player2 extends CyberarmState { @Override public void exec() { +// loops+=1; +// robot.record(""+runTime()+", "+loops+", "+robot.getRotation()); //Collector control - double rt = engine.gamepad2.right_trigger; double lt = engine.gamepad2.left_trigger; if (rt < lt) { @@ -58,6 +66,8 @@ public class Player2 extends CyberarmState { robot.collectionMotor.setPower(0); } + //The childrenHaveFinished() method tracks if there are parallel states that are still + //running. if (childrenHaveFinished()) { //belt progression control boolean rb = engine.gamepad2.right_bumper; @@ -140,33 +150,40 @@ public class Player2 extends CyberarmState { } lbPrev = lb; -// if (engine.gamepad1.y) { -// setHasFinished(true); -// } + if (engine.gamepad2.back) { + for (CyberarmState state : children) { + engine.stopState(state); + } + } + } + + //This just checks if the wobble arm runmode is already the desired mode before setting it. + //I don't know if this is actually helpful private void setArmMode(DcMotor.RunMode runMode) { if (robot.wobbleArmMotor.getMode() != runMode) { robot.wobbleArmMotor.setMode(runMode); } } + @Override public void telemetry() { // engine.telemetry.addLine("belt"); // engine.telemetry.addData("power", robot.ringBeltMotor.getPower()); // engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition()); // engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition()); - +// // engine.telemetry.addData("ring belt stage", robot.ringBeltStage); // engine.telemetry.addData("Touch Sensor Pressed", robot.wobbleTouchSensor.isPressed()); // engine.telemetry.addData(" Sensor value", robot.wobbleTouchSensor.getValue()); -// engine.telemetry.addData("Player 2 children", childrenHaveFinished()); -// for (CyberarmState state : children) { -// if (!state.getHasFinished()) { -// engine.telemetry.addLine("" + state.getClass()); -// } -// } + engine.telemetry.addData("Player 2 children", childrenHaveFinished()); + for (CyberarmState state : children) { + if (!state.getHasFinished()) { + engine.telemetry.addLine("" + state.getClass()); + } + } } } 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 a3b3b60..f63409c 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,12 +1,13 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; import org.timecrafters.UltimateGoal.Competition.Autonomous.Face; import org.timecrafters.UltimateGoal.Competition.Robot; - +@Disabled @TeleOp (name = "TeleOp",group = "comp") public class TeleOpEngine extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java index c2e897f..c53dc7b 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java @@ -3,6 +3,7 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; import org.cyberarm.engine.V2.CyberarmState; import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; import org.timecrafters.UltimateGoal.Competition.Autonomous.Face; +import org.timecrafters.UltimateGoal.Competition.Pause; import org.timecrafters.UltimateGoal.Competition.Robot; import java.util.ArrayList; @@ -15,6 +16,8 @@ public class powerShotsControl extends CyberarmState { private double endGameY; private float endGameRot; + private double endGamePower; + private int nextState = 0; private ArrayList states = new ArrayList(); @@ -28,11 +31,13 @@ public class powerShotsControl extends CyberarmState { endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value(); endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value(); endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value(); + endGamePower = robot.stateConfiguration.variable("tele","_endGameStart", "power").value(); endGameX = robot.inchesToTicks(endGameX); endGameY = robot.inchesToTicks(endGameY); endGameRot = (float) robot.inchesToTicks(endGameRot); + states.add(new Pause(robot, "tele","_endGameStart")); states.add(new DriveToCoordinates(robot, "tele", "_pow1")); states.add(new Face(robot,"tele","_faceZero")); states.add(new LaunchControl(robot)); @@ -47,7 +52,7 @@ public class powerShotsControl extends CyberarmState { @Override public void start() { robot.setLocalization(endGameRot,endGameX,endGameY); - robot.launchMotor.setPower(Robot.LAUNCH_POWER); + robot.launchMotor.setPower(endGamePower); } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/javaClass/aubrey/AubreyFirstState.java b/TeamCode/src/main/java/org/timecrafters/javaClass/aubrey/AubreyFirstState.java new file mode 100644 index 0000000..24b2106 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/javaClass/aubrey/AubreyFirstState.java @@ -0,0 +1,22 @@ +package org.timecrafters.javaClass.aubrey; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.javaClass.samples.SampleRobot; + +public class AubreyFirstState extends CyberarmState { + + //here, you'll find some of your variables. you can add more as you need them. + private SampleRobot robot; + + //This is the constructor. It lets other code bits run use the code you put here + public AubreyFirstState(SampleRobot robot) { + this.robot = robot; + } + + //This is a method. methods are bits of code that can be run elsewhere. + //This one is set up to repeat every few milliseconds + @Override + public void exec() { + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/javaClass/samples/AubreyFirstEngine.java b/TeamCode/src/main/java/org/timecrafters/javaClass/samples/AubreyFirstEngine.java new file mode 100644 index 0000000..769f194 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/javaClass/samples/AubreyFirstEngine.java @@ -0,0 +1,26 @@ +package org.timecrafters.javaClass.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.javaClass.aubrey.AubreyFirstState; + +@Autonomous(name = "Aubrey: First Program", group = "aubrey") +public class AubreyFirstEngine extends CyberarmEngine { + + SampleRobot robot; + + @Override + public void init() { + robot = new SampleRobot(hardwareMap); + robot.initHardware(); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + super.init(); + } + + @Override + public void setup() { + addState(new AubreyFirstState(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobot.java b/TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobot.java new file mode 100644 index 0000000..543cbec --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobot.java @@ -0,0 +1,191 @@ + package org.timecrafters.javaClass.samples; + +/* +The robot object contains all the hardware and functions that are used in both teleOp and +Autonomous. This includes drive functions, localization functions, shared constants, and a few +general calculations and debugging tools. +*/ + +import android.os.Environment; +import android.util.Log; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.hardware.rev.RevColorSensorV3; +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +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; +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; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; +import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration; + +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + +import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES; +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 SampleRobot { + + private HardwareMap hardwareMap; + + public SampleRobot(HardwareMap hardwareMap) { + this.hardwareMap = hardwareMap; + } + + //The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit + //variables saved on the phone, without having to re-download the whole program. This is + //especially useful for autonomous route tuning + public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration(); + + //We use the IMU to get reliable rotation and angular velocity information. Experimentation has + //demonstrated that the accelerometer and related integrations aren't as accurate. + public BNO055IMU imu; + + //The LEDs are used to provide driver feedback and for looking beautiful + public RevBlinkinLedDriver ledDriver; + + //drive and dead-wheal hardware + public DcMotor driveFrontLeft; + public DcMotor driveBackLeft; + public DcMotor driveFrontRight; + public DcMotor driveBackRight; + + public DcMotor encoderLeft; + public DcMotor encoderRight; + public DcMotor encoderBack; + + //Unit Conversion Constants + static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622; + static final int COUNTS_PER_REVOLUTION = 8192; + //Launcher + public DcMotor launchMotor; + public static final double LAUNCH_POWER = 0.715; + + //Ring Intake + public DcMotor collectionMotor; + + //Ring Belt + public DcMotor ringBeltMotor; + public RevTouchSensor limitSwitch; + + //Wobble Goal Arm & Grabber + public DcMotor wobbleArmMotor; + public Servo wobbleGrabServo; + public RevColorSensorV3 wobbleColorSensor; + public RevTouchSensor wobbleTouchSensor; + + //All our hardware initialization in one place, for everything that is the same in TeleOp and + //Autonomous + public void initHardware() { + + //drive motors + driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft"); + driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight"); + driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft"); + driveBackRight = hardwareMap.dcMotor.get("driveBackRight"); + + driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD); + driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD); + driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + //Localization encoders + encoderRight = hardwareMap.dcMotor.get("driveFrontRight"); + encoderBack = hardwareMap.dcMotor.get("driveFrontLeft"); + encoderLeft = hardwareMap.dcMotor.get("collect"); + + 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); + + //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); + + wobbleGrabServo = hardwareMap.servo.get("wobbleGrab"); + + wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color"); + wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch"); + + + //init collection motor + collectionMotor = hardwareMap.dcMotor.get("collect"); + 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); + + limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim"); + + //init IMU + imu = hardwareMap.get(BNO055IMU.class, "imu"); + + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + + parameters.mode = BNO055IMU.SensorMode.IMU; + parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; + parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; + parameters.loggingEnabled = false; + + imu.initialize(parameters); + + //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); + launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds"); + } + + public float getIMURotation() { + return imu.getAngularOrientation().firstAngle; + } + + //Unit conversions + public double ticksToInches(double ticks) { + return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION); + } + + public double inchesToTicks(double inches) { + return inches * (COUNTS_PER_REVOLUTION / ENCODER_CIRCUMFERENCE); + + } + + + } diff --git a/TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobotEx.java b/TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobotEx.java new file mode 100644 index 0000000..91b3321 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobotEx.java @@ -0,0 +1,515 @@ + package org.timecrafters.javaClass.samples; + +/* +The robot object contains all the hardware and functions that are used in both teleOp and +Autonomous. This includes drive functions, localization functions, shared constants, and a few +general calculations and debugging tools. +*/ + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.hardware.rev.RevColorSensorV3; +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +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.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +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.VuforiaTrackables; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; +import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration; +import org.timecrafters.UltimateGoal.Competition.Robot; + +import java.util.ArrayList; +import java.util.List; + + public class SampleRobotEx { + + private HardwareMap hardwareMap; + + public SampleRobotEx(HardwareMap hardwareMap) { + this.hardwareMap = hardwareMap; + } + + //The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit + //variables saved on the phone, without having to re-download the whole program. This is + //especially useful for autonomous route tuning + public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration(); + + //We use the IMU to get reliable rotation and angular velocity information. Experimentation has + //demonstrated that the accelerometer and related integrations aren't as accurate. + public BNO055IMU imu; + + //The LEDs are used to provide driver feedback and for looking beautiful + public RevBlinkinLedDriver ledDriver; + + //drive and dead-wheal hardware + public DcMotor driveFrontLeft; + public DcMotor driveBackLeft; + public DcMotor driveFrontRight; + public DcMotor driveBackRight; + + public DcMotor encoderLeft; + public DcMotor encoderRight; + public DcMotor encoderBack; + + //Motion Constants + static final double CUBIC_CORRECTION = 0.035; + static final double FACE_CUBIC_CORRECTION = 0.025; + static final double LINEAR_CORRECTION = 0.055; + static final double FACE_MIN_CORRECTION = 0.2; + static final double FACE_LINEAR_CORRECTION = 0.025; + static final double MOMENTUM_CORRECTION = 1.05; + static final double MOMENTUM_MAX_CORRECTION = 1.4; + static final double MOMENTUM_HORIZONTAL_CORRECTION = + -(Math.log10(MOMENTUM_MAX_CORRECTION-1) / Math.log10(MOMENTUM_CORRECTION)); + static final double FACE_MOMENTUM_MAX_CORRECTION = 1.1; + static final double FACE_MOMENTUM_CORRECTION = 1.06; + static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION = + -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1) / Math.log10(FACE_MOMENTUM_CORRECTION)); + static final double ZERO_POWER_THRESHOLD = 0.25; + + //Unit Conversion Constants + static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622; + static final int COUNTS_PER_REVOLUTION = 8192; + static final float mmPerInch = 25.4f; + static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD = 12.3; + static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD = 18.8; + static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4; + static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6; + + //Robot Localization + private static double locationX; + private static double locationY; + private static float rotation; + + private int encoderLeftPrevious = 0; + private int encoderBackPrevious = 0; + private int encoderRightPrevious = 0; + private float rotationPrevious = 0; + public float angularVelocity; + + public double forwardVector; + public double sidewaysVector; + + + //vuforia && tensorFlow Stuff + private WebcamName webcam; + private VuforiaLocalizer vuforia; + + // Inches Forward of axis of rotation + static final float CAMERA_FORWARD_DISPLACEMENT = 8f; + // Inches above Ground + static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f; + // Inches Left of axis of rotation + static final float CAMERA_LEFT_DISPLACEMENT = 4f; + + static final double CAMERA_DISPLACEMENT_MAG = + Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT); + static final float CAMERA_DISPLACEMENT_DIRECTION = + (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT); + + public boolean trackableVisible; + private VuforiaTrackables targetsUltimateGoal; + private List trackables = new ArrayList(); + private OpenGLMatrix lastConfirmendLocation; + + private long timeStartZeroVelocity = 0; + private long minCheckDurationMs = 500; + private int minCheckVelocity = 1; + private float vuforiaRotationCull; + + //The servo mount for our camera allows us to look down for ideal TensorFlow and look up for + //ideal Vuforia Navigation + public Servo webCamServo; + public static final double CAM_SERVO_DOWN = 0.15; + + //TensorFlow Object Detection + public TFObjectDetector tfObjectDetector; + private static final float MINIMUM_CONFIDENCE = 0.8f; + + //Launcher + public DcMotor launchMotor; + public static final double LAUNCH_POWER = 0.715; + + private static final long LAUNCH_ACCEL_TIME = 500; + //These variables were originally going to be used in both autonomous and teleOp + public double launchPositionX; + public double launchPositionY; + public float launchRotation; + public int reduceLaunchPos; + + public boolean initLauncher; + + //Ring Intake + public DcMotor collectionMotor; + + //Ring Belt + public DcMotor ringBeltMotor; + public RevTouchSensor limitSwitch; + public int ringBeltStage; + public int ringBeltGap = 700; + public static final double RING_BELT_SLOW_POWER = 0.2; + public static final double RING_BELT_NORMAL_POWER = 0.6; + private int ringBeltPrev; + public long beltMaxStopTime; + public int beltReverseTicks; + public int beltMaxStopTicks; + + //Wobble Goal Arm & Grabber + public DcMotor wobbleArmMotor; + public Servo wobbleGrabServo; + public int wobbleDownPos; + public int wobbleUpPos; + public int wobbleDropPos; + public static final double WOBBLE_SERVO_OPEN = 0; + public static final double WOBBLE_SERVO_CLOSED = 1; + public RevColorSensorV3 wobbleColorSensor; + public double wobbleScoreX; + public double wobbleScoreY; + public RevTouchSensor wobbleTouchSensor; + + //Debugging + public double totalV; + private String TestingRecord = "Raw IMU, Delta, Saved"; + + public double traveledForward = 0; + public DcMotorEx motorAmpsTest; + + //All our hardware initialization in one place, for everything that is the same in TeleOp and + //Autonomous + public void initHardware() { + + //drive motors + driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft"); + driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight"); + driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft"); + driveBackRight = hardwareMap.dcMotor.get("driveBackRight"); + + driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD); + driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD); + driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + //Localization encoders + encoderRight = hardwareMap.dcMotor.get("driveFrontRight"); + encoderBack = hardwareMap.dcMotor.get("driveFrontLeft"); + encoderLeft = hardwareMap.dcMotor.get("collect"); + + 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); + + //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); + + wobbleGrabServo = hardwareMap.servo.get("wobbleGrab"); + + wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color"); + wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch"); + + + //init collection motor + collectionMotor = hardwareMap.dcMotor.get("collect"); + 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); + + limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim"); + + //init IMU + imu = hardwareMap.get(BNO055IMU.class, "imu"); + + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + + parameters.mode = BNO055IMU.SensorMode.IMU; + parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; + parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; + parameters.loggingEnabled = false; + + 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); + + //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); + launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds"); + } + + //Localization Function! This function is represented in a flow diagram, earlier in the + //software section + //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); + + int encoderLeftCurrent = -encoderLeft.getCurrentPosition(); + int encoderRightCurrent = encoderRight.getCurrentPosition(); + int encoderBackCurrent = encoderBack.getCurrentPosition(); + + double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious; + double encoderRightChange = encoderRightCurrent - encoderRightPrevious; + double encoderBackChange = encoderBackCurrent - encoderBackPrevious; + + encoderLeftPrevious = encoderLeftCurrent; + encoderRightPrevious = encoderRightCurrent; + encoderBackPrevious = encoderBackCurrent; + rotationPrevious = imuAngle; + + //The forward Vector has the luxury of having an odometer on both sides of the robot. + //This allows us to reduce the unwanted influence of turning the robot by averaging + //the two. unfortunatly we the current positioning of the odometers + + //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 ticksPerDegreeForward; + double ticksPerDegreeSideways; + + if (rotationChange < 0) { + ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE; + ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD; + } else { + ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_CLOCKWISE; + ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD; + } + + forwardVector = ((encoderLeftChange+encoderRightChange)/2) - + (rotationChange* ticksPerDegreeForward); + + sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways); + + double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector)); + double direction = Math.toRadians(SampleRobotEx.rotation + (rotationChange/2)) + + Math.atan2(sidewaysVector,forwardVector); + + double xChange = magnitude * (Math.sin(direction)); + double yChange = magnitude * (Math.cos(direction)); + + locationX += xChange; + locationY += yChange; + + SampleRobotEx.rotation += rotationChange; + SampleRobotEx.rotation = scaleAngleRange(SampleRobotEx.rotation); + + } + + public float getRotation() { + return SampleRobotEx.rotation; + } + + public double getLocationX() { + return SampleRobotEx.locationX; + } + + public double getLocationY() { + return SampleRobotEx.locationY; + } + + //This is meant to only be used to indicate starting positions and to reorient the robot. + public void setLocalization(float rotation, double x, double y) { + SampleRobotEx.rotation = rotation; + SampleRobotEx.locationX = x; + SampleRobotEx.locationY = y; + } + + //returns the angle from the robot's current position to the given target position. + public float getAngleToPosition (double x, double y) { + double differenceX = x- getLocationX(); + double differenceY = y- getLocationY(); + double angle = Math.toDegrees(Math.atan2(differenceX, differenceY )); + + return (float) angle; + } + + //Unit conversions + public double ticksToInches(double ticks) { + return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION); + } + + public double inchesToTicks(double inches) { + return inches * (COUNTS_PER_REVOLUTION / ENCODER_CIRCUMFERENCE); + + } + + //Returns the shortest angle between two directions, with positive angles indicating that the + // reference is to the right (clockwise) of the current. Negative angles indicate that the + // reference is to the left. + public float getRelativeAngle(float reference, float current) { + return scaleAngleRange(current - reference); + } + + //Drive Functions + public void setDrivePower(double powerFrontLeft, double powerFrontRight, + double powerBackLeft, double powerBackRight){ + driveFrontLeft.setPower(powerFrontLeft); + driveFrontRight.setPower(powerFrontRight); + driveBackLeft.setPower(powerBackLeft); + driveBackRight.setPower(powerBackRight); + } + + public void setDrivePower(double[] powers){ + driveFrontLeft.setPower(powers[0]); + driveFrontRight.setPower(powers[1]); + driveBackLeft.setPower(powers[2]); + driveBackRight.setPower(powers[3]); + } + + //returns an array of the powers necessary to execute the provided motion. + //"degreesDirectionMotion" is the angle relative to the field that the robot should drive at. + //"degreesDirectionFace" is the angle the robot should face relative to the field. The order of + //the output powers 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 = 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, SampleRobotEx.rotation); + double turnCorrection = + Math.pow(CUBIC_CORRECTION * relativeRotation, 3) + + LINEAR_CORRECTION * relativeRotation; + + if (relativeRotation != 0) { + double momentumRelative = angularVelocity * + (relativeRotation / Math.abs(relativeRotation)); + double exponential = + Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); + double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential); + //reduces concern for momentum when the angle is far away from target + turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) * + (1 - momentumCorrection)) / 180 ); + } + + double powerForwardRight = scalar * (q + turnCorrection); + double powerForwardLeft = scalar * (p - turnCorrection); + double powerBackRight = scalar * (p + turnCorrection); + double powerBackLeft = scalar * (q - turnCorrection); + + + // The "extreme" is the power value that is furthest from zero. When this values exceed the + // -1 to 1 power range, dividing the powers by the "extreme" scales everything back into the + // workable range without altering the final motion vector. + + double extreme = Math.max( + Math.max(Math.abs(powerForwardRight),Math.abs(powerForwardLeft)), + Math.max(Math.abs(powerBackRight),Math.abs(powerBackLeft))); + + if (extreme > 1) { + powerForwardRight = powerForwardRight/extreme; + powerForwardLeft = powerForwardLeft/extreme; + powerBackRight = powerBackRight/extreme; + powerBackLeft = powerBackLeft/extreme; + } + + double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight}; + + double totalPower = Math.abs(powerForwardLeft) + + Math.abs(powerForwardRight) + + Math.abs(powerBackLeft) + + Math.abs(powerBackRight); + if (totalPower < ZERO_POWER_THRESHOLD) { + powers = new double[] {0,0,0,0}; + } + + + return powers; + } + //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, SampleRobotEx.rotation); + double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) + + FACE_LINEAR_CORRECTION * relativeAngle; + + if (relativeAngle > 0.5) { + scaler += FACE_MIN_CORRECTION; + } else if (relativeAngle < -0.5) { + scaler -= FACE_MIN_CORRECTION; + } + + if (relativeAngle != 0) { + double momentumRelative = angularVelocity * (relativeAngle / Math.abs(relativeAngle)); + double exponential = Math.pow(FACE_MOMENTUM_CORRECTION, + FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); + double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential); + + scaler *= momentumCorrection; + } + + double left = -power * scaler; + double right = power *scaler; + + double[] powers = {left,right}; + + double totalPower = 2 * (Math.abs(left) + Math.abs(right)); + if (totalPower < ZERO_POWER_THRESHOLD) { + powers = new double[] {0,0}; + } + + return powers; + } + + public float scaleAngleRange(float angle) { + if (angle < -180) { + angle += 360; + } + if (angle > 180) { + angle -= 360; + } + return angle; + } + + } diff --git a/TeamCode/src/main/java/org/timecrafters/javaClass/samples/SpencerFirstEngine.java b/TeamCode/src/main/java/org/timecrafters/javaClass/samples/SpencerFirstEngine.java new file mode 100644 index 0000000..97f89bb --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/javaClass/samples/SpencerFirstEngine.java @@ -0,0 +1,27 @@ +package org.timecrafters.javaClass.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.javaClass.aubrey.AubreyFirstState; +import org.timecrafters.javaClass.spencer.SpencerFirstState; + +@Autonomous (name = "Spencer: First Program", group = "spencer") +public class SpencerFirstEngine extends CyberarmEngine { + + SampleRobot robot; + + @Override + public void init() { + robot = new SampleRobot(hardwareMap); + robot.initHardware(); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + super.init(); + } + + @Override + public void setup() { + addState(new SpencerFirstState(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/javaClass/spencer/SpencerFirstState.java b/TeamCode/src/main/java/org/timecrafters/javaClass/spencer/SpencerFirstState.java new file mode 100644 index 0000000..b50d769 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/javaClass/spencer/SpencerFirstState.java @@ -0,0 +1,22 @@ +package org.timecrafters.javaClass.spencer; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.javaClass.samples.SampleRobot; + +public class SpencerFirstState extends CyberarmState { + + //here, you'll find some of your variables. you can add more as you need them. + private SampleRobot robot; + + //This is the constructor. It lets other code bits run use the code you put here + public SpencerFirstState(SampleRobot robot) { + this.robot = robot; + } + + //This is a method. methods are bits of code that can be run elsewhere. + //This one is set up to repeat every few milliseconds + @Override + public void exec() { + + } +} diff --git a/build.gradle b/build.gradle index 87f0507..8a2a157 100644 --- a/build.gradle +++ b/build.gradle @@ -15,7 +15,7 @@ buildscript { jcenter() } dependencies { - classpath 'com.android.tools.build:gradle:4.0.1' + classpath 'com.android.tools.build:gradle:4.1.2' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 530fb22..e399ab6 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ -#Fri Jul 24 14:30:03 PDT 2020 +#Sat Jul 24 09:32:10 CDT 2021 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-6.1.1-all.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-6.5-bin.zip