diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 6b004c1..d1566e7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -33,7 +33,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93; - public static double spindexerSpeedIncrease = 0.008; + public static double spindexerSpeedIncrease = 0.014; // These values are ADDED to turrDefault public static double redObeliskTurrPos1 = 0.12; @@ -355,15 +355,6 @@ public class Auto_LT_Close extends LinearOpMode { void shoot(){ Actions.runBlocking( new ParallelAction( - autoActions.manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501, - 0.501, - false - ), autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) ) @@ -380,12 +371,8 @@ public class Auto_LT_Close extends LinearOpMode { flywheel0Time, x1, y1, - posXTolerance, - posYTolerance, - h1, - false + h1 ) - ) ); } @@ -394,16 +381,12 @@ public class Auto_LT_Close extends LinearOpMode { Actions.runBlocking( new ParallelAction( pickup1.build(), - autoActions.manageShooterAuto( + autoActions.intake( intake1Time, x2b, y2b, - posXTolerance, - posYTolerance, - h2b, - true + h2b ), - autoActions.intake(intake1Time), autoActions.detectObelisk( intake1Time, x2b, @@ -436,17 +419,15 @@ public class Auto_LT_Close extends LinearOpMode { Actions.runBlocking( new ParallelAction( - autoActions.manageShooterAuto( + shoot1.build(), + autoActions.prepareShootAll( + colorSenseTime, shoot1Time, + motif, posX, posY, - posXTolerance, - posYTolerance, - posH, - false - ), - shoot1.build(), - autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif) + posH + ) ) ); } @@ -455,16 +436,12 @@ public class Auto_LT_Close extends LinearOpMode { Actions.runBlocking( new ParallelAction( pickup2.build(), - autoActions.manageShooterAuto( + autoActions.intake( intake2Time, x3b, y3b, - posXTolerance, - posYTolerance, - h3b, - true + h3b ), - autoActions.intake(intake2Time), autoActions.detectObelisk( intake2Time, x3b, @@ -497,17 +474,14 @@ public class Auto_LT_Close extends LinearOpMode { Actions.runBlocking( new ParallelAction( - autoActions.manageShooterAuto( + shoot2.build(), + autoActions.prepareShootAll( + colorSenseTime, shoot2Time, + motif, posX, posY, - posXTolerance, - posYTolerance, - posH, - false - ), - shoot2.build(), - autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif) + posH) ) ); } @@ -516,16 +490,12 @@ public class Auto_LT_Close extends LinearOpMode { Actions.runBlocking( new ParallelAction( pickup3.build(), - autoActions.manageShooterAuto( + autoActions.intake( intake3Time, x4b, y4b, - posXTolerance, - posYTolerance, - h4b, - true + h4b ), - autoActions.intake(intake3Time), autoActions.detectObelisk( intake3Time, x4b, @@ -545,17 +515,15 @@ public class Auto_LT_Close extends LinearOpMode { Actions.runBlocking( new ParallelAction( - autoActions.manageShooterAuto( + shoot3.build(), + autoActions.prepareShootAll( + colorSenseTime, shoot3Time, + motif, xLeave, yLeave, - posXTolerance, - posYTolerance, - hLeave, - false - ), - shoot3.build(), - autoActions.prepareShootAll(colorSenseTime, shoot3Time, motif) + hLeave + ) ) ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 156f0b9..e0a2841 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -60,16 +60,16 @@ public class Auto_LT_Far extends LinearOpMode { public static int pickupStackSpeed = 12; public static int pickupGateSpeed = 25; int prevMotif = 0; - public static double spindexerSpeedIncrease = 0.008; + public static double spindexerSpeedIncrease = 0.014; public static double shootAllTime = 2; // ---- POSITION TOLERANCES ---- public static double posXTolerance = 5.0; public static double posYTolerance = 5.0; public static double shootStackTime = 2; - public static double shootGateTime = 2; + public static double shootGateTime = 2.5; public static double colorSenseTime = 1; - public static double intakeStackTime = 3; - public static double intakeGateTime = 2.5; + public static double intakeStackTime = 2.5; + public static double intakeGateTime = 2; public static double redObeliskTurrPos1 = 0.12; public static double redObeliskTurrPos2 = 0.13; public static double redObeliskTurrPos3 = 0.14; @@ -79,7 +79,7 @@ public class Auto_LT_Far extends LinearOpMode { double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; - public static double endAutoTime = 25; + public static double endAutoTime = 26; // initialize path variables here TrajectoryActionBuilder leave3Ball = null; @@ -276,7 +276,14 @@ public class Auto_LT_Far extends LinearOpMode { } while (gatePickup && getRuntime() - stamp < endAutoTime){ - cycleGatePickup(); + cycleGatePickupBalls(); + if (getRuntime() - stamp > endAutoTime){ + break; + } + cycleGatePrepareShoot(); + if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){ + break; + } shoot(); } @@ -307,15 +314,6 @@ public class Auto_LT_Far extends LinearOpMode { void shoot(){ Actions.runBlocking( new ParallelAction( - autoActions.manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501, - 0.501, - false - ), autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) ) @@ -329,10 +327,7 @@ public class Auto_LT_Far extends LinearOpMode { flywheel0Time, 0.501, 0.501, - 0.501, - 0.501, - 0.501, - false + 0.501 ) ) @@ -353,16 +348,12 @@ public class Auto_LT_Far extends LinearOpMode { Actions.runBlocking( new ParallelAction( pickup3.build(), - autoActions.manageShooterAuto( + autoActions.intake( intakeStackTime, xStackPickupB, yStackPickupB, - posXTolerance, - posYTolerance, - hStackPickupB, - true + hStackPickupB ), - autoActions.intake(intakeStackTime), autoActions.detectObelisk( intakeStackTime, xStackPickupB, @@ -382,35 +373,28 @@ public class Auto_LT_Far extends LinearOpMode { Actions.runBlocking( new ParallelAction( - autoActions.manageShooterAuto( + shoot3.build(), + autoActions.prepareShootAll( + colorSenseTime, shootStackTime, + motif, xShoot, yShoot, - posXTolerance, - posYTolerance, - hShoot, - false - ), - shoot3.build(), - autoActions.prepareShootAll(colorSenseTime, shootStackTime, motif) + hShoot) ) ); } - void cycleGatePickup(){ + void cycleGatePickupBalls(){ Actions.runBlocking( new ParallelAction( pickupGate.build(), - autoActions.manageShooterAuto( - intakeGateTime, + autoActions.intake( + intakeStackTime, pickupGateX, pickupGateY, - posXTolerance, - posYTolerance, - pickupGateH, - true + pickupGateH ), - autoActions.intake(intakeStackTime), autoActions.detectObelisk( intakeGateTime, pickupGateX, @@ -426,20 +410,20 @@ public class Auto_LT_Far extends LinearOpMode { if (motif == 0) motif = prevMotif; prevMotif = motif; + } + void cycleGatePrepareShoot(){ Actions.runBlocking( new ParallelAction( - autoActions.manageShooterAuto( + shootGate.build(), + autoActions.prepareShootAll( + colorSenseTime, shootGateTime, + motif, xShoot, yShoot, - posXTolerance, - posYTolerance, - hShoot, - false - ), - shootGate.build(), - autoActions.prepareShootAll(colorSenseTime, shootGateTime, motif) + hShoot + ) ) ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index be1ab39..7998cdc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -49,11 +49,12 @@ public class AutoActions{ private int passengerSlotGreen = 0; private int rearSlotGreen = 0; private int mostGreenSlot = 0; - private double firstSpindexShootPos = spinStartPos; + private double firstSpindexShootPos = spindexer_outtakeBall1; private boolean shootForward = true; public static double firstShootTime = 0.3; public int motif = 0; double spinEndPos = ServoPositions.spinEndPos; + int waitFirstBallTicks = 4; public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig){ this.robot = rob; @@ -68,7 +69,14 @@ public class AutoActions{ this.light = lig; } - public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { + public Action prepareShootAll( + double colorSenseTime, + double time, + int motif_id, + double posX, + double posY, + double posH + ) { return new Action() { double stamp = 0.0; int ticker = 0; @@ -107,10 +115,12 @@ public class AutoActions{ spinEndPos = spindexer_outtakeBall2 + 0.1; } + Action manageShooter = null; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { stamp = System.currentTimeMillis(); + manageShooter = manageShooterAuto(time, posX, posY, posH); } ticker++; servos.setTransferPos(transferServo_out); @@ -120,6 +130,8 @@ public class AutoActions{ teleStart = drive.localizer.getPose(); + manageShooter.run(telemetryPacket); + if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) { spindexerWiggle *= -1.0; @@ -199,22 +211,16 @@ public class AutoActions{ }; } - private boolean doneShooting = false; public Action shootAllAuto(double shootTime, double spindexSpeed) { return new Action() { int ticker = 1; double stamp = 0.0; - double velo = 0.0; - int shooterTicker = 0; - + Action manageShooter = null; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - velo = flywheel.getVelo(); - drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -226,9 +232,13 @@ public class AutoActions{ if (ticker == 1) { stamp = System.currentTimeMillis(); + manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501); + } ticker++; + manageShooter.run(telemetryPacket); + double prevSpinPos = servos.getSpinCmdPos(); boolean end; @@ -238,18 +248,18 @@ public class AutoActions{ end = prevSpinPos < spinEndPos; } - if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < 2)) { + if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < waitFirstBallTicks+1)) { - if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 3) { + if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) { servos.setTransferPos(transferServo_out); servos.setSpinPos(firstSpindexShootPos); } else { servos.setTransferPos(transferServo_in); shooterTicker++; - if (shootForward) { + if (shootForward && shooterTicker > waitFirstBallTicks) { servos.setSpinPos(prevSpinPos + spindexSpeed); - } else { + } else if (shooterTicker > waitFirstBallTicks){ servos.setSpinPos(prevSpinPos - spindexSpeed); } @@ -262,7 +272,6 @@ public class AutoActions{ spindexer.resetSpindexer(); spindexer.processIntake(); - doneShooting = true; return false; @@ -271,15 +280,21 @@ public class AutoActions{ }; } - public Action intake(double intakeTime) { + public Action intake( + double time, + double posX, + double posY, + double posH + ) { return new Action() { double stamp = 0.0; int ticker = 0; - + Action manageShooter = null; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { stamp = System.currentTimeMillis(); + manageShooter = manageShooterAuto(time, posX, posY, posH); } ticker++; @@ -292,7 +307,9 @@ public class AutoActions{ teleStart = drive.localizer.getPose(); - return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); + manageShooter.run(telemetryPacket); + + return ((System.currentTimeMillis() - stamp) < (time * 1000)) && !spindexer.isFull(); } }; } @@ -360,22 +377,15 @@ public class AutoActions{ double time, double posX, double posY, - double posXTolerance, - double posYTolerance, - double posH, - boolean whileIntaking + double posH ) { return new Action() { double stamp = 0.0; int ticker = 0; - int shootingTicker = 0; - double shootingStamp = 0; final boolean timeFallback = (time != 0.501); - final boolean posXFallback = (posX != 0.501); - final boolean posYFallback = (posY != 0.501); @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { @@ -418,31 +428,18 @@ public class AutoActions{ servos.setHoodPos(targetingSettings.hoodAngle); double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(targetingSettings.flywheelRPM); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance; - boolean shouldFinish; - if (whileIntaking) { - shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull(); - } else { - shouldFinish = timeDone || (xDone && yDone) || doneShooting; - } + boolean shouldFinish = timeDone || flywheel.getSteady(); teleStart = currentPose; TELE.addData("Steady?", flywheel.getSteady()); TELE.update(); - if (shouldFinish) { - doneShooting = false; - return false; - } else { - return true; - } - + return !shouldFinish; } }; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 643a917..1b48cdb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -14,10 +14,10 @@ public class Back_Poses { public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140; public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; - public static double rStackPickupBX = 55, rStackPickupBY = 83, rStackPickupBH = 140.1; + public static double rStackPickupBX = 55, rStackPickupBY = 73, rStackPickupBH = 140.1; public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1; - public static double rPickupGateX = 65, rPickupGateY = 80, rPickupGateH = 140; + public static double rPickupGateX = 70, rPickupGateY = 90, rPickupGateH = 140; public static double bPickupGateX = 60, bPickupGateY = -88, bPickupGateH = -140; public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index 63f7f6e..ebffe23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -39,8 +39,8 @@ public class Front_Poses { public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; public static double bxPrep = 45, byPrep = -10, bhPrep = -50; - public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50; - public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; + public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55; + public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55; public static Pose2d teleStart = new Pose2d(0, 0, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 1ceff64..1b422ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -16,7 +16,7 @@ public class ServoPositions { public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6; public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4; - public static double spinStartPos = 0.22; + public static double spinStartPos = 0.3; public static double spinEndPos = 0.85; public static double shootAllSpindexerSpeedIncrease = 0.014; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index cdc8d6a..59779be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -210,7 +210,7 @@ public class TeleopV3 extends LinearOpMode { //SHOOTER: double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); //HOOD: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index dcb2d73..3769e09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -37,7 +37,7 @@ public class ShooterTest extends LinearOpMode { public static double P = 255.0; public static double I = 0.0; public static double D = 0.0; - public static double F = 90; + public static double F = 75; public static double transferPower = 1.0; public static double hoodPos = 0.501; public static double turretPos = 0.501; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index ddb598b..0af4300 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -68,17 +68,15 @@ public class Flywheel { // Add code here to set PIDF based on desired RPM //robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); //robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); - robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); - robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); - - // Record Current Velocity - velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); - velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); - velo = Math.max(velo1, velo2); } + robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); + robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); + velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); + velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); + velo = Math.max(velo1, velo2); // really should be a running average of the last 5 - steady = (Math.abs(commandedVelocity - velo) < 200.0); + steady = (Math.abs(commandedVelocity - velo) < 50); return powPID; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 4ce0461..361d03f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -31,10 +31,10 @@ public class Robot { public DcMotorEx intake; public DcMotorEx transfer; public PIDFCoefficients shooterPIDF; - public double shooterPIDF_P = 255.0; - public double shooterPIDF_I = 0.0; - public double shooterPIDF_D = 0.0; - public double shooterPIDF_F = 90; + public static double shooterPIDF_P = 255; + public static double shooterPIDF_I = 0.0; + public static double shooterPIDF_D = 0.0; + public static double shooterPIDF_F = 75; public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public DcMotorEx shooter1; public DcMotorEx shooter2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index fa44604..909ecd6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -27,7 +27,7 @@ public class Turret { public static double turret180Range = 0.4; public static double turrDefault = 0.37; public static double turrMin = 0.15; - public static double turrMax = 0.85; + public static double turrMax = 0.8; public static boolean limelightUsed = true; public static double manualOffset = 0.0; @@ -40,7 +40,7 @@ public class Turret { public static double clampTolerance = 0.03; //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; - public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007; + public static double B_PID_P = 0.08, B_PID_I = 0.0, B_PID_D = 0.007; Robot robot; MultipleTelemetry TELE; Limelight3A webcam;