From afcd9f0e62c7536f481fe6d17eadb84e7d95f519 Mon Sep 17 00:00:00 2001 From: Nathaniel Palme Date: Thu, 11 Mar 2021 20:52:54 -0600 Subject: [PATCH] Driver controlled Enhancements & Ring belt debugging --- .../Competition/Autonomous/AutoEngine.java | 5 + .../UltimateGoal/Competition/Demo/Demo1.java | 164 ++++++++++++++++++ .../UltimateGoal/Competition/Demo/Demo2.java | 142 +++++++++++++++ .../Competition/Demo/DemoControl.java | 39 +++++ .../Competition/Demo/DemoEngine.java | 34 ++++ .../UltimateGoal/Competition/Launch.java | 12 +- .../Competition/ProgressRingBelt.java | 21 ++- .../UltimateGoal/Competition/Robot.java | 111 ++++++------ .../Competition/TeleOp/Player1.java | 68 ++++++-- .../Competition/TeleOp/Player2.java | 18 +- .../Competition/UnstickRingBelt.java | 4 +- 11 files changed, 539 insertions(+), 79 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo1.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo2.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoControl.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java index 0bb6a12..bf5b0a4 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 @@ -41,6 +41,11 @@ public class AutoEngine extends CyberarmEngine { // since we've preloaded three rings, the ring belt stage is set to account for this; robot.ringBeltStage = 3; + 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()); + robot.setLocalization(rotation,locationX,locationY); + launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value()); launchPower = robot.stateConfiguration.variable("auto","04_0","power").value(); launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value(); 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 new file mode 100644 index 0000000..bfe95bd --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo1.java @@ -0,0 +1,164 @@ +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 new file mode 100644 index 0000000..643919d --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo2.java @@ -0,0 +1,142 @@ +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 new file mode 100644 index 0000000..86bbd50 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoControl.java @@ -0,0 +1,39 @@ +package org.timecrafters.UltimateGoal.Competition.Demo; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.TeleOp.Player1; +import org.timecrafters.UltimateGoal.Competition.TeleOp.Player2; + +public class DemoControl extends CyberarmState { + + private Robot robot; + + public DemoControl(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + addParallelState(new Demo1(robot)); + addParallelState(new Demo2(robot)); + } + + @Override + public void exec() { + + } + + @Override + public void telemetry() { + + engine.telemetry.addLine("Location"); + engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")"); + engine.telemetry.addData("Rotation ", robot.getRotation()); + } + + private float round(double number,double unit) { + return (float) (Math.floor(number/unit) * unit); + } + +} 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 new file mode 100644 index 0000000..a6319e8 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java @@ -0,0 +1,34 @@ +package org.timecrafters.UltimateGoal.Competition.Demo; + +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; + +@TeleOp (name = "Demo") +public class DemoEngine extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + robot.wobbleGrabServo.setPosition(0); + robot.webCamServo.setPosition(0); + super.init(); + } + + @Override + public void setup() { + addState(new DemoControl(robot)); + } + + @Override + public void stop() { + robot.deactivateVuforia(); + robot.saveRecording(); + super.stop(); + } +} 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 02ecc42..092e38c 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java @@ -1,6 +1,7 @@ package org.timecrafters.UltimateGoal.Competition; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; @@ -29,14 +30,15 @@ public class Launch extends CyberarmState { public void start() { try { if (robot.stateConfiguration.action(groupName, actionName).enabled) { - robot.ringBeltMotor.setPower(0.5); - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_BLUE); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ringBeltMotor.setPower(0.7); + } else { setHasFinished(true); } } catch (NullPointerException e){ - robot.ringBeltMotor.setPower(0.5); - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_BLUE); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ringBeltMotor.setPower(0.7); } } @@ -74,7 +76,7 @@ public class Launch extends CyberarmState { robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN); } else { hasCycled = true; - reducePos = (int) (beltPos + (1.5 * Robot.RING_BELT_GAP)); + reducePos = (int) (beltPos + (robot.reduceLaunchPos)); } } detectedPass = detectingPass; 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 bc3fd9f..cfabc00 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java @@ -1,6 +1,7 @@ package org.timecrafters.UltimateGoal.Competition; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; @@ -15,20 +16,24 @@ public class ProgressRingBelt extends CyberarmState { this.robot = robot; } + private void prep(){ + robot.ringBeltMotor.setTargetPosition(targetPos); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.ringBeltMotor.setPower(0.7); + robot.ringBeltStage += 1; + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); + } + @Override public void start() { int currentPos = robot.ringBeltMotor.getCurrentPosition(); if (robot.ringBeltStage < 2) { targetPos = currentPos + Robot.RING_BELT_GAP; - robot.ringBeltOn(); - robot.ringBeltStage += 1; - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); + prep(); } else if (robot.ringBeltStage == 2) { - targetPos = currentPos + 160; - robot.ringBeltOn(); - robot.ringBeltStage += 1; + targetPos = currentPos + 240; + prep(); prepLaunch = !robot.initLauncher; - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); } else if (robot.ringBeltStage > 2) { setHasFinished(true); } @@ -41,8 +46,6 @@ public class ProgressRingBelt extends CyberarmState { int currentPos = robot.ringBeltMotor.getCurrentPosition(); if (currentPos >= targetPos) { - robot.ringBeltMotor.setPower(0); - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN); 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 3daa955..69900a6 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -63,11 +63,16 @@ public class Robot { public DcMotor encoderBack; //Steering Constants - static final double FINE_CORRECTION = 0.055 ; - static final double LARGE_CORRECTION = 0.025; + static final double 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; static final double MOMENTUM_CORRECTION = 1.05; static final double MOMENTUM_MAX_CORRECTION = 1.4; static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION)); + 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)); //Conversion Constants static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622; @@ -89,9 +94,9 @@ public class Robot { static final float CAMERA_DISPLACEMENT_DIRECTION = (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT); //Robot Localization - public double locationX; - public double locationY; - private float rotation; + private static double locationX; + private static double locationY; + private static float rotation; private int encoderLeftPrevious = 0; private int encoderBackPrevious = 0; @@ -108,6 +113,7 @@ public class Robot { public double launchPositionX; 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; @@ -125,7 +131,7 @@ public class Robot { public RevTouchSensor limitSwitch; public int ringBeltStage; public static final int RING_BELT_LOOP_TICKS = 2544; - public static final int RING_BELT_GAP = 670; + public static final int RING_BELT_GAP = 700; public static final double RING_BELT_POWER = 0.2; private int ringBeltPrev; public long beltMaxStopTime; @@ -253,10 +259,6 @@ public class Robot { webCamServo = hardwareMap.servo.get("look"); webCamServo.setDirection(Servo.Direction.REVERSE ); - rotation = stateConfiguration.variable("system", "startPos", "direction").value(); - locationX = inchesToTicks((double) stateConfiguration.variable("system", "startPos", "x").value()); - locationY = inchesToTicks((double) stateConfiguration.variable("system", "startPos", "y").value()); - minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value(); minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "minCheckMS").value(); @@ -274,6 +276,7 @@ public class Robot { launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); initLauncher = stateConfiguration.action("system","initLauncher").enabled; + reduceLaunchPos = stateConfiguration.variable("system", "launchPos", "reducePower").value(); if (initLauncher) { double launcherPower = 0; @@ -399,7 +402,7 @@ public class Robot { sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways); double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector)); - double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector); + double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector); double xChange = magnitude * (Math.sin(direction)); double yChange = magnitude * (Math.cos(direction)); @@ -407,31 +410,31 @@ public class Robot { locationX += xChange; locationY += yChange; - rotation += rotationChange; + Robot.rotation += rotationChange; - totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange); +// 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; +// } - if (totalV < minCheckVelocity) { - long timeCurrent = System.currentTimeMillis(); - - if (timeStartZeroVelocity == 0) { - timeStartZeroVelocity = timeCurrent; - } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs) { - syncWithVuforia(); - } - - } else { - timeStartZeroVelocity = 0; + if (Robot.rotation > 180) { + Robot.rotation -= 360; } - - - if (rotation > 180) { - rotation -= 360; - } - if (rotation < -180) { - rotation += 360; + if (Robot.rotation < -180) { + Robot.rotation += 360; } } @@ -452,18 +455,18 @@ 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); - this.rotation = 90-rotation.thirdAngle; + Robot.rotation = 90-rotation.thirdAngle; - if (this.rotation > 180) { - this.rotation -= -180; + if (Robot.rotation > 180) { + Robot.rotation -= -180; } VectorF translation = lastConfirmendLocation.getTranslation(); double camX = -translation.get(1) / mmPerInch; double camY = translation.get(0) / mmPerInch; - double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(this.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION); - double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(this.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION); + 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); locationX = inchesToTicks(camX - displaceX); locationY = inchesToTicks(camY - displaceY); @@ -474,26 +477,28 @@ public class Robot { } public float getRotation() { - return rotation; + return Robot.rotation; } public double getLocationX() { - return locationX; + return Robot.locationX; } public double getLocationY() { - return locationY; + return Robot.locationY; } - public void resetRotation(float rotation) { - this.rotation = rotation; + public void setLocalization(float rotation, double x, double y) { + Robot.rotation = rotation; + Robot.locationX = x; + Robot.locationY = y; } //Manually set the position of the robot on the field. public void setCurrentPosition(float rotation, double x, double y) { - this.rotation = rotation; - locationX = x; - locationY = y; + Robot.rotation = rotation; + Robot.locationX = x; + Robot.locationY = y; } //returns the angle from the robot's current position to the given target position. @@ -560,10 +565,10 @@ public class Robot { //calculating correction needed to steer the robot towards the degreesDirectionFace float relativeRotation = - getRelativeAngle(degreesDirectionFace, rotation); + getRelativeAngle(degreesDirectionFace, Robot.rotation); double turnCorrection = - Math.pow(LARGE_CORRECTION * relativeRotation, 3) + - FINE_CORRECTION * relativeRotation; + Math.pow(CUBIC_CORRECTION * relativeRotation, 3) + + LINEAR_CORRECTION * relativeRotation; double powerForwardRight = scalar * (q + turnCorrection); double powerForwardLeft = scalar * (p - turnCorrection); @@ -604,12 +609,18 @@ public class Robot { //Outputs the power necessary to turn and face a provided direction public double[] getFacePowers(float direction, double power) { angularVelocity = imu.getAngularVelocity().xRotationRate; - double relativeAngle = getRelativeAngle(direction, rotation); - double scaler = Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle; + double relativeAngle = getRelativeAngle(direction, Robot.rotation); + double scaler = Math.pow(CUBIC_CORRECTION * relativeAngle, 3) + FACE_LINEAR_CORRECTION * relativeAngle; + + if (relativeAngle > 0.5) { + scaler += FACE_MIN_CORRECTION; + } else if (relativeAngle < -0.5) { + scaler -= FACE_MIN_CORRECTION; + } if (relativeAngle != 0) { double momentumRelative = angularVelocity * (relativeAngle / Math.abs(relativeAngle)); - double exponential = Math.pow(MOMENTUM_CORRECTION, 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; 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 349e626..0873d29 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 @@ -1,12 +1,10 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import org.cyberarm.engine.V2.CyberarmState; import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; -import org.timecrafters.UltimateGoal.Competition.Launch; -import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; import org.timecrafters.UltimateGoal.Competition.Robot; public class Player1 extends CyberarmState { @@ -42,6 +40,11 @@ public class Player1 extends CyberarmState { private double launchPower; private long launchBrakeTime; + private float launchAngleGoal; + private float launchAnglePower1; + private float launchAnglePower2; + private float launchAnglePower3; + public Player1(Robot robot) { this.robot = robot; } @@ -52,9 +55,19 @@ public class Player1 extends CyberarmState { pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value(); faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value(); - launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value()); - launchPower = robot.stateConfiguration.variable("auto","04_0","power").value(); - launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value(); + 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(); + } + + @Override + public void start() { + faceDirection = robot.getRotation(); } @Override @@ -115,6 +128,14 @@ public class Player1 extends CyberarmState { 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 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; + } + //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) { @@ -123,15 +144,19 @@ public class Player1 extends CyberarmState { } rightJoystickMagnitudePrevious = rightJoystickMagnitude; - //allows the the driver to indicate which direction the robot is currently looking so - //so that the controller and robot can be re-synced in the event of a bad initial - //position. - if (engine.gamepad1.right_stick_button) { - robot.resetRotation(faceDirection); + //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; + } else if (engine.gamepad1.dpad_left) { + faceDirection = launchAnglePower3; } if (leftJoystickMagnitude == 0) { - double[] facePowers = robot.getFacePowers(faceDirection, drivePower); + 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 @@ -143,10 +168,29 @@ public class Player1 extends CyberarmState { robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]); } + + double ringBeltPower = robot.ringBeltMotor.getPower(); + if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - robot.ringBeltMotor.getCurrentPosition()) > 10) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE ); + } else if (ringBeltPower < 0) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED); + } else { + if (drivePower == 1) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN); + } else { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GOLD); + } + } } @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()) { 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 67cd1af..57a7262 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 @@ -19,6 +19,7 @@ public class Player2 extends CyberarmState { private boolean wobbleGrabOpen = false; private boolean aPrev; private double beltPowerPrev; + private DcMotor.RunMode runModePrev; private boolean lbPrev; private boolean manualArmHold; @@ -45,7 +46,13 @@ public class Player2 extends CyberarmState { //Collector control if (childrenHaveFinished()) { - robot.collectionMotor.setPower(engine.gamepad2.right_trigger); + double rt = engine.gamepad2.right_trigger; + double lt = engine.gamepad2.left_trigger; + if (rt >= lt) { + robot.collectionMotor.setPower(rt); + } else { + robot.collectionMotor.setPower(-lt); + } } else { robot.collectionMotor.setPower(0); } @@ -117,20 +124,27 @@ public class Player2 extends CyberarmState { //allows the driver to revers the belt in the event of a jam boolean lb = engine.gamepad2.left_bumper; if (lb && !lbPrev) { + runModePrev = robot.ringBeltMotor.getMode(); beltPowerPrev = robot.ringBeltMotor.getPower(); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER); } if (!lb && lbPrev) { robot.ringBeltMotor.setPower(beltPowerPrev); + robot.ringBeltMotor.setMode(runModePrev); } lbPrev = lb; - } @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.addData("Player 2 children", childrenHaveFinished()); for (CyberarmState state : children) { if (!state.getHasFinished()) { 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 0f3187c..8438a7d 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,7 @@ package org.timecrafters.UltimateGoal.Competition; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; @@ -9,6 +10,7 @@ public class UnstickRingBelt extends CyberarmState { private Robot robot; private int targetPos; private double lastPower; + private DcMotor.RunMode lastRunMode; public UnstickRingBelt(Robot robot) { this.robot = robot; @@ -17,10 +19,10 @@ public class UnstickRingBelt extends CyberarmState { @Override public void start() { lastPower = robot.ringBeltMotor.getPower(); + lastRunMode = robot.ringBeltMotor.getMode(); int currentPos = robot.ringBeltMotor.getCurrentPosition(); targetPos = currentPos - robot.beltReverseTicks; robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER); - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED); } @Override