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 bf5b0a4..0a26eb4 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java @@ -1,10 +1,12 @@ package org.timecrafters.UltimateGoal.Competition.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmEngine; import org.cyberarm.engine.V2.CyberarmState; import org.timecrafters.UltimateGoal.Competition.Launch; +import org.timecrafters.UltimateGoal.Competition.Pause; import org.timecrafters.UltimateGoal.Competition.Robot; import org.timecrafters.UltimateGoal.Competition.WobbleArm; import org.timecrafters.UltimateGoal.Competition.WobbleGrab; @@ -21,26 +23,19 @@ public class AutoEngine extends CyberarmEngine { private double launchPower; private long launchBrakeTime; - private float scoreAFaceAngle; - private double scoreATolerance; - private double scoreAPower; - private long scoreABrakeTime; - - 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); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + robot.wobbleArmMotor.setTargetPosition(0); + robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // since we've preloaded three rings, the ring belt stage is set to account for this; robot.ringBeltStage = 3; + //Autonomous specific 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()); @@ -50,17 +45,6 @@ public class AutoEngine extends CyberarmEngine { launchPower = robot.stateConfiguration.variable("auto","04_0","power").value(); launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value(); -// scoreAFaceAngle = robot.stateConfiguration.variable("auto","05_0","face").value(); -// scoreATolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value()); -// scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value(); -// scoreABrakeTime = robot.stateConfiguration.variable("auto","05_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(); } @@ -85,18 +69,18 @@ public class AutoEngine extends CyberarmEngine { //aligns to goal addState(new Face(robot, "auto", "04_1")); - //launch rings + //launch rings and drive to scoreing area. LaunchDriveControl allows makes the robot begin + //driving while the belt is resetting CyberarmState launchState = new Launch(robot, "auto", "04_2"); - //drive to scoring area CyberarmState driveState = new DriveToCoordinates(robot, "auto", "05_0", true); addState(new LaunchDriveControl(robot,launchState,driveState)); //turn arm towards scoreing area. - ArrayList threadStates = new ArrayList<>(); - threadStates.add(new Face(robot, "auto", "05_1")); - threadStates.add(new WobbleArm(robot, "auto", "05_2",false)); - addState(new ThreadStates(threadStates)); + ArrayList threadStates0 = new ArrayList<>(); + threadStates0.add(new Face(robot, "auto", "05_1")); + threadStates0.add(new WobbleArm(robot, "auto", "05_2",robot.wobbleDownPos)); + addState(new ThreadStates(threadStates0)); //open the wobble grab arm addState(new WobbleGrab(robot, "auto", "06_0", true)); @@ -105,25 +89,32 @@ public class AutoEngine extends CyberarmEngine { addState(new DriveToCoordinates(robot, "auto","06_1")); addState(new DriveToCoordinates(robot, "auto", "07_0")); - addState(new FindWobbleGoal(robot, "auto", "08_0")); + addState(new Face(robot,"auto","07_1")); - //close grabber - addState(new WobbleGrab(robot, "auto", "09_0", false)); + addState(new FindWobbleGoal(robot, "auto", "08_0")); + addState(new Pause(robot,"auto","09_0")); //drive to scoring area -// addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreBFaceAngle,scoreBTolerance,scoreBPower,scoreBBrakeTime)); - addState(new DriveToCoordinates(robot, "auto", "10_0", true)); + ArrayList threadStates1 = new ArrayList<>(); +// threadStates1.add(new Face(robot, "auto", "09_0")); + threadStates1.add(new WobbleArm(robot, "auto", "09_1",robot.wobbleUpPos)); + threadStates1.add(new DriveToCoordinates(robot, "auto", "10_0", true)); + addState(new ThreadStates(threadStates1)); + + + + ArrayList threadStates2 = new ArrayList<>(); + threadStates2.add(new Face(robot, "auto", "11_0")); + threadStates2.add(new WobbleArm(robot, "auto", "11_1",robot.wobbleDownPos)); + addState(new ThreadStates(threadStates2)); //release wobble goal 2 - addState(new Face(robot, "auto", "11_0")); addState(new WobbleGrab(robot, "auto", "12_0", true)); //drive to park - 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)); + ArrayList threadStates3 = new ArrayList<>(); + threadStates3.add(new WobbleArm(robot, "auto", "12_2", 0)); + threadStates3.add(new Park(robot,"auto","13_0")); + addState(new ThreadStates(threadStates3)); } } 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 ba42f32..fe1e914 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 @@ -16,6 +16,8 @@ public class FindWobbleGoal extends CyberarmState { private boolean cCheckPrev; private boolean ccCheckPrev; private float startRotation; + private boolean foundGoalRotation; + private float wobbleGoalRotation; public FindWobbleGoal(Robot robot, String groupName, String actionName) { this.robot = robot; @@ -44,6 +46,7 @@ public class FindWobbleGoal extends CyberarmState { double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM); if (sensorValue > turnCheck) { + float rotation = robot.getRelativeAngle(startRotation,robot.getRotation()); boolean cCheck = rotation > range; @@ -61,9 +64,15 @@ public class FindWobbleGoal extends CyberarmState { } else { if (sensorValue > driveCheck) { - robot.setDrivePower(-power,-power,-power,-power); + if (!foundGoalRotation) { + foundGoalRotation = true; + wobbleGoalRotation = robot.getRotation(); + } + 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); } } 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 new file mode 100644 index 0000000..70f3206 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java @@ -0,0 +1,45 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +import java.util.ArrayList; + +public class Park extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + double parkY; + float parkFaceAngle; + double parkTolerance; + double parkPower; + long parkBrakeTime; + + public Park(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void init() { + parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"yPos").value()); + parkFaceAngle = robot.stateConfiguration.variable(groupName,actionName,"face").value(); + 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(); + } + + @Override + public void start() { + if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8)) + addState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime)); + } + + @Override + public void exec() { + setHasFinished(childrenHaveFinished()); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo1.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo1.java deleted file mode 100644 index bfe95bd..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo1.java +++ /dev/null @@ -1,164 +0,0 @@ -package org.timecrafters.UltimateGoal.Competition.Demo; - -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.Robot; - -public class Demo1 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; - - private float launchAngleGoal; - private float launchAnglePower1; - private float launchAnglePower2; - private float launchAnglePower3; - - public Demo1(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(); - - } - - @Override - public void start() { - faceDirection = robot.getRotation(); - } - - @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.dpad_up; - 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; - - 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); - - //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.setLocalization(rightJoystickDegrees, robot.getLocationX(), robot.getLocationY()); - faceDirection = rightJoystickDegrees; - } - - //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; - - - 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]); - } - - } - - @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()); - } - } - } - - 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/Demo2.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo2.java deleted file mode 100644 index 643919d..0000000 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo2.java +++ /dev/null @@ -1,142 +0,0 @@ -package org.timecrafters.UltimateGoal.Competition.Demo; - -import com.qualcomm.hardware.rev.RevBlinkinLedDriver; -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 Demo2 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 Demo2(Robot robot) { - this.robot = robot; - } - - - @Override - public void init() { - robot.wobbleArmMotor.setTargetPosition(0); - robot.wobbleArmMotor.setPower(0.5); - } - - @Override - public void start() { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN); - } - - @Override - public void exec() { - - //Collector control - if (childrenHaveFinished()) { - robot.collectionMotor.setPower(engine.gamepad1.right_trigger); - } else { - robot.collectionMotor.setPower(0); - } - - //belt progression control - boolean rb = engine.gamepad1.right_bumper; - if (rb && !rbPrev && childrenHaveFinished()) { - addParallelState(new ProgressRingBelt(robot)); - } - rbPrev = rb; - - //launch sequence control - boolean y2 = engine.gamepad1.y; - if (y2 && !yPrev && childrenHaveFinished()) { - addParallelState(new Launch(robot)); - } - yPrev = y2; - - //toggles wobble grabber open and closed - boolean x = engine.gamepad1.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.gamepad1.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.gamepad1.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.gamepad1.dpad_up) { - robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.wobbleArmMotor.setPower(0.5); - manualArmHold = true; - } else if (engine.gamepad1.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.gamepad1.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/Demo/DemoControl.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoControl.java index 86bbd50..82a1ed6 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoControl.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoControl.java @@ -15,8 +15,8 @@ public class DemoControl extends CyberarmState { @Override public void start() { - addParallelState(new Demo1(robot)); - addParallelState(new Demo2(robot)); +// addParallelState(new Demo1(robot)); +// addParallelState(new Demo2(robot)); } @Override 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 a6319e8..3145699 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,11 +1,13 @@ 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") public class DemoEngine extends CyberarmEngine { 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 092e38c..1d003c7 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java @@ -31,14 +31,14 @@ public class Launch extends CyberarmState { try { if (robot.stateConfiguration.action(groupName, actionName).enabled) { robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.ringBeltMotor.setPower(0.7); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); } else { setHasFinished(true); } } catch (NullPointerException e){ robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.ringBeltMotor.setPower(0.7); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); } } @@ -65,25 +65,25 @@ public class Launch extends CyberarmState { //the first receiving position. if (hasCycled) { - robot.ringBeltMotor.setPower(0); robot.ringBeltStage = 0; + robot.ringBeltMotor.setTargetPosition(beltPos); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); if (!robot.initLauncher) { robot.launchMotor.setPower(0); } setHasFinished(true); - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN); } else { hasCycled = true; - reducePos = (int) (beltPos + (robot.reduceLaunchPos)); + reducePos = beltPos + (robot.reduceLaunchPos); } } detectedPass = detectingPass; boolean reduceCondition = (hasCycled && beltPos > reducePos); if (reduceCondition && !reduceConditionPrev){ - robot.ringBeltOn(); + 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. diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/CamServo.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java similarity index 59% rename from TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/CamServo.java rename to TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java index c381a2c..102dcf0 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/CamServo.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java @@ -2,40 +2,37 @@ package org.timecrafters.UltimateGoal.Competition; import org.cyberarm.engine.V2.CyberarmState; -public class CamServo extends CyberarmState { +public class Pause extends CyberarmState { private Robot robot; private String groupName; private String actionName; - private boolean open; private long waitTime = -1; + private boolean enabled; - public CamServo(Robot robot, String groupName, String actionName, boolean open) { + public Pause(Robot robot, String groupName, String actionName) { this.robot = robot; this.groupName = groupName; this.actionName = actionName; - this.open = open; } - public CamServo(Robot robot, boolean armUp, long waitTime) { + public Pause(Robot robot, boolean open, long waitTime) { this.robot = robot; - this.open = armUp; this.waitTime = waitTime; } @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 (open) { - robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX); - } else { - robot.wobbleGrabServo.setPosition(0); + if (!enabled) { + setHasFinished(true); } } @@ -44,4 +41,10 @@ public class CamServo extends CyberarmState { setHasFinished(runTime() > waitTime); } + @Override + public void telemetry() { + engine.telemetry.addData("runTime", runTime()); + engine.telemetry.addData("wait", waitTime); + } + } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java index d72229e..da726cd 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java @@ -13,7 +13,7 @@ public class FindLimitSwitch extends CyberarmState { @Override public void start() { - robot.ringBeltOn(); + robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER); } @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 cfabc00..938005c 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java @@ -19,7 +19,7 @@ public class ProgressRingBelt extends CyberarmState { private void prep(){ robot.ringBeltMotor.setTargetPosition(targetPos); robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.ringBeltMotor.setPower(0.7); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); robot.ringBeltStage += 1; robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); } @@ -28,7 +28,7 @@ public class ProgressRingBelt extends CyberarmState { public void start() { int currentPos = robot.ringBeltMotor.getCurrentPosition(); if (robot.ringBeltStage < 2) { - targetPos = currentPos + Robot.RING_BELT_GAP; + targetPos = currentPos + robot.ringBeltGap; prep(); } else if (robot.ringBeltStage == 2) { targetPos = currentPos + 240; 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 69900a6..e12e6ba 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -1,5 +1,11 @@ package org.timecrafters.UltimateGoal.Competition; +/* +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; @@ -47,12 +53,18 @@ public class Robot { 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. 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 system + //drive and dead-wheal hardware public DcMotor driveFrontLeft; public DcMotor driveBackLeft; public DcMotor driveFrontRight; @@ -62,8 +74,9 @@ public class Robot { public DcMotor encoderRight; public DcMotor encoderBack; - //Steering Constants - static final double CUBIC_CORRECTION = 0.025; + //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; @@ -74,7 +87,7 @@ public class Robot { 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)); - //Conversion Constants + //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; @@ -83,16 +96,6 @@ public class Robot { static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4; static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6; - // Inches Forward of axis of rotation - static final float CAMERA_FORWARD_DISPLACEMENT = 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); - //Robot Localization private static double locationX; private static double locationY; @@ -104,6 +107,38 @@ public class Robot { private float rotationPrevious = 0; public float angularVelocity; + //vuforia navigation + 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; @@ -114,57 +149,36 @@ public class Robot { public double launchPositionY; public float launchRotation; public int reduceLaunchPos; - public static final double LAUNCH_TOLERANCE_POS = 0.5 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); - public static final double LAUNCH_TOLERANCE_FACE = 0.5; public boolean initLauncher; //Ring Intake public DcMotor collectionMotor; - public Rev2mDistanceSensor ringIntakeSensor; - - public static final double RING_DETECT_DISTANCE = 100; - public static final double RING_DETECT_DELAY = 1000; //Ring Belt public DcMotor ringBeltMotor; public RevTouchSensor limitSwitch; public int ringBeltStage; - public static final int RING_BELT_LOOP_TICKS = 2544; - public static final int RING_BELT_GAP = 700; - public static final double RING_BELT_POWER = 0.2; + 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 - public DcMotor wobbleArmMotor; + //Wobble Goal Arm & Grabber + public DcMotor wobbleArmMotor; public Servo wobbleGrabServo; - public static final int WOBBLE_ARM_DOWN = -710; - public static final double WOBBLE_SERVO_MAX = 0.3; + 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; - - //vuforia navigation - private WebcamName webcam; - private VuforiaLocalizer vuforia; - public Servo webCamServo; - public static final double CAM_SERVO_DOWN = 0.15; - - 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; - - //TensorFlow Object Detection - public TFObjectDetector tfObjectDetector; - private static final float MINIMUM_CONFIDENCE = 0.8f; + public RevTouchSensor wobbleTouchSensor; //Debugging public double totalV; @@ -172,7 +186,6 @@ public class Robot { public double visionY; public double visionZ; public float rawAngle; -// private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight"; private String TestingRecord = "x,y"; public double forwardVector; @@ -213,9 +226,15 @@ 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(); + wobbleGrabServo = hardwareMap.servo.get("wobbleGrab"); wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color"); + wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch"); + //init ring belt collectionMotor = hardwareMap.dcMotor.get("collect"); @@ -228,6 +247,7 @@ public class Robot { 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"); @@ -259,8 +279,9 @@ public class Robot { webCamServo = hardwareMap.servo.get("look"); webCamServo.setDirection(Servo.Direction.REVERSE ); - minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value(); - minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "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"); @@ -352,7 +373,7 @@ 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", "tensorFlow", "minConfidence").value(); + parameters.minResultConfidence = stateConfiguration.variable("system", "camera", "minConfidence").value(); tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia); tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single"); } @@ -413,22 +434,7 @@ public class Robot { Robot.rotation += rotationChange; -// totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange); - -// -// if (totalV < minCheckVelocity) { -// long timeCurrent = System.currentTimeMillis(); -// -// if (timeStartZeroVelocity == 0) { -// timeStartZeroVelocity = timeCurrent; -// } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs) { -// syncWithVuforia(); -// } -// -// } else { -// timeStartZeroVelocity = 0; -// } - + totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange); if (Robot.rotation > 180) { Robot.rotation -= 360; @@ -439,6 +445,22 @@ public class Robot { } + public void syncIfStationary() { + if (totalV < minCheckVelocity) { + long timeCurrent = System.currentTimeMillis(); + + if (timeStartZeroVelocity == 0) { + timeStartZeroVelocity = timeCurrent; + } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs ) { + syncWithVuforia(); + } + + } else { + timeStartZeroVelocity = 0; + } + + } + public void syncWithVuforia() { trackableVisible = false; for (VuforiaTrackable trackable : trackables) { @@ -454,24 +476,28 @@ 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 rotation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES); - Robot.rotation = 90-rotation.thirdAngle; + Orientation orientation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES); + float vuforiaRotation = 90-orientation.thirdAngle; - if (Robot.rotation > 180) { - Robot.rotation -= -180; + if (vuforiaRotation > 180) { + vuforiaRotation -= -180; } - VectorF translation = lastConfirmendLocation.getTranslation(); - double camX = -translation.get(1) / mmPerInch; - double camY = translation.get(0) / mmPerInch; + if (Math.abs(rotation - vuforiaRotation) < vuforiaRotationCull) { + rotation = vuforiaRotation; - 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); + VectorF translation = lastConfirmendLocation.getTranslation(); + double camX = -translation.get(1) / mmPerInch; + double camY = translation.get(0) / mmPerInch; - locationX = inchesToTicks(camX - displaceX); - locationY = inchesToTicks(camY - displaceY); + 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); - break; + locationX = inchesToTicks(camX - displaceX); + locationY = inchesToTicks(camY - displaceY); + + break; + } } } } @@ -570,22 +596,21 @@ public class Robot { 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 ); +// turnCorrection *= momentumCorrection; + } + double powerForwardRight = scalar * (q + turnCorrection); double powerForwardLeft = scalar * (p - turnCorrection); double powerBackRight = scalar * (p + turnCorrection); double powerBackLeft = scalar * (q - turnCorrection); - 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); - powerForwardRight *= momentumCorrection; - powerForwardLeft *= momentumCorrection; - powerBackRight *= momentumCorrection; - powerBackLeft *= momentumCorrection; - } - // 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. @@ -610,7 +635,7 @@ public class Robot { public double[] getFacePowers(float direction, double power) { angularVelocity = imu.getAngularVelocity().xRotationRate; double relativeAngle = getRelativeAngle(direction, Robot.rotation); - double scaler = Math.pow(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; @@ -633,10 +658,6 @@ public class Robot { return powers; } - public void ringBeltOn() { - ringBeltMotor.setPower(RING_BELT_POWER); - } - public boolean beltIsStuck() { int ringBeltPos = ringBeltMotor.getCurrentPosition(); boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks); 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 0873d29..62e1f79 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 @@ -30,6 +30,7 @@ public class Player1 extends CyberarmState { private FindWobbleGoal findWobbleGoal; private boolean runNextFindWobble; private boolean findWobbleInputPrev; + private boolean aPrev; //Drive to launch control private DriveToCoordinates driveToLaunch; @@ -84,20 +85,43 @@ public class Player1 extends CyberarmState { } lbPrev = lb; + if (engine.gamepad1.guide) { + robot.syncIfStationary(); + } + runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished()); boolean findWobbleInput = engine.gamepad1.x; if (findWobbleInput) { - if (runNextFindWobble && !findWobbleInputPrev) { - findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0"); - addParallelState(findWobbleGoal); + //if the claw is open, run FindWobbleGoal + if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) { + + faceDirection = robot.getRotation(); + + if (runNextFindWobble && !findWobbleInputPrev) { + findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0"); + addParallelState(findWobbleGoal); + } + //if the claw is closed, open the claw. + } else if (!findWobbleInputPrev) { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN); } - faceDirection = robot.getRotation(); + //if the button is released cancel the search } else if (!runNextFindWobble) { findWobbleGoal.setHasFinished(true); } findWobbleInputPrev = findWobbleInput; + //toggles wobble grabber open and closed + boolean a = engine.gamepad1.a; + if (a && !aPrev) { + if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + } else { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN); + } + } + aPrev = a; runNextDriveToLaunch = (driveToLaunch == null || driveToLaunch.getHasFinished()); @@ -168,7 +192,7 @@ public class Player1 extends CyberarmState { 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 ); 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 57a7262..dde0575 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 @@ -23,6 +23,8 @@ public class Player2 extends CyberarmState { private boolean lbPrev; private boolean manualArmHold; + + private boolean launchInput = false; public Player2(Robot robot) { @@ -45,14 +47,13 @@ public class Player2 extends CyberarmState { public void exec() { //Collector control - if (childrenHaveFinished()) { - double rt = engine.gamepad2.right_trigger; - double lt = engine.gamepad2.left_trigger; - if (rt >= lt) { - robot.collectionMotor.setPower(rt); - } else { - robot.collectionMotor.setPower(-lt); - } + + double rt = engine.gamepad2.right_trigger; + double lt = engine.gamepad2.left_trigger; + if (rt < lt) { + robot.collectionMotor.setPower(-lt); + } else if (childrenHaveFinished()) { + robot.collectionMotor.setPower(rt); } else { robot.collectionMotor.setPower(0); } @@ -71,29 +72,48 @@ public class Player2 extends CyberarmState { } 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); +// 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; + + if (engine.gamepad2.dpad_right) { + if (!robot.wobbleTouchSensor.isPressed()) { + setArmMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.wobbleArmMotor.setPower(-0.2); + robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition()); } else { + setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.wobbleArmMotor.setTargetPosition(0); + setArmMode(DcMotor.RunMode.RUN_TO_POSITION); + } + } else if (leftStickY != 0 ) { + setArmMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.wobbleArmMotor.setPower(0.15 * leftStickY); + robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition()); + } else { + setArmMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.wobbleArmMotor.setPower(0.5); + if (engine.gamepad2.dpad_up) { + robot.wobbleArmMotor.setTargetPosition(robot.wobbleUpPos); + } else if (engine.gamepad2.dpad_down) { + robot.wobbleArmMotor.setTargetPosition(robot.wobbleDownPos); + } else if (engine.gamepad2.dpad_left) { + robot.wobbleArmMotor.setTargetPosition(robot.wobbleDropPos ); } } - bPrev = b; //manually toggle the launch wheel for emergencies boolean a = engine.gamepad2.a; @@ -106,20 +126,8 @@ public class Player2 extends CyberarmState { } 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; @@ -127,7 +135,7 @@ public class Player2 extends CyberarmState { runModePrev = robot.ringBeltMotor.getMode(); beltPowerPrev = robot.ringBeltMotor.getPower(); robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER); + robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER); } if (!lb && lbPrev) { @@ -138,13 +146,21 @@ public class Player2 extends CyberarmState { lbPrev = lb; } + 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.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("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()) { 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 553a2d5..23a9e22 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 @@ -14,7 +14,7 @@ public class TeleOpEngine extends CyberarmEngine { public void init() { robot = new Robot(hardwareMap); robot.initHardware(); - robot.wobbleGrabServo.setPosition(0); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN); robot.webCamServo.setPosition(0); super.init(); } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java index 8438a7d..825d805 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java @@ -1,6 +1,5 @@ package org.timecrafters.UltimateGoal.Competition; -import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; @@ -22,7 +21,7 @@ public class UnstickRingBelt extends CyberarmState { lastRunMode = robot.ringBeltMotor.getMode(); int currentPos = robot.ringBeltMotor.getCurrentPosition(); targetPos = currentPos - robot.beltReverseTicks; - robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER); + robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER); } @Override 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 6f07386..4bf2ce1 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java @@ -7,19 +7,20 @@ public class WobbleArm extends CyberarmState { private Robot robot; private String groupName; private String actionName; - private boolean armUp; + private int suggestedPos; + private int targetPos = -1; private long waitTime = -1; - public WobbleArm(Robot robot, String groupName, String actionName, boolean armUp) { + public WobbleArm(Robot robot, String groupName, String actionName, int targetPos) { this.robot = robot; this.groupName = groupName; this.actionName = actionName; - this.armUp = armUp; + this.suggestedPos = targetPos; } - public WobbleArm(Robot robot, boolean armUp, long waitTime) { + public WobbleArm(Robot robot, int targetPos, long waitTime) { this.robot = robot; - this.armUp = armUp; + this.targetPos = targetPos; this.waitTime = waitTime; } @@ -27,17 +28,17 @@ public class WobbleArm extends CyberarmState { public void init() { if (waitTime == -1) { waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value(); + targetPos = robot.stateConfiguration.variable(groupName,actionName,"armPos").value(); + if (targetPos == -1) { + targetPos = suggestedPos; + } } } @Override public void start() { - robot.wobbleArmMotor.setPower(0.3); - if (armUp) { - robot.wobbleArmMotor.setTargetPosition(0); - } else { - robot.wobbleArmMotor.setTargetPosition(Robot.WOBBLE_ARM_DOWN); - } + robot.wobbleArmMotor.setPower(0.5); + robot.wobbleArmMotor.setTargetPosition(targetPos); } @Override @@ -48,5 +49,7 @@ public class WobbleArm extends CyberarmState { @Override public void telemetry() { engine.telemetry.addData("runTime", runTime()); + engine.telemetry.addData("target", targetPos); + engine.telemetry.addData("wait", waitTime); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java index 53c486c..e24f75d 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java @@ -36,9 +36,9 @@ public class WobbleGrab extends CyberarmState { public void start() { if (enabled) { if (open) { - robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN); } else { - robot.wobbleGrabServo.setPosition(0); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); } } else { setHasFinished(true); @@ -50,4 +50,10 @@ public class WobbleGrab extends CyberarmState { setHasFinished(runTime() > waitTime); } + @Override + public void telemetry() { + engine.telemetry.addData("runTime", runTime()); + engine.telemetry.addData("wait", waitTime); + } + } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java index f00af01..71d5579 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java @@ -20,12 +20,11 @@ public class ServoPosTest extends CyberarmState { @Override public void init() { servo = engine.hardwareMap.servo.get("look"); - servo.setDirection(Servo.Direction.REVERSE ); } @Override public void exec() { - servoPos = engine.gamepad1.right_stick_y * 0.2; + servoPos = engine.gamepad1.right_stick_y; servo.setPosition(servoPos); } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java index 3f92521..2d55990 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -13,7 +13,7 @@ public class TestingEngine extends CyberarmEngine { @Override public void setup() { - addState(new LEDTest()); + addState(new ServoPosTest()); } }