From 902a7cefff386d7cc68b5f26999b0b1e9312af4e Mon Sep 17 00:00:00 2001 From: Nathaniel Palme Date: Fri, 19 Feb 2021 21:03:49 -0600 Subject: [PATCH] Emergency features & autonomous touches --- .../Competition/Autonomous/AutoEngine.java | 42 +++-- .../Autonomous/DriveToCoordinates.java | 17 +- .../Competition/Autonomous/Face.java | 6 + .../Autonomous/FindWobbleGoal.java | 76 +++++++++ .../Autonomous/LaunchDriveControl.java | 41 +++++ .../UltimateGoal/Competition/Launch.java | 24 ++- .../Competition/PreInit/LoadRings.java | 5 + .../UltimateGoal/Competition/Robot.java | 42 ++--- .../Competition/TeleOp/TeleOpState.java | 159 ++++++++++++------ .../UltimateGoal/Competition/WobbleArm.java | 2 +- .../UltimateGoal/Competition/WobbleGrab.java | 4 +- .../HardwareTesting/TestingEngine.java | 17 +- 12 files changed, 319 insertions(+), 116 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/LaunchDriveControl.java 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 1368fde..0bb6a12 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 @@ -26,16 +26,20 @@ public class AutoEngine extends CyberarmEngine { private double scoreAPower; private long scoreABrakeTime; - float scoreBFaceAngle; - double scoreBTolerance; - double scoreBPower; - long scoreBBrakeTime; + double parkY; + float parkFaceAngle; + double parkTolerance; + double parkPower; + long parkBrakeTime; @Override public void init() { robot = new Robot(hardwareMap); robot.initHardware(); robot.webCamServo.setPosition(Robot.CAM_SERVO_DOWN); + robot.wobbleGrabServo.setPosition(0.1 * Robot.WOBBLE_SERVO_MAX); + // since we've preloaded three rings, the ring belt stage is set to account for this; + robot.ringBeltStage = 3; launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value()); launchPower = robot.stateConfiguration.variable("auto","04_0","power").value(); @@ -46,10 +50,11 @@ public class AutoEngine extends CyberarmEngine { // scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value(); // scoreABrakeTime = robot.stateConfiguration.variable("auto","05_0","brakeMS").value(); // -// scoreBFaceAngle = robot.stateConfiguration.variable("auto","10_0","face").value(); -// scoreBTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","10_0","tolPos").value()); -// scoreBPower = robot.stateConfiguration.variable("auto","10_0","power").value(); -// scoreBBrakeTime = robot.stateConfiguration.variable("auto","10_0","brakeMS").value(); + parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","13_0","yPos").value()); + parkFaceAngle = robot.stateConfiguration.variable("auto","13_0","face").value(); + parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","13_0","tolPos").value()); + parkPower = robot.stateConfiguration.variable("auto","13_0","power").value(); + parkBrakeTime = robot.stateConfiguration.variable("auto","13_0","brakeMS").value(); super.init(); } @@ -72,15 +77,15 @@ public class AutoEngine extends CyberarmEngine { //drive to launch position addState(new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime)); - // + //aligns to goal addState(new Face(robot, "auto", "04_1")); //launch rings - addState(new Launch(robot, "auto", "04_2")); + CyberarmState launchState = new Launch(robot, "auto", "04_2"); //drive to scoring area - addState(new DriveToCoordinates(robot, "auto", "05_0", true)); -// addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreAFaceAngle,scoreATolerance,scoreAPower,scoreABrakeTime)); + CyberarmState driveState = new DriveToCoordinates(robot, "auto", "05_0", true); + addState(new LaunchDriveControl(robot,launchState,driveState)); //turn arm towards scoreing area. ArrayList threadStates = new ArrayList<>(); @@ -95,7 +100,7 @@ public class AutoEngine extends CyberarmEngine { addState(new DriveToCoordinates(robot, "auto","06_1")); addState(new DriveToCoordinates(robot, "auto", "07_0")); - addState(new DriveWithColorSensor(robot, "auto", "08_0")); + addState(new FindWobbleGoal(robot, "auto", "08_0")); //close grabber addState(new WobbleGrab(robot, "auto", "09_0", false)); @@ -109,10 +114,11 @@ public class AutoEngine extends CyberarmEngine { addState(new WobbleGrab(robot, "auto", "12_0", true)); //drive to park - ArrayList threadStatesB = new ArrayList<>(); - threadStatesB.add(new WobbleGrab(robot, "auto", "12_1", false)); - threadStatesB.add(new WobbleArm(robot, "auto", "12_2",true)); - threadStatesB.add(new DriveToCoordinates(robot, "auto", "13_0")); - addState(new ThreadStates(threadStatesB)); + addState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime)); +// ArrayList threadStatesB = new ArrayList<>(); +// threadStatesB.add(new WobbleGrab(robot, "auto", "12_1", false)); +// threadStatesB.add(new WobbleArm(robot, "auto", "12_2",true)); +// threadStatesB.add(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime)); +// addState(new ThreadStates(threadStatesB)); } } 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 e7207bf..29b5b1f 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 @@ -45,6 +45,8 @@ public class DriveToCoordinates extends CyberarmState { @Override public void init() { if (!groupName.equals("manual")) { + 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(); tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value()); brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value(); @@ -62,12 +64,15 @@ public class DriveToCoordinates extends CyberarmState { if (!groupName.equals("manual")) { setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled); - if (!scoringArea) { - xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value()); - yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value()); - } else { - xTarget = robot.wobbleScoreX; - yTarget = robot.wobbleScoreY; + //used to navigate towards the randomly generated scoreing area. the original target + //becomes an offset of the scoring area position. + if (scoringArea) { + xTarget += robot.wobbleScoreX; + yTarget += robot.wobbleScoreY; + } + + if (faceAngle == 360) { + faceAngle = robot.getRelativeAngle(180, robot.getAngleToPosition(xTarget,yTarget)); } } 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 index 7c165e5..c735ab8 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java @@ -58,6 +58,12 @@ public class Face extends CyberarmState { double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value()); faceAngle = robot.getAngleToPosition(x, y); } + + if (faceAngle == 360) { + double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value()); + double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value()); + faceAngle = robot.getRelativeAngle(180,robot.getAngleToPosition(x, y)); + } } } 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 new file mode 100644 index 0000000..3c2d44c --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java @@ -0,0 +1,76 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class FindWobbleGoal extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private double power; + private double turnCheck; + private double driveCheck; + private double range; + private boolean cCheckPrev; + private boolean ccCheckPrev; + + public FindWobbleGoal(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void init() { + power = robot.stateConfiguration.variable(groupName,actionName,"power").value(); + turnCheck = robot.stateConfiguration.variable(groupName,actionName,"max").value(); + driveCheck = robot.stateConfiguration.variable(groupName,actionName,"min").value(); + range = robot.stateConfiguration.variable(groupName,actionName,"r").value(); + } + + @Override + public void start() { + setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled); + robot.setDrivePower(power,-power,power,-power); + } + + @Override + public void exec() { + robot.updateLocation(); + double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM); + + if (sensorValue > turnCheck) { + boolean cCheck = robot.getRotation() > range; + boolean ccCheck = robot.getRotation() < -range; + + if (cCheck && !cCheckPrev) { + robot.setDrivePower(-power, power, -power, power); + } + + if (ccCheck && !ccCheckPrev) { + robot.setDrivePower(power, -power, power, -power); + } + cCheckPrev = cCheck; + ccCheckPrev = ccCheck; + + } else { + if (sensorValue > driveCheck) { + robot.setDrivePower(-power,-power,-power,-power); + } else { + robot.setDrivePower(0,0,0,0); + setHasFinished(true); + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("sensor", robot.wobbleColorSensor.getDistance(DistanceUnit.MM)); + engine.telemetry.addData("red", robot.wobbleColorSensor.red()); + engine.telemetry.addData("green", robot.wobbleColorSensor.green()); + engine.telemetry.addData("blue", robot.wobbleColorSensor.blue()); + } +} + diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/LaunchDriveControl.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/LaunchDriveControl.java new file mode 100644 index 0000000..c05c032 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/LaunchDriveControl.java @@ -0,0 +1,41 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class LaunchDriveControl extends CyberarmState { + + private Robot robot; + private CyberarmState launchState; + private CyberarmState driveState; + private boolean checkConditionPrev; + + public LaunchDriveControl(Robot robot, CyberarmState launchState, CyberarmState driveState) { + this.robot = robot; + this.launchState = launchState; + this.driveState = driveState; + } + + @Override + public void start() { + addParallelState(launchState); + } + + @Override + public void exec() { + boolean checkCondition = (robot.ringBeltStage > 3); + if (checkCondition && !checkConditionPrev) { + addParallelState(driveState); + } + checkConditionPrev = checkCondition; + + setHasFinished(childrenHaveFinished()); + } + + @Override + public void telemetry() { + engine.telemetry.addData("ring belt stage", robot.ringBeltStage); + engine.telemetry.addData("check prev", checkConditionPrev); + engine.telemetry.addData("drive prev", checkConditionPrev); + } +} 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 d83b859..f0bc532 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java @@ -9,6 +9,8 @@ public class Launch extends CyberarmState { private String actionName; boolean hasCycled; boolean detectedPass; + private boolean reduceConditionPrev; + private double reducePos; public Launch(Robot robot) { this.robot = robot; @@ -24,12 +26,12 @@ public class Launch extends CyberarmState { public void start() { try { if (robot.stateConfiguration.action(groupName, actionName).enabled) { - robot.ringBeltOn(); + robot.ringBeltMotor.setPower(0.5); } else { setHasFinished(true); } } catch (NullPointerException e){ - robot.ringBeltOn(); + robot.ringBeltMotor.setPower(0.5); } } @@ -37,9 +39,12 @@ public class Launch extends CyberarmState { public void exec() { //detect when limit switch is initially triggered boolean detectingPass = robot.limitSwitch.isPressed(); + int beltPos = robot.getBeltPos(); + 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; @@ -51,9 +56,24 @@ public class Launch extends CyberarmState { setHasFinished(true); } else { hasCycled = true; + reducePos = robot.loopPos((int) (beltPos + (1.5 * Robot.RING_BELT_GAP))); } } detectedPass = detectingPass; + boolean reduceCondition = (hasCycled && beltPos > reducePos && beltPos < reducePos + Robot.RING_BELT_GAP); + if (reduceCondition && !reduceConditionPrev){ + robot.ringBeltOn(); + //the ring belt stage lets other states know that the robot has finished launching all three rings + //and is now returning to loading position. + + robot.ringBeltStage += 1; + } + reduceConditionPrev = reduceCondition; + } + + @Override + public void telemetry() { + engine.telemetry.addData("hasCycled", hasCycled); } } 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 index d9fcbb5..4c62b31 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java @@ -26,5 +26,10 @@ public class LoadRings extends CyberarmState { addParallelState(ringBeltState); } + + if (robot.ringBeltStage > 2) { + robot.launchMotor.setPower(0); + 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 10e8581..d683cc8 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -98,7 +98,7 @@ public class Robot { //Launcher public DcMotor launchMotor; - public static final double LAUNCH_POWER = 0.7; + public static final double LAUNCH_POWER = 0.715; private static final long LAUNCH_ACCEL_TIME = 500; public double launchPositionX; @@ -212,7 +212,7 @@ public class Robot { 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"); @@ -364,8 +364,8 @@ public class Robot { rotationPrevious = imuAngle; //The forward Vector has the luxury of having an odometer on both sides of the robot. - //This allows us to eliminate the unwanted influence of turning the robot by averaging - //the two. + //This 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 @@ -474,6 +474,10 @@ public class Robot { return locationY; } + public void resetRotation(float rotation) { + this.rotation = rotation; + } + //Manually set the position of the robot on the field. public void setCurrentPosition(float rotation, double x, double y) { this.rotation = rotation; @@ -541,27 +545,20 @@ public class Robot { double p = y + x; double q = y - x; + + //calculating correction needed to steer the robot towards the degreesDirectionFace - float relativeRotation = getRelativeAngle(degreesDirectionFace, rotation); - double turnCorrection = Math.pow(LARGE_CORRECTION * relativeRotation, 3) + FINE_CORRECTION * relativeRotation; + float relativeRotation = + getRelativeAngle(degreesDirectionFace, rotation); + double turnCorrection = + Math.pow(LARGE_CORRECTION * relativeRotation, 3) + + FINE_CORRECTION * relativeRotation; double powerForwardRight = scalar * (q + turnCorrection); double powerForwardLeft = scalar * (p - turnCorrection); double powerBackRight = scalar * (p + turnCorrection); double powerBackLeft = scalar * (q - turnCorrection); -// //the turnCorrection often results in powers with magnitudes significantly larger than the -// //scalar. The scaleRatio mitigates this without altering the quality of the motion by making -// //it so that the average of the four magnitudes is equal to the scalar magnitude. -// double powerSum = Math.abs(powerForwardRight) + Math.abs(powerForwardLeft) + -// Math.abs(powerBackRight) + Math.abs(powerBackLeft); -// double scaleRatio = (4 * Math.abs(scalar))/powerSum; -// -// powerForwardRight *= scaleRatio; -// powerForwardLeft *= scaleRatio; -// powerBackRight *= scaleRatio; -// powerBackLeft *= scaleRatio; - if (relativeRotation != 0) { double momentumRelative = angularVelocity * (relativeRotation / Math.abs(relativeRotation)); @@ -588,15 +585,6 @@ public class Robot { powerBackLeft = powerBackLeft/extreme; } -// double powerControlThreshold = 0.6 * ; -// -// if (Math.min(extreme, 1) > powerControlThreshold) { -// powerForwardLeft *= powerControlThreshold; -// powerForwardRight *= powerControlThreshold; -// powerBackLeft *= powerControlThreshold; -// powerBackRight *= powerControlThreshold; -// } - double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight}; return powers; 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 f0ebc8e..d43d04b 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,5 +1,7 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + import org.cyberarm.engine.V2.CyberarmState; import org.timecrafters.UltimateGoal.Competition.Launch; import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; @@ -27,18 +29,23 @@ public class TeleOpState extends CyberarmState { private double[] powers = {0,0,0,0}; private double drivePower = 1; private static final double TURN_POWER = 1; + private static final double LAUNCH_TOLERANCE_FACE = 1; private Launch launchState; private boolean launching; private ProgressRingBelt ringBeltState; private boolean CollectorOn; - private boolean xPrev; + private boolean rbPrev; private boolean yPrev; - private boolean aPrev; + private boolean xPrev; private boolean bPrev; private boolean lbPrev; - private boolean wobbleArmUp = true; + private boolean wobbleArmUp = false; private boolean wobbleGrabOpen = false; - + private boolean emergencyLaunchPrev; + private double beltPowerPrev; + private boolean reverseBeltPrev; + private int manualArmHoldPos; + private boolean manualArmHold; private boolean launchInput = false; @@ -47,11 +54,17 @@ public class TeleOpState extends CyberarmState { this.robot = robot; } + private double LAUNCH_TOLERANCE_POS; + @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(); + robot.wobbleArmMotor.setTargetPosition(0); + robot.wobbleArmMotor.setPower(0.5); + LAUNCH_TOLERANCE_POS = robot.inchesToTicks(3); + robot.resetRotation(180); } @Override @@ -59,25 +72,6 @@ public class TeleOpState extends CyberarmState { robot.updateLocation(); robot.record(""+robot.ticksToInches(robot.getLocationX())+", "+robot.ticksToInches(robot.getLocationY())); - double leftJoystickX = engine.gamepad1.left_stick_x; - double leftJoystickY = engine.gamepad1.left_stick_y; - - leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)); - leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY); - - double rightJoystickX = engine.gamepad1.right_stick_x; - double rightJoystickY = engine.gamepad1.right_stick_y; - - rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)); - rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); - - //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; - boolean lb = engine.gamepad1.left_stick_button; if (lb && !lbPrev) { if (drivePower == 1) { @@ -91,37 +85,58 @@ public class TeleOpState extends CyberarmState { double[] powers = {0,0,0,0}; boolean y = engine.gamepad1.y; + if (y) { - //Launch Sequence - + //Drive to Launch Pos double distanceToTarget = Math.hypot(robot.launchPositionX - robot.getLocationX(), robot.launchPositionY - robot.getLocationY()); - if (distanceToTarget > Robot.LAUNCH_TOLERANCE_POS) { + if (distanceToTarget > LAUNCH_TOLERANCE_POS) { powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.launchPositionX, robot.launchPositionY), drivePower, robot.launchRotation); - } else if (robot.getRelativeAngle(robot.launchRotation, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) { - - launchState = new Launch(robot); - addParallelState(launchState); - - } else { - + } else if (Math.abs(robot.getRelativeAngle(robot.launchRotation,robot.getRotation())) > LAUNCH_TOLERANCE_FACE) { double[] facePowers = robot.getFacePowers(robot.launchRotation, TURN_POWER); powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; + } else { } - } - - if (!y || ( launchState != null && launchState.getHasFinished())) { - + } else { //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))); + 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))); + rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); + + //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; + + //allows the the driver to indicate which direction the robot is currently looking so + //so that the controller and robot can be re-synced in the event of a bad initial + //position. + if (engine.gamepad1.right_stick_button) { + robot.resetRotation(faceDirection); + } + if (leftJoystickMagnitude == 0) { double[] facePowers = robot.getFacePowers(faceDirection, TURN_POWER); 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); } @@ -137,11 +152,11 @@ public class TeleOpState extends CyberarmState { robot.collectionMotor.setPower(0); } - boolean x = engine.gamepad2.x; - if (x && !xPrev && childrenHaveFinished()) { + boolean rb = engine.gamepad2.right_bumper; + if (rb && !rbPrev && childrenHaveFinished()) { addParallelState(new ProgressRingBelt(robot)); } - xPrev = x; + rbPrev = rb; boolean y2 = engine.gamepad2.y; if (y2 && !yPrev && childrenHaveFinished()) { @@ -150,24 +165,64 @@ public class TeleOpState extends CyberarmState { } yPrev = y2; - boolean a = engine.gamepad2.a; - if (a && !aPrev && childrenHaveFinished()) { + boolean x = engine.gamepad2.x; + if (x && !xPrev) { wobbleGrabOpen = !wobbleGrabOpen; - addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100)); + if (wobbleGrabOpen) { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX); + } else { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.1 ); + } } - aPrev = a; + xPrev = x; boolean b = engine.gamepad2.b; - if (b && !bPrev && childrenHaveFinished()) { + if (b && !bPrev) { wobbleArmUp = !wobbleArmUp; - addParallelState(new WobbleArm(robot, wobbleArmUp, 100)); + if (wobbleArmUp) { + robot.wobbleArmMotor.setTargetPosition(550); + } else { + robot.wobbleArmMotor.setTargetPosition(0); + } } bPrev = b; - - if (engine.gamepad2.right_bumper && engine.gamepad2.left_bumper) { - robot.launchMotor.setPower(Robot.LAUNCH_POWER); + boolean emergencyLaunch = engine.gamepad2.a; + if (emergencyLaunch && !emergencyLaunchPrev) { + if (robot.launchMotor.getPower() == 0) { + robot.launchMotor.setPower(Robot.LAUNCH_POWER); + } else { + robot.launchMotor.setPower(0); + } } + emergencyLaunchPrev = emergencyLaunch; + + if (engine.gamepad2.dpad_up) { + robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.wobbleArmMotor.setPower(0.5); + manualArmHold = true; + } else if (engine.gamepad2.dpad_down) { + robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.wobbleArmMotor.setPower(-0.5); + manualArmHold = true; + } else if (manualArmHold) { + manualArmHold = false; + robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition()); + robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + + boolean reverseBelt = engine.gamepad2.left_bumper; + if (reverseBelt && !reverseBeltPrev) { + beltPowerPrev = robot.ringBeltMotor.getPower(); + robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER); + } + + if (!reverseBelt && reverseBeltPrev) { + robot.ringBeltMotor.setPower(beltPowerPrev); + } + + reverseBeltPrev = reverseBelt; } @@ -181,9 +236,7 @@ public class TeleOpState extends CyberarmState { } } - engine.telemetry.addData("Vision Z", robot.visionZ); - - engine.telemetry.addData("belt stage", robot.ringBeltStage); + engine.telemetry.addData("wobble Arm Up", wobbleArmUp); engine.telemetry.addLine("Location"); engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")"); diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java index 1e4684a..6f07386 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java @@ -12,7 +12,7 @@ public class WobbleArm extends CyberarmState { public WobbleArm(Robot robot, String groupName, String actionName, boolean armUp) { this.robot = robot; - this.groupName = groupName; + this.groupName = groupName; this.actionName = actionName; this.armUp = armUp; } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java index bf541c9..53c486c 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java @@ -9,6 +9,7 @@ public class WobbleGrab extends CyberarmState { private String actionName; private boolean open; private long waitTime = -1; + private boolean enabled; public WobbleGrab(Robot robot, String groupName, String actionName, boolean open) { this.robot = robot; @@ -26,13 +27,14 @@ public class WobbleGrab extends CyberarmState { @Override public void init() { if (waitTime == -1) { + enabled = robot.stateConfiguration.action(groupName, actionName).enabled; waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value(); } } @Override public void start() { - if (robot.stateConfiguration.action(groupName, actionName).enabled) { + if (enabled) { if (open) { robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX); } else { 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 cc27030..9913ea9 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -3,22 +3,23 @@ package org.timecrafters.UltimateGoal.HardwareTesting; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; 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 ServoPosTest()); + addState(new FindWobbleGoal(robot, "auto", "08_0")); } // @Override