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 29b5b1f..8b49dc1 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 @@ -12,11 +12,13 @@ public class DriveToCoordinates extends CyberarmState { private double yTarget; private float faceAngle; private double tolerancePos; - private double power; + private double basePower; private boolean braking; private long breakStartTime; private long brakeTime; private boolean autoFace; + private double decelRange; + private double decelMin; private boolean scoringArea; public DriveToCoordinates(Robot robot, String groupName, String actionName) { @@ -38,25 +40,49 @@ public class DriveToCoordinates extends CyberarmState { this.yTarget = yTarget; this.faceAngle = faceAngle; this.tolerancePos = tolerance; - this.power = power; + this.basePower = power; this.brakeTime = brakeTime; } + public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime, double decelRange, double decelMin) { + this.robot = robot; + this.xTarget = xTarget; + this.yTarget = yTarget; + this.faceAngle = faceAngle; + this.tolerancePos = tolerance; + this.basePower = power; + this.brakeTime = brakeTime; + this.decelRange = decelRange; + this.decelMin = decelMin; + } + @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(); + basePower = 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(); try { faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value(); - } catch (NullPointerException e) { + } catch (RuntimeException e) { autoFace = true; } + + try { + decelRange = robot.stateConfiguration.variable(groupName, actionName, "decelR").value(); + decelMin = robot.stateConfiguration.variable(groupName, actionName, "decelM").value(); + } catch (RuntimeException e) { + decelRange = 0; + } } + + if (decelRange > 0) { + decelRange = robot.inchesToTicks(decelRange); + } + } @Override @@ -87,9 +113,16 @@ public class DriveToCoordinates extends CyberarmState { double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY()); + // deceleration + double power = basePower; + if (distanceToTarget < decelRange) { + power = ((distanceToTarget / decelRange) * (basePower - decelMin)) + decelMin; + } + double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle); robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]); + //Once the robot is within the acceptable range, if (distanceToTarget > tolerancePos) { braking = false; } else { 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 fe1e914..c5bd4f6 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 @@ -18,6 +18,7 @@ public class FindWobbleGoal extends CyberarmState { private float startRotation; private boolean foundGoalRotation; private float wobbleGoalRotation; + private long timeLimit; public FindWobbleGoal(Robot robot, String groupName, String actionName) { this.robot = robot; @@ -31,6 +32,7 @@ public class FindWobbleGoal extends CyberarmState { turnCheck = robot.stateConfiguration.variable(groupName,actionName,"max").value(); driveCheck = robot.stateConfiguration.variable(groupName,actionName,"min").value(); range = robot.stateConfiguration.variable(groupName,actionName,"r").value(); + timeLimit = robot.stateConfiguration.variable(groupName,actionName,"limit").value(); } @Override @@ -71,11 +73,19 @@ public class FindWobbleGoal extends CyberarmState { double[] powers = robot.getMecanumPowers(wobbleGoalRotation - 90, power*2 , wobbleGoalRotation); robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]); } else { - robot.setDrivePower(0,0,0,0); - robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); - setHasFinished(true); + endSearch(); } } + + if (runTime() > timeLimit) { + endSearch(); + } + } + + private void endSearch() { + robot.setDrivePower(0,0,0,0); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + setHasFinished(true); } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java index a5b48c7..88bf2ff 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java @@ -15,6 +15,8 @@ public class Park extends CyberarmState { double parkTolerance; double parkPower; long parkBrakeTime; + double parkDecelRange; + double parkDecelMin; public Park(Robot robot, String groupName, String actionName) { this.robot = robot; @@ -29,12 +31,15 @@ public class Park extends CyberarmState { parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto",actionName,"tolPos").value()); parkPower = robot.stateConfiguration.variable(groupName,actionName,"power").value(); parkBrakeTime = robot.stateConfiguration.variable(groupName,actionName,"brakeMS").value(); + parkDecelMin = robot.stateConfiguration.variable(groupName,actionName,"decelM").value(); + parkDecelRange = robot.stateConfiguration.variable(groupName,actionName,"decelR").value(); } @Override public void start() { - if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8)) - addParallelState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime)); + if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8)) { + addParallelState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower, parkBrakeTime,parkDecelRange,parkDecelMin)); + } } @Override 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 938005c..4df0635 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java @@ -31,7 +31,7 @@ public class ProgressRingBelt extends CyberarmState { targetPos = currentPos + robot.ringBeltGap; prep(); } else if (robot.ringBeltStage == 2) { - targetPos = currentPos + 240; + targetPos = currentPos + 150; prep(); prepLaunch = !robot.initLauncher; } else if (robot.ringBeltStage > 2) { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java new file mode 100644 index 0000000..f929553 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java @@ -0,0 +1,57 @@ +package org.timecrafters.UltimateGoal.Competition; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; + +public class ResetRingBelt extends CyberarmState { + + private Robot robot; + boolean detectedPass; + int reducePos; + boolean reduceConditionPrev; + + public ResetRingBelt(Robot robot) { + this.robot = robot; + } + + + @Override + public void start() { + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); + reducePos = robot.ringBeltMotor.getCurrentPosition() + robot.reduceLaunchPos; + } + + @Override + public void exec() { + //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. + + + robot.ringBeltStage = 0; + robot.ringBeltMotor.setTargetPosition(beltPos); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + robot.launchMotor.setPower(0); + setHasFinished(true); + } + detectedPass = detectingPass; + + boolean reduceCondition = (beltPos > reducePos); + if (reduceCondition && !reduceConditionPrev){ + robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER); + + //the ring belt stage lets other states know that the robot has finished launching all three rings + //and is now returning to loading position. + + } + reduceConditionPrev = reduceCondition; + } + +} 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 e12e6ba..89d619a 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -10,7 +10,6 @@ import android.os.Environment; import android.util.Log; import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.hardware.rev.RevTouchSensor; @@ -86,6 +85,7 @@ public class Robot { 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; @@ -432,19 +432,13 @@ public class Robot { locationY += yChange; Robot.rotation += rotationChange; - + Robot.rotation = scaleAngleRange(Robot.rotation); totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange); - - if (Robot.rotation > 180) { - Robot.rotation -= 360; - } - if (Robot.rotation < -180) { - Robot.rotation += 360; - } - } + + public void syncIfStationary() { if (totalV < minCheckVelocity) { long timeCurrent = System.currentTimeMillis(); @@ -551,16 +545,7 @@ public class Robot { //to the right (clockwise) of the current. Negative angles indicate that the reference is to the //left. public float getRelativeAngle(float reference, float current) { - float relative = current - reference; - - if (relative < -180) { - relative += 360; - } - - if (relative > 180) { - relative -= 360; - } - return relative; + return scaleAngleRange(current - reference); } //Drive Functions @@ -628,6 +613,15 @@ public class Robot { 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; } @@ -635,7 +629,8 @@ public class Robot { public double[] getFacePowers(float direction, double power) { angularVelocity = imu.getAngularVelocity().xRotationRate; double relativeAngle = getRelativeAngle(direction, Robot.rotation); - double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) + FACE_LINEAR_CORRECTION * relativeAngle; + double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) + + FACE_LINEAR_CORRECTION * relativeAngle; if (relativeAngle > 0.5) { scaler += FACE_MIN_CORRECTION; @@ -645,7 +640,8 @@ public class Robot { if (relativeAngle != 0) { double momentumRelative = angularVelocity * (relativeAngle / Math.abs(relativeAngle)); - double exponential = Math.pow(FACE_MOMENTUM_CORRECTION, FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); + double exponential = Math.pow(FACE_MOMENTUM_CORRECTION, + FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential); scaler *= momentumCorrection; @@ -655,6 +651,12 @@ public class Robot { 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; } @@ -665,6 +667,16 @@ public class Robot { return notMoved; } + public float scaleAngleRange(float angle) { + if (angle < -180) { + angle += 360; + } + if (angle > 180) { + angle -= 360; + } + return angle; + } + 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 new file mode 100644 index 0000000..689f396 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java @@ -0,0 +1,58 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Launch; +import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; +import org.timecrafters.UltimateGoal.Competition.ResetRingBelt; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class LaunchControl extends CyberarmState { + + private Robot robot; + private boolean ranLaunch; + + public LaunchControl(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + robot.launchMotor.setPower(Robot.LAUNCH_POWER); + + if (robot.ringBeltStage > 2) { + if (robot.ringBeltStage > 4) { + addParallelState(new ResetRingBelt(robot)); + } else { + addParallelState(new LaunchSingle(robot)); + } + ranLaunch = true; + } + } + + @Override + public void exec() { + if (childrenHaveFinished()) { + if (!ranLaunch) { + if (robot.ringBeltStage <= 2) { + addParallelState(new ProgressRingBelt(robot)); + } else { + addParallelState(new LaunchSingle(robot)); + ranLaunch = true; + } + } else { + setHasFinished(true); + } + } + } + + @Override + public void telemetry() { + + engine.telemetry.addData("Launch Control 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/LaunchSingle.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchSingle.java new file mode 100644 index 0000000..337b662 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchSingle.java @@ -0,0 +1,52 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.UnstickRingBelt; + +public class LaunchSingle extends CyberarmState { + + private Robot robot; + boolean hasCycled; + boolean detectedPass; + private int targetPos; + private boolean reduceConditionPrev; + private double reducePos; + private long stuckStartTime; + + public LaunchSingle(Robot robot) { + this.robot = robot; + } + + + @Override + public void start() { + robot.ringBeltStage +=1; + robot.ringBeltMotor.setTargetPosition(robot.ringBeltMotor.getCurrentPosition() + robot.ringBeltGap); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); + } + + @Override + public void exec() { + int currentPos = robot.ringBeltMotor.getCurrentPosition(); + if (currentPos >= targetPos) { + setHasFinished(true); + } + + 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; + } + + } + +} 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 62e1f79..543621b 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 @@ -33,18 +33,15 @@ public class Player1 extends CyberarmState { private boolean aPrev; //Drive to launch control - private DriveToCoordinates driveToLaunch; + private powerShotsControl powerShots; private boolean runNextDriveToLaunch; private boolean driveToLaunchInputPrev; - private double launchTolerance; - private double launchPower; - private long launchBrakeTime; + private double endGameX; + private double endGameY; + private float endGameRot; - private float launchAngleGoal; - private float launchAnglePower1; - private float launchAnglePower2; - private float launchAnglePower3; + private double refinePower; public Player1(Robot robot) { this.robot = robot; @@ -55,15 +52,11 @@ public class Player1 extends CyberarmState { 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(); - launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("tele","launchPosG","tolPos").value()); - launchPower = robot.stateConfiguration.variable("tele","launchPosG","power").value(); - launchBrakeTime = robot.stateConfiguration.variable("tele","launchPosG","brakeMS").value(); - - launchAngleGoal = robot.stateConfiguration.variable("tele","launchAngles","goal").value(); - launchAnglePower1 = robot.stateConfiguration.variable("tele","launchAngles","p1").value(); - launchAnglePower2 = robot.stateConfiguration.variable("tele","launchAngles","p2").value(); - launchAnglePower3 = robot.stateConfiguration.variable("tele","launchAngles","p3").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 @@ -85,9 +78,6 @@ public class Player1 extends CyberarmState { } lbPrev = lb; - if (engine.gamepad1.guide) { - robot.syncIfStationary(); - } runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished()); @@ -123,20 +113,25 @@ public class Player1 extends CyberarmState { } aPrev = a; - runNextDriveToLaunch = (driveToLaunch == null || driveToLaunch.getHasFinished()); + runNextDriveToLaunch = (powerShots == null || powerShots.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); + powerShots = new powerShotsControl(robot); + addParallelState(powerShots); } faceDirection = robot.getRotation(); } else if (!runNextDriveToLaunch) { - driveToLaunch.setHasFinished(true); + powerShots.setHasFinished(true); } driveToLaunchInputPrev = driveToLaunchInput; +// if (engine.gamepad1.y) { +// robot.setLocalization(endGameRot,endGameX,endGameY); +// setHasFinished(true); +// } + if (childrenHaveFinished()) { //Normal Driver Controls @@ -156,8 +151,14 @@ public class Player1 extends CyberarmState { //so that the controller and robot can be re-synced in the event of a bad initial //position. if (engine.gamepad1.back) { - robot.setLocalization(rightJoystickDegrees, robot.getLocationX(), robot.getLocationY()); - faceDirection = rightJoystickDegrees; + 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. @@ -168,18 +169,13 @@ public class Player1 extends CyberarmState { } rightJoystickMagnitudePrevious = rightJoystickMagnitude; - //sets the launch positions to - if (engine.gamepad1.dpad_up) { - faceDirection = launchAngleGoal; - } else if (engine.gamepad1.dpad_right) { - faceDirection = launchAnglePower1; - } else if (engine.gamepad1.dpad_down) { - faceDirection = launchAnglePower2; + if (engine.gamepad1.dpad_right) { + powers = new double[]{refinePower, -refinePower, refinePower, -refinePower}; + faceDirection = robot.getRotation(); } else if (engine.gamepad1.dpad_left) { - faceDirection = launchAnglePower3; - } - - if (leftJoystickMagnitude == 0) { + 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 { 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 dde0575..624665e 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 @@ -58,33 +58,29 @@ public class Player2 extends CyberarmState { robot.collectionMotor.setPower(0); } - //belt progression control - boolean rb = engine.gamepad2.right_bumper; - if (rb && !rbPrev && childrenHaveFinished()) { - addParallelState(new ProgressRingBelt(robot)); + if (childrenHaveFinished()) { + //belt progression control + boolean rb = engine.gamepad2.right_bumper; + if (rb && !rbPrev) { + addParallelState(new ProgressRingBelt(robot)); + } + rbPrev = rb; + + //main launch sequence control + boolean y2 = engine.gamepad2.y; + if (y2 && !yPrev) { + addParallelState(new Launch(robot)); + } + yPrev = y2; + + + //special launch sequence for single shots + boolean x = engine.gamepad2.x; + if (x && !bPrev) { + addParallelState(new LaunchControl(robot)); + } + bPrev = x; } - rbPrev = rb; - - //launch sequence control - boolean y2 = engine.gamepad2.y; - if (y2 && !yPrev && childrenHaveFinished()) { - addParallelState(new Launch(robot)); - } - yPrev = y2; - - - - //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 control the wobble arm for when it's initialized in an unexpected position. double leftStickY = engine.gamepad2.left_stick_y; @@ -142,8 +138,11 @@ public class Player2 extends CyberarmState { robot.ringBeltMotor.setPower(beltPowerPrev); robot.ringBeltMotor.setMode(runModePrev); } - lbPrev = lb; + +// if (engine.gamepad1.y) { +// setHasFinished(true); +// } } private void setArmMode(DcMotor.RunMode runMode) { @@ -159,14 +158,15 @@ public class Player2 extends CyberarmState { // engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition()); // engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition()); - 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("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()); +// } +// } } } 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 23a9e22..a3b3b60 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 @@ -3,6 +3,8 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; 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; @TeleOp (name = "TeleOp",group = "comp") @@ -14,7 +16,7 @@ public class TeleOpEngine extends CyberarmEngine { public void init() { robot = new Robot(hardwareMap); robot.initHardware(); - robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); robot.webCamServo.setPosition(0); super.init(); } @@ -22,6 +24,8 @@ public class TeleOpEngine extends CyberarmEngine { @Override public void setup() { addState(new TeleOpState(robot)); + + } @Override 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 c190924..c32801e 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 @@ -25,7 +25,7 @@ public class TeleOpState extends CyberarmState { @Override public void exec() { - +// setHasFinished(childrenHaveFinished()); } @Override 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 new file mode 100644 index 0000000..c2e897f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java @@ -0,0 +1,75 @@ +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.Robot; + +import java.util.ArrayList; + +public class powerShotsControl extends CyberarmState { + + private Robot robot; + + private double endGameX; + private double endGameY; + private float endGameRot; + + private int nextState = 0; + + private ArrayList states = new ArrayList(); + + public powerShotsControl(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + 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.inchesToTicks(endGameX); + endGameY = robot.inchesToTicks(endGameY); + endGameRot = (float) robot.inchesToTicks(endGameRot); + + states.add(new DriveToCoordinates(robot, "tele", "_pow1")); + states.add(new Face(robot,"tele","_faceZero")); + states.add(new LaunchControl(robot)); + states.add(new DriveToCoordinates(robot, "tele", "_pow2")); + states.add(new Face(robot,"tele","_faceZero")); + states.add(new LaunchControl(robot)); + states.add(new DriveToCoordinates(robot, "tele", "_pow3")); + states.add(new Face(robot,"tele","_faceZero")); + states.add(new LaunchControl(robot)); + } + + @Override + public void start() { + robot.setLocalization(endGameRot,endGameX,endGameY); + robot.launchMotor.setPower(Robot.LAUNCH_POWER); + } + + @Override + public void exec() { + if (childrenHaveFinished()) { + if (nextState < states.size()) { + addParallelState(states.get(nextState)); + nextState += 1; + } else { + robot.launchMotor.setPower(0); + setHasFinished(true); + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("Power shots children", childrenHaveFinished()); + for (CyberarmState state : children) { + if (!state.getHasFinished()) { + engine.telemetry.addLine("" + state.getClass()); + } + } + } +}