diff --git a/.idea/compiler.xml b/.idea/compiler.xml new file mode 100644 index 0000000..61a9130 --- /dev/null +++ b/.idea/compiler.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml index 37a7509..d5d35ec 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,6 +1,6 @@ - + diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java index 90d4ee4..f477226 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java @@ -32,7 +32,7 @@ public class OdometryCalibration extends CyberarmState { rotation += robot.getRelativeAngle(imu, rotationPrev); rotationPrev = imu; - currentTick = robot.encoderBack.getCurrentPosition(); + currentTick = (robot.encoderRight.getCurrentPosition() + robot.encoderLeft.getCurrentPosition()) / 2; if (engine.gamepad1.x) { robot.setDrivePower(power, -power, power, -power); 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 5c4cfa0..11ce84c 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 @@ -3,12 +3,15 @@ package org.timecrafters.UltimateGoal.Competition.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Launch; import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.WobbleGrab; @Autonomous (name = "Autonomous") public class AutoEngine extends CyberarmEngine { private Robot robot; + private TensorFlowCheck tensorFlowCheck; @Override public void init() { @@ -20,8 +23,55 @@ public class AutoEngine extends CyberarmEngine { @Override public void setup() { //drive to view - addState(new DriveToCoordinates(robot, "auto", "001_0")); - addState(new DriveToCoordinates(robot, "auto", "001_0")); + addState(new DriveToCoordinates(robot, "auto", "01_0")); + //select scoring area + tensorFlowCheck = new TensorFlowCheck(robot, "auto", "02_0"); + addState(tensorFlowCheck); + + //drive around ring stack + addState(new DriveToCoordinates(robot, "auto", "03_0")); + + //drive to launch position + double launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value()); + double launchPower = robot.stateConfiguration.variable("auto","04_0","power").value(); + long launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","bakeMS").value(); + addState(new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime)); + + //launch rings + addState(new Launch(robot)); + + //drive to scoring area + float scoreAFaceAngle = robot.stateConfiguration.variable("auto","05_0","faceAngle").value(); + double scoreATolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value()); + double scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value(); + long scoreABrakeTime = robot.stateConfiguration.variable("auto","05_0","bakeMS").value(); + addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreAFaceAngle,scoreATolerance,scoreAPower,scoreABrakeTime)); + + //turn arm towards scoreing area. + addState(new Face(robot, "auto", "05_1")); + + //open the wobble grab arm + addState(new WobbleGrab(robot, "auto", "06_0", true)); + + //drive to second wobble + addState(new DriveToCoordinates(robot, "auto", "07_0")); + addState(new DriveWithColorSensor(robot, "auto", "08_0")); + + //close grabber + addState(new WobbleGrab(robot, "auto", "09_0", false)); + + //drive to scoring area + float scoreBFaceAngle = robot.stateConfiguration.variable("auto","05_0","faceAngle").value(); + double scoreBTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value()); + double scoreBPower = robot.stateConfiguration.variable("auto","05_0","power").value(); + long scoreBBrakeTime = robot.stateConfiguration.variable("auto","05_0","bakeMS").value(); + addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreBFaceAngle,scoreBTolerance,scoreBPower,scoreBBrakeTime)); + + //release wobble goal 2 + addState(new WobbleGrab(robot, "auto", "10_0", true)); + + //drive to park + addState(new DriveToCoordinates(robot, "auto", "11_0")); } } 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 3fab97d..117be67 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 @@ -15,7 +15,7 @@ public class DriveToCoordinates extends CyberarmState { private double power; private boolean braking; private long breakStartTime; - private long breakTime; + private long brakeTime; private boolean autoFace; public DriveToCoordinates(Robot robot, String groupName, String actionName) { @@ -24,14 +24,14 @@ public class DriveToCoordinates extends CyberarmState { this.actionName = actionName; } - public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long breakStartTime) { + public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime) { this.robot = robot; this.xTarget = xTarget; this.yTarget = yTarget; this.faceAngle = faceAngle; this.tolerancePos = tolerance; this.power = power; - this.breakTime = breakTime; + this.brakeTime = brakeTime; } @Override @@ -41,7 +41,7 @@ public class DriveToCoordinates extends CyberarmState { yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value()); power = robot.stateConfiguration.variable(groupName, actionName, "power").value(); tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value()); - breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value(); + brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value(); try { faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value(); @@ -53,6 +53,11 @@ public class DriveToCoordinates extends CyberarmState { @Override public void start() { + + if (robot.stateConfiguration.action(groupName,actionName).enabled) { + setHasFinished(true); + } + if (autoFace) { faceAngle = robot.getAngleToPosition(xTarget,yTarget); } @@ -73,7 +78,7 @@ public class DriveToCoordinates extends CyberarmState { breakStartTime = currentTime; braking = true; } - setHasFinished(currentTime - breakStartTime >= breakTime); + setHasFinished(currentTime - breakStartTime >= brakeTime); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java new file mode 100644 index 0000000..378beb4 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java @@ -0,0 +1,41 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class DriveWithColorSensor extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private double power; + private double minimum; + private double maximum; + + public DriveWithColorSensor(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void init() { + power = robot.stateConfiguration.variable(groupName,actionName,"power").value(); + minimum = robot.stateConfiguration.variable(groupName,actionName,"min").value(); + maximum = robot.stateConfiguration.variable(groupName,actionName,"max").value(); + } + + @Override + public void exec() { + double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM); + if (sensorValue < minimum) { + robot.getMecanumPowers(180,power,0); + } else if (sensorValue > maximum) { + robot.getMecanumPowers(0,power,0); + } else { + robot.setDrivePower(0,0,0,0); + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java index 803348f..e8c4434 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java @@ -17,12 +17,18 @@ public class TensorFlowCheck extends CyberarmState { private long checkTime; public double wobblePosX; public double wobblePosY; + private int manualPath; - + public TensorFlowCheck(Robot robot, String group, String action) { + this.robot = robot; + this.group = group; + this.action = action; + } @Override public void init() { checkTime = robot.stateConfiguration.variable(group,action,"time").value(); + manualPath = robot.stateConfiguration.variable(group,action,"path").value(); } @Override @@ -32,27 +38,41 @@ public class TensorFlowCheck extends CyberarmState { @Override public void exec() { - recognitions = robot.tfObjectDetector.getUpdatedRecognitions(); - if (recognitions != null) { + if (manualPath == -1) { + recognitions = robot.tfObjectDetector.getUpdatedRecognitions(); - if (recognitions.size() == 1) { - Recognition recognition = recognitions.get(0); - String label = recognition.getLabel(); - if (label.equals("Single")) { - path = 1; - } else if (label.equals("Quad")) { - path = 2; + if (recognitions != null) { + + if (recognitions.size() == 1) { + Recognition recognition = recognitions.get(0); + String label = recognition.getLabel(); + if (label.equals("Single")) { + path = 1; + } else if (label.equals("Quad")) { + path = 2; + } + } else if (recognitions.size() > 1) { + path = 1; } - } else if (recognitions.size() > 1) { - path = 1; } + } else { + path = manualPath; } if (runTime() >= checkTime) { - if (checkTime == 0) { - wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos1","x").value(); + if (path == 0) { + wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos0","x").value(); + wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos0","y").value(); } + if (path == 1) { + wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos1","x").value(); + wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos1","y").value(); + } + if (path == 2) { + wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos2","x").value(); + wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos2","y").value(); + } } } 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 1b114e1..25fbf96 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java @@ -27,6 +27,11 @@ public class Launch extends CyberarmState { if (hasCycled) { robot.ringBeltMotor.setPower(0); robot.ringBeltStage = 0; + + if (!robot.initLauncher) { + robot.launchMotor.setPower(0); + } + setHasFinished(true); } else { hasCycled = true; @@ -34,6 +39,5 @@ public class Launch extends CyberarmState { } detectedPass = detectingPass; -// if (robot.getBeltPos() > robot.loopPos(Robot.RING_BELT_GAP * 3) && hasCycled); } } 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 13813c3..3a2912e 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java @@ -6,17 +6,12 @@ public class ProgressRingBelt extends CyberarmState { private Robot robot; private int targetPos; - private boolean useLaunchPrep; + private boolean prepLaunch; public ProgressRingBelt(Robot robot) { this.robot = robot; } - @Override - public void init() { - useLaunchPrep = (robot.launchMotor.getPower() == 0); - } - @Override public void start() { int currentPos = robot.getBeltPos(); @@ -28,6 +23,7 @@ public class ProgressRingBelt extends CyberarmState { targetPos = robot.loopPos(currentPos + 160); robot.ringBeltOn(); robot.ringBeltStage += 1; + prepLaunch = !robot.initLauncher; } } @@ -38,7 +34,7 @@ public class ProgressRingBelt extends CyberarmState { if (currentPos >= targetPos && currentPos < targetPos + Robot.RING_BELT_GAP) { robot.ringBeltMotor.setPower(0); - if(useLaunchPrep) { + if(prepLaunch) { robot.launchMotor.setPower(Robot.LAUNCH_POWER); } 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 121877b..a80834f 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -5,6 +5,7 @@ import android.util.Log; import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -60,9 +61,9 @@ public class Robot { //Steering Constants static final double FINE_CORRECTION = 0.055 ; - static final double LARGE_CORRECTION = 0.025 ; - static final double MOMENTUM_CORRECTION = 1.02; - static final double MOMENTUM_MAX_CORRECTION = 1.3; + static final double LARGE_CORRECTION = 0.025; + static final double MOMENTUM_CORRECTION = 1.045; + static final double MOMENTUM_MAX_CORRECTION = 1.4; static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION)); //Conversion Constants @@ -95,13 +96,13 @@ public class Robot { public static final double LAUNCH_POWER = 0.75; private static final long LAUNCH_ACCEL_TIME = 500; - public static final double LAUNCH_POSITION_X = 36 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); - public static final double LAUNCH_POSITION_Y = -8 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); - public static final float LAUNCH_ROTATION = 0; + public double launchPositionX; + public double launchPositionY; + public float launchRotation; 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; @@ -123,8 +124,8 @@ public class Robot { public DcMotor wobbleArmMotor; public Servo wobbleGrabServo; public static final int WOBBLE_ARM_DOWN = -710; - public static final double WOBBLE_SERVO_MAX = 0.3; + public RevColorSensorV3 wobbleColorSensor; //vuforia navigation private WebcamName webcam; @@ -249,8 +250,11 @@ public class Robot { launchMotor = launcher; launchMotor.setMotorType(launchMotorType); launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - if (stateConfiguration.action("system","initLauncher").enabled) { + initLauncher = stateConfiguration.action("system","initLauncher").enabled; + + if (initLauncher) { double launcherPower = 0; long launchAccelStart = System.currentTimeMillis(); while (launcherPower < LAUNCH_POWER) { @@ -258,6 +262,10 @@ public class Robot { launchMotor.setPower(launcherPower); } } + // + launchPositionX = stateConfiguration.variable("system", "launchPos","x").value(); + launchPositionX = stateConfiguration.variable("system", "launchPos","y").value(); + launchRotation = stateConfiguration.variable("system", "launchPos","rot").value(); } @@ -353,7 +361,7 @@ public class Robot { double ticksPerDegree; - if(rotationChange < 0) { + if (rotationChange < 0) { ticksPerDegree = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE; } else { ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE; 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 9f37b0f..98502ad 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 @@ -14,11 +14,12 @@ public class TeleOpState extends CyberarmState { private double leftJoystickMagnitude; private float rightJoystickDegrees; private double rightJoystickMagnitude; + 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 boolean toggleSpeedInput = false; private Launch launchState; private boolean launching; private ProgressRingBelt ringBeltState; @@ -27,11 +28,12 @@ public class TeleOpState extends CyberarmState { private boolean yPrev; private boolean aPrev; private boolean bPrev; - private boolean rbPrev; + private boolean lbPrev; private boolean wobbleArmUp = true; private boolean wobbleGrabOpen = false; + private boolean launchInput = false; @@ -40,8 +42,9 @@ public class TeleOpState extends CyberarmState { } @Override - public void start() { - + public void init() { + cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value(); + pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value(); } @Override @@ -57,14 +60,19 @@ public class TeleOpState extends CyberarmState { double rightJoystickX = engine.gamepad1.right_stick_x; double rightJoystickY = engine.gamepad1.right_stick_y; - rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)); + rightJoystickDegrees = snapToCardinal((float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)),cardinalSnapping,0); rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); - if (leftJoystickMagnitude > 0.66) { - drivePower = 1 ; - } else { - drivePower = 0.6; + + 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}; @@ -73,18 +81,18 @@ public class TeleOpState extends CyberarmState { //Launch Sequence - double distanceToTarget = Math.hypot(Robot.LAUNCH_POSITION_X - robot.getLocationX(), Robot.LAUNCH_POSITION_Y - robot.getLocationY()); + double distanceToTarget = Math.hypot(robot.launchPositionX - robot.getLocationX(), robot.launchPositionY - robot.getLocationY()); if (distanceToTarget > Robot.LAUNCH_TOLERANCE_POS) { - powers = robot.getMecanumPowers(robot.getAngleToPosition(Robot.LAUNCH_POSITION_X, Robot.LAUNCH_POSITION_Y), drivePower, Robot.LAUNCH_ROTATION); + powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.launchPositionX, robot.launchPositionY), drivePower, robot.launchRotation); - } else if (robot.getRelativeAngle(Robot.LAUNCH_ROTATION, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) { + } else if (robot.getRelativeAngle(robot.launchRotation, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) { launchState = new Launch(robot); addParallelState(launchState); } else { - double[] facePowers = robot.getFacePowers(Robot.LAUNCH_ROTATION, TURN_POWER); + double[] facePowers = robot.getFacePowers(robot.launchRotation, TURN_POWER); powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; } @@ -97,14 +105,16 @@ public class TeleOpState extends CyberarmState { if (rightJoystickMagnitude == 0) { if (leftJoystickMagnitude !=0) { - powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, leftJoystickDegrees); + float direction = snapToCardinal(leftJoystickDegrees,cardinalSnapping,0); + powers = robot.getMecanumPowers(direction, drivePower, direction); } } else { if (leftJoystickMagnitude == 0) { double[] facePowers = robot.getFacePowers(rightJoystickDegrees, TURN_POWER * rightJoystickMagnitude); powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; } else { - powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, rightJoystickDegrees); + + powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees,pairSnapping,rightJoystickDegrees), drivePower, rightJoystickDegrees); } } } @@ -113,16 +123,15 @@ public class TeleOpState extends CyberarmState { this.powers = powers; + if (childrenHaveFinished()) { + robot.collectionMotor.setPower(engine.gamepad2.right_trigger); + } else { + robot.collectionMotor.setPower(0); + } boolean x = engine.gamepad2.x; if (x && !xPrev && childrenHaveFinished()) { - if (CollectorOn) { - robot.collectionMotor.setPower(0); - CollectorOn = false; - } else { - robot.collectionMotor.setPower(1); - CollectorOn = true; - } + addParallelState(new ProgressRingBelt(robot)); } xPrev = x; @@ -135,41 +144,48 @@ public class TeleOpState extends CyberarmState { boolean a = engine.gamepad2.a; if (a && !aPrev && childrenHaveFinished()) { - addParallelState(new ProgressRingBelt(robot)); + wobbleGrabOpen = !wobbleGrabOpen; + addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100)); } aPrev = a; boolean b = engine.gamepad2.b; - if (b && !bPrev) { + if (b && !bPrev && childrenHaveFinished()) { wobbleArmUp = !wobbleArmUp; addParallelState(new WobbleArm(robot, wobbleArmUp, 100)); } bPrev = b; - boolean rb = engine.gamepad2.right_bumper; - if (rb && !rbPrev) { - wobbleGrabOpen = !wobbleGrabOpen ; - addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100)); - } - rbPrev = rb; + } @Override public void telemetry() { - engine.telemetry.addLine("Powers"); - for (double power : powers) { - engine.telemetry.addData(" ", power); - } + engine.telemetry.addData("childrenHaveFinished", childrenHaveFinished()); + for (CyberarmState state : children) { + engine.telemetry.addLine(""+state.getClass()); + } engine.telemetry.addLine("Location"); - engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()))+","+round(robot.ticksToInches(robot.getLocationY()))+")"); + engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")"); engine.telemetry.addData("Rotation ", robot.getRotation()); engine.telemetry.addData("totalV", robot.totalV); } - private float round(double number) { - return ((float) Math.floor(number*100)) / 100; + private float round(double number,double unit) { + 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/WobbleGrab.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java index 5ee929c..5dc7a0a 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java @@ -17,9 +17,9 @@ public class WobbleGrab extends CyberarmState { this.open = open; } - public WobbleGrab(Robot robot, boolean armUp, long waitTime) { + public WobbleGrab(Robot robot, boolean open, long waitTime) { this.robot = robot; - this.open = armUp; + this.open = open; this.waitTime = waitTime; }