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 3c2d44c..ba42f32 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 @@ -15,6 +15,7 @@ public class FindWobbleGoal extends CyberarmState { private double range; private boolean cCheckPrev; private boolean ccCheckPrev; + private float startRotation; public FindWobbleGoal(Robot robot, String groupName, String actionName) { this.robot = robot; @@ -33,6 +34,7 @@ public class FindWobbleGoal extends CyberarmState { @Override public void start() { setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled); + startRotation = robot.getRotation(); robot.setDrivePower(power,-power,power,-power); } @@ -42,8 +44,10 @@ public class FindWobbleGoal extends CyberarmState { double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM); if (sensorValue > turnCheck) { - boolean cCheck = robot.getRotation() > range; - boolean ccCheck = robot.getRotation() < -range; + float rotation = robot.getRelativeAngle(startRotation,robot.getRotation()); + + boolean cCheck = rotation > range; + boolean ccCheck = rotation < -range; if (cCheck && !cCheckPrev) { robot.setDrivePower(-power, power, -power, power); 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 f0bc532..180989e 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java @@ -11,6 +11,7 @@ public class Launch extends CyberarmState { boolean detectedPass; private boolean reduceConditionPrev; private double reducePos; + private long stuckStartTime; public Launch(Robot robot) { this.robot = robot; @@ -39,7 +40,18 @@ public class Launch extends CyberarmState { public void exec() { //detect when limit switch is initially triggered boolean detectingPass = robot.limitSwitch.isPressed(); - int beltPos = robot.getBeltPos(); + int beltPos = robot.ringBeltMotor.getCurrentPosition(); + + if (robot.beltIsStuck() && childrenHaveFinished()) { + long currentTime = System.currentTimeMillis(); + if (stuckStartTime == 0) { + stuckStartTime = currentTime; + } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) { + addParallelState(new UnstickRingBelt(robot)); + } + } else { + stuckStartTime = 0; + } if (detectingPass && !detectedPass) { //finish once the ring belt has cycled all the way through and then returned to @@ -61,7 +73,7 @@ public class Launch extends CyberarmState { } detectedPass = detectingPass; - boolean reduceCondition = (hasCycled && beltPos > reducePos && beltPos < reducePos + Robot.RING_BELT_GAP); + boolean reduceCondition = (hasCycled && beltPos > reducePos); if (reduceCondition && !reduceConditionPrev){ robot.ringBeltOn(); //the ring belt stage lets other states know that the robot has finished launching all three rings 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 4b13c97..bc7c13f 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java @@ -7,6 +7,7 @@ public class ProgressRingBelt extends CyberarmState { private Robot robot; private int targetPos; private boolean prepLaunch; + private long stuckStartTime; public ProgressRingBelt(Robot robot) { this.robot = robot; @@ -14,7 +15,7 @@ public class ProgressRingBelt extends CyberarmState { @Override public void start() { - int currentPos = robot.getBeltPos(); + int currentPos = robot.ringBeltMotor.getCurrentPosition(); if (robot.ringBeltStage < 2) { targetPos = robot.loopPos(currentPos + Robot.RING_BELT_GAP); robot.ringBeltOn(); @@ -33,8 +34,19 @@ public class ProgressRingBelt extends CyberarmState { @Override public void exec() { - int currentPos = robot.getBeltPos(); - if (currentPos >= targetPos && currentPos < targetPos + Robot.RING_BELT_GAP) { + if (robot.beltIsStuck() && childrenHaveFinished()) { + long currentTime = System.currentTimeMillis(); + if (stuckStartTime == 0) { + stuckStartTime = currentTime; + } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) { + addParallelState(new UnstickRingBelt(robot)); + } + } else { + stuckStartTime = 0; + } + + int currentPos = robot.ringBeltMotor.getCurrentPosition(); + if (currentPos >= targetPos) { robot.ringBeltMotor.setPower(0); if(prepLaunch) { 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 d683cc8..ab63650 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -96,6 +96,7 @@ public class Robot { private float rotationPrevious = 0; public float angularVelocity; + //Launcher public DcMotor launchMotor; public static final double LAUNCH_POWER = 0.715; @@ -122,8 +123,11 @@ public class Robot { public int ringBeltStage; public static final int RING_BELT_LOOP_TICKS = 2544; public static final int RING_BELT_GAP = 670; - public static final double RING_BELT_POWER = 0.2; + private int ringBeltPrev; + public long beltMaxStopTime; + public int beltReverseTicks; + public int beltMaxStopTicks; //Wobble Goal Arm public DcMotor wobbleArmMotor; @@ -212,7 +216,9 @@ 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(); //init IMU imu = hardwareMap.get(BNO055IMU.class, "imu"); @@ -615,8 +621,11 @@ public class Robot { ringBeltMotor.setPower(RING_BELT_POWER); } - public int getBeltPos(){ - return loopPos(ringBeltMotor.getCurrentPosition()); + public boolean beltIsStuck() { + int ringBeltPos = ringBeltMotor.getCurrentPosition(); + boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks); + ringBeltPrev = ringBeltPos; + return notMoved; } public int loopPos(int pos) { 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 new file mode 100644 index 0000000..349e626 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java @@ -0,0 +1,168 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; +import org.timecrafters.UltimateGoal.Competition.Launch; +import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class Player1 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; + + //find wobble goal control + private FindWobbleGoal findWobbleGoal; + private boolean runNextFindWobble; + private boolean findWobbleInputPrev; + + //Drive to launch control + private DriveToCoordinates driveToLaunch; + private boolean runNextDriveToLaunch; + private boolean driveToLaunchInputPrev; + + private double launchTolerance; + private double launchPower; + private long launchBrakeTime; + + public Player1(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(); + + launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value()); + launchPower = robot.stateConfiguration.variable("auto","04_0","power").value(); + launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value(); + } + + @Override + public void exec() { + robot.updateLocation(); + + boolean lb = engine.gamepad1.left_stick_button; + if (lb && !lbPrev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + lbPrev = lb; + + runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished()); + + boolean findWobbleInput = engine.gamepad1.x; + if (findWobbleInput) { + if (runNextFindWobble && !findWobbleInputPrev) { + findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0"); + addParallelState(findWobbleGoal); + } + faceDirection = robot.getRotation(); + } else if (!runNextFindWobble) { + findWobbleGoal.setHasFinished(true); + } + findWobbleInputPrev = findWobbleInput; + + + runNextDriveToLaunch = (driveToLaunch == null || driveToLaunch.getHasFinished()); + + boolean driveToLaunchInput = engine.gamepad1.y && !findWobbleInput; + if (driveToLaunchInput) { + if (runNextDriveToLaunch && !driveToLaunchInputPrev) { + driveToLaunch = new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime); + addParallelState(driveToLaunch); + } + faceDirection = robot.getRotation(); + } else if (!runNextDriveToLaunch) { + driveToLaunch.setHasFinished(true); + } + driveToLaunchInputPrev = driveToLaunchInput; + + 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))); + 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, drivePower); + 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]); + } + + } + + @Override + public void telemetry() { + engine.telemetry.addData("Player 1 children", childrenHaveFinished()); + for (CyberarmState state : children) { + if (!state.getHasFinished()) { + engine.telemetry.addLine("" + state.getClass()); + } + } + } + + 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/TeleOp/Player2.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java new file mode 100644 index 0000000..bc35acc --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java @@ -0,0 +1,136 @@ +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; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class Player2 extends CyberarmState { + private Robot robot; + + private boolean rbPrev; + private boolean yPrev; + private boolean xPrev; + private boolean bPrev; + private boolean wobbleArmUp = false; + private boolean wobbleGrabOpen = false; + private boolean aPrev; + private double beltPowerPrev; + private boolean lbPrev; + private boolean manualArmHold; + + private boolean launchInput = false; + + public Player2(Robot robot) { + this.robot = robot; + } + + + @Override + public void init() { + robot.wobbleArmMotor.setTargetPosition(0); + robot.wobbleArmMotor.setPower(0.5); + } + + @Override + public void exec() { + + //Collector control + if (childrenHaveFinished()) { + robot.collectionMotor.setPower(engine.gamepad2.right_trigger); + } else { + robot.collectionMotor.setPower(0); + } + + //belt progression control + boolean rb = engine.gamepad2.right_bumper; + if (rb && !rbPrev && childrenHaveFinished()) { + addParallelState(new ProgressRingBelt(robot)); + } + rbPrev = rb; + + //launch sequence control + boolean y2 = engine.gamepad2.y; + if (y2 && !yPrev && childrenHaveFinished()) { + addParallelState(new Launch(robot)); + } + yPrev = y2; + + //toggles wobble grabber open and closed + boolean x = engine.gamepad2.x; + if (x && !xPrev) { + wobbleGrabOpen = !wobbleGrabOpen; + if (wobbleGrabOpen) { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX); + } else { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.05 ); + } + } + xPrev = x; + + //toggles the wobble arm up and down. + boolean b = engine.gamepad2.b; + if (b && !bPrev) { + wobbleArmUp = !wobbleArmUp; + if (wobbleArmUp) { + robot.wobbleArmMotor.setTargetPosition(550); + } else { + robot.wobbleArmMotor.setTargetPosition(0); + } + } + bPrev = b; + + //manually toggle the launch wheel for emergencies + boolean a = engine.gamepad2.a; + if (a && !aPrev) { + if (robot.launchMotor.getPower() == 0) { + robot.launchMotor.setPower(Robot.LAUNCH_POWER); + } else { + robot.launchMotor.setPower(0); + } + } + aPrev = a; + + //manually control the wobble arm for when it's initialized in an unexpected position. + 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.1); + manualArmHold = true; + } else if (manualArmHold) { + manualArmHold = false; + robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition()); + robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + //allows the driver to revers the belt in the event of a jam + boolean lb = engine.gamepad2.left_bumper; + if (lb && !lbPrev) { + beltPowerPrev = robot.ringBeltMotor.getPower(); + robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER); + } + + if (!lb && lbPrev) { + robot.ringBeltMotor.setPower(beltPowerPrev); + } + + lbPrev = lb; + + } + + @Override + public void telemetry() { + 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/TeleOpState.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java index d43d04b..c190924 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 @@ -3,241 +3,34 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; import org.timecrafters.UltimateGoal.Competition.Launch; import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; import org.timecrafters.UltimateGoal.Competition.Robot; -import org.timecrafters.UltimateGoal.Competition.WobbleArm; -import org.timecrafters.UltimateGoal.Competition.WobbleGrab; public class TeleOpState extends CyberarmState { private Robot robot; - private float leftJoystickDegrees; - private double leftJoystickMagnitude; - private float rightJoystickDegrees; - private double rightJoystickMagnitude; - private double rightJoystickMagnitudePrevious; - - private float faceDirection = 0; - - private double faceControlThreshold; - private float cardinalSnapping; - private float pairSnapping; - - - - 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 rbPrev; - private boolean yPrev; - private boolean xPrev; - private boolean bPrev; - private boolean lbPrev; - 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; - public TeleOpState(Robot robot) { 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); + public void start() { + addParallelState(new Player1(robot)); + addParallelState(new Player2(robot)); } @Override public void exec() { - robot.updateLocation(); - robot.record(""+robot.ticksToInches(robot.getLocationX())+", "+robot.ticksToInches(robot.getLocationY())); - - boolean lb = engine.gamepad1.left_stick_button; - if (lb && !lbPrev) { - if (drivePower == 1) { - drivePower = 0.5; - } else { - drivePower = 1; - } - } - lbPrev = lb; - - double[] powers = {0,0,0,0}; - - boolean y = engine.gamepad1.y; - - if (y) { - - //Drive to Launch Pos - double distanceToTarget = Math.hypot(robot.launchPositionX - robot.getLocationX(), robot.launchPositionY - robot.getLocationY()); - if (distanceToTarget > LAUNCH_TOLERANCE_POS) { - powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.launchPositionX, robot.launchPositionY), drivePower, robot.launchRotation); - - } 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 { - - } - - } 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); - } - - } - - robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]); - - this.powers = powers; - - if (childrenHaveFinished()) { - robot.collectionMotor.setPower(engine.gamepad2.right_trigger); - } else { - robot.collectionMotor.setPower(0); - } - - boolean rb = engine.gamepad2.right_bumper; - if (rb && !rbPrev && childrenHaveFinished()) { - addParallelState(new ProgressRingBelt(robot)); - } - rbPrev = rb; - - boolean y2 = engine.gamepad2.y; - if (y2 && !yPrev && childrenHaveFinished()) { - launchState = new Launch(robot); - addParallelState(launchState); - } - yPrev = y2; - - boolean x = engine.gamepad2.x; - if (x && !xPrev) { - wobbleGrabOpen = !wobbleGrabOpen; - if (wobbleGrabOpen) { - robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX); - } else { - robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.1 ); - } - } - xPrev = x; - - boolean b = engine.gamepad2.b; - if (b && !bPrev) { - wobbleArmUp = !wobbleArmUp; - if (wobbleArmUp) { - robot.wobbleArmMotor.setTargetPosition(550); - } else { - robot.wobbleArmMotor.setTargetPosition(0); - } - } - bPrev = b; - - 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; } @Override public void telemetry() { - engine.telemetry.addData("childrenHaveFinished", childrenHaveFinished()); - for (CyberarmState state : children) { - if (!state.getHasFinished()) { - engine.telemetry.addLine("" + state.getClass()); - } - } - - 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)+")"); engine.telemetry.addData("Rotation ", robot.getRotation()); @@ -247,16 +40,4 @@ public class TeleOpState extends CyberarmState { return (float) (Math.floor(number/unit) * unit); } - 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/UnstickRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java new file mode 100644 index 0000000..7452b91 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java @@ -0,0 +1,33 @@ +package org.timecrafters.UltimateGoal.Competition; + +import org.cyberarm.engine.V2.CyberarmState; + +public class UnstickRingBelt extends CyberarmState { + + private Robot robot; + private int targetPos; + private double lastPower; + + public UnstickRingBelt(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + lastPower = robot.ringBeltMotor.getPower(); + int currentPos = robot.ringBeltMotor.getCurrentPosition(); + targetPos = robot.loopPos(currentPos - robot.beltReverseTicks); + robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER); + + } + + @Override + public void exec() { + int currentPos = robot.ringBeltMotor.getCurrentPosition(); + if (currentPos < targetPos) { + robot.ringBeltMotor.setPower(lastPower); + setHasFinished(true); + } + } + +}