a lot of stuff happened

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java

@@ -33,6 +33,8 @@ public class TeleopV4 extends LinearOpMode {
     public static Pose relocalizePose = new Pose(56, 11, 0);
     public static Pose teleStart = new Pose(0,0,0);
 
+    private boolean firstTickFull = true;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -77,6 +79,25 @@ public class TeleopV4 extends LinearOpMode {
         TELE.addData("TELE START", teleStart);
         TELE.update();
 
+        while (opModeInInit()){
+            if (gamepad1.triangleWasPressed()){
+                VelocityCommander.lockFront = true;
+                VelocityCommander.lockBack = false;
+            } else if (gamepad1.squareWasPressed()){
+                VelocityCommander.lockBack = true;
+                VelocityCommander.lockFront = false;
+            } else if (gamepad1.circleWasPressed()){
+                VelocityCommander.lockBack = false;
+                VelocityCommander.lockFront = false;
+            }
+            TELE.addLine("Initialization is done");
+            TELE.addData("Starting Position", follower.getPose());
+            TELE.addData("TELE START", teleStart);
+            TELE.addData("Front?:", VelocityCommander.lockFront);
+            TELE.addData("Back?:", VelocityCommander.lockBack);
+            TELE.update();
+        }
+
         waitForStart();
 
         if (isStopRequested()) return;
@@ -86,7 +107,8 @@ public class TeleopV4 extends LinearOpMode {
             drivetrain.drive(
                     -gamepad1.right_stick_y,
                     gamepad1.right_stick_x,
-                    gamepad1.left_stick_x
+                    gamepad1.left_stick_x,
+                    gamepad1.left_trigger
             );
 
             if (gamepad1.crossWasPressed()){
@@ -116,6 +138,23 @@ public class TeleopV4 extends LinearOpMode {
                 robot.setHoodPos(0.6);
             }
 
+            if (gamepad1.triangleWasPressed()){
+                VelocityCommander.lockFront = true;
+                VelocityCommander.lockBack = false;
+                TELE.addData("Front?:", VelocityCommander.lockFront);
+                TELE.addData("Back?:", VelocityCommander.lockBack);
+            } else if (gamepad1.squareWasPressed()){
+                VelocityCommander.lockBack = true;
+                VelocityCommander.lockFront = false;
+                TELE.addData("Front?:", VelocityCommander.lockFront);
+                TELE.addData("Back?:", VelocityCommander.lockBack);
+            } else if (gamepad1.circleWasPressed()){
+                VelocityCommander.lockBack = false;
+                VelocityCommander.lockFront = false;
+                TELE.addData("Front?:", VelocityCommander.lockFront);
+                TELE.addData("Back?:", VelocityCommander.lockBack);
+            }
+
             shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
 
@@ -130,6 +169,13 @@ public class TeleopV4 extends LinearOpMode {
                             state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
 
                 spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                SpindexerTransferIntake.intakeFull = false;
+                firstTickFull = true;
+            }
+
+            if (SpindexerTransferIntake.intakeFull && firstTickFull){
+                gamepad1.rumble(100);
+                firstTickFull = false;
             }
 
             if (gamepad1.right_trigger > 0.5 &&
@@ -159,20 +205,20 @@ public class TeleopV4 extends LinearOpMode {
             }
 
             loopTimes.loop();
-            TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
+//            TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
-            TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
+//            TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
-            TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
+//            TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
-
+//
             TELE.addData("Distance From Goal", commander.getDistance());
-            TELE.addData("Hood Position", commander.getHoodPredicted());
+//            TELE.addData("Hood Position", commander.getHoodPredicted());
-            TELE.addData("Transfer Power", robot.transfer.getPower());
+//            TELE.addData("Transfer Power", robot.transfer.getPower());
             TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
             TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
-
+//
-            TELE.addData("Current Position", currentPose);
+//            TELE.addData("Current Position", currentPose);
-
+//
-            TELE.addData("Current LL Pipeline", turret.pipeline());
+//            TELE.addData("Current LL Pipeline", turret.pipeline());
-
+//
             TELE.update();
         }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java

@@ -81,17 +81,20 @@ public class NewShooterTest extends LinearOpMode {
             drivetrain.drive(
                     -gamepad1.right_stick_y,
                     gamepad1.right_stick_x,
-                    gamepad1.left_stick_x
+                    gamepad1.left_stick_x,
+                    gamepad1.left_trigger
             );
 
-            if (Color.redAlliance){
+            if (gamepad1.crossWasPressed()){
-                TeleopV4.relocalizePose = new Pose(57.5, 5, 0);
+                if (Color.redAlliance){
-            } else {
+                    TeleopV4.relocalizePose = new Pose(57.5, 5, 0);
-                TeleopV4.relocalizePose = new Pose(-57.5, 5, Math.toRadians(180));
+                } else {
+                    TeleopV4.relocalizePose = new Pose(-57.5, 5, Math.toRadians(180));
+                }
+                follower.setPose(TeleopV4.relocalizePose);
+                sleep(500);
+                gamepad1.rumble(100);
             }
-            follower.setPose(TeleopV4.relocalizePose);
-            sleep(500);
-            gamepad1.rumble(100);
 
             follower.update();
 
@@ -154,6 +157,7 @@ public class NewShooterTest extends LinearOpMode {
             TELE.addData("Hood Position", commander.getHoodPredicted());
             TELE.addData("Transfer Power", commander.getTransferPow());
             TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
+            TELE.addData("Manuel Velocity RPM", flywheelVelo);
             TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
             TELE.addData("TX:", turret.getTX());
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java

@@ -91,7 +91,8 @@ public class SortedSpindexerTest extends LinearOpMode {
             drivetrain.drive(
                     -gamepad1.right_stick_y,
                     gamepad1.right_stick_x,
-                    gamepad1.left_stick_x
+                    gamepad1.left_stick_x,
+                    gamepad1.left_trigger
             );
 
             follower.update();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java

@@ -37,7 +37,7 @@ public class Drivetrain {
         tele = input;
     }
 
-    public void drive(double y, double x, double rx) {
+    public void drive(double y, double x, double rx, double stop) {
 
         boolean snappedForward = false;
         boolean snappedStrafe = false;
@@ -79,18 +79,30 @@ public class Drivetrain {
         robot.setFrontRightPower(frontRightPower);
         robot.setBackRightPower(backRightPower);
 
-        if (tele) {
+        if (stop > 0.5){
-
+            robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            telemetry.addData("Forward Snap", snappedForward);
+            robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            telemetry.addData("Strafe Snap", snappedStrafe);
+            robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
+            robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            telemetry.addData("Correction RX", correctionRX);
-
-            telemetry.addData("FL", frontLeftPower);
-            telemetry.addData("BL", backLeftPower);
-            telemetry.addData("FR", frontRightPower);
-            telemetry.addData("BR", backRightPower);
 
+            robot.setFrontLeftPower(0);
+            robot.setBackLeftPower(0);
+            robot.setFrontRightPower(0);
+            robot.setBackRightPower(0);
         }
+
+//        if (tele) {
+//
+//            telemetry.addData("Forward Snap", snappedForward);
+//            telemetry.addData("Strafe Snap", snappedStrafe);
+//
+//            telemetry.addData("Correction RX", correctionRX);
+//
+//            telemetry.addData("FL", frontLeftPower);
+//            telemetry.addData("BL", backLeftPower);
+//            telemetry.addData("FR", frontRightPower);
+//            telemetry.addData("BR", backRightPower);
+//
+//        }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java

@@ -1,6 +1,8 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
 import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PIDFController;
+import com.arcrobotics.ftclib.controller.wpilibcontroller.SimpleMotorFeedforward;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -16,10 +18,20 @@ public class Flywheel {
 
 //    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
     public static PIDFCoefficients shooterPIDF;
+    PIDFController pidf;
+    SimpleMotorFeedforward feedforward;
+
+    public static double kS = 0.01; // Static feedforward
+    public static double kV = 0.0001935; // Velocity feedforward
+
     public static double shooterPIDF_P = 500;
     public static double shooterPIDF_I = 1;
     public static double shooterPIDF_D = 0.0;
     public static double shooterPIDF_F = 93;
+//    public static double shooterPIDF_P = 0.0001;
+//    public static double shooterPIDF_I = 0;
+//    public static double shooterPIDF_D = 0.00001;
+//    public static double shooterPIDF_F = 0;
 
     private double velo = 0.0;
     private double velo1 = 0.0;
@@ -36,6 +48,8 @@ public class Flywheel {
     public Flywheel(Robot rob) {
         robot = rob;
         shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12);
+//        pidf = new PIDFController(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0);
+//        feedforward = new SimpleMotorFeedforward(kS, kV);
     }
 
     public double getVelo() {
@@ -67,19 +81,23 @@ public class Flywheel {
         shooterPIDF.d = d;
         shooterPIDF.f = f;
 
+//        pidf.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0);
+
         robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
     }
 
     private double prevF = 0;
 
     public static double voltagePIDFDifference = 1;
+    double averageVoltage = 0;
     public void setF(double voltage){
+        averageVoltage = ALPHA * voltage + (1 - ALPHA) * averageVoltage;
         double f = shooterPIDF_F / voltage;
-        if (Math.abs(prevF - f) > voltagePIDFDifference) {
+        if (Math.abs(prevF - f) > voltagePIDFDifference && !steady) {
             shooterPIDF.f = f;
             robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
-            prevF = f;
         }
+        prevF = f;
     }
 
     // Convert from RPM to Ticks per Second
@@ -92,25 +110,30 @@ public class Flywheel {
         return (TPS * 60.0) / 28.0;
     }
 
+    double ALPHA = 0.3;
     private void updateVelocityAverage(double newVelocity) {
 
-        velocityHistory.add(newVelocity);
+//        velocityHistory.add(newVelocity);
+//
+//        int velocityHistorySize = 5;
+//        if (velocityHistory.size() > velocityHistorySize) {
+//            velocityHistory.removeFirst();
+//        }
+//
+//        double sum = 0.0;
+//
+//        for (double v : velocityHistory) {
+//            sum += v;
+//        }
+//
+//        averageVelocity = sum / velocityHistory.size();
 
-        int velocityHistorySize = 5;
+        averageVelocity = ALPHA * newVelocity + (1 - ALPHA) * averageVelocity;
-        if (velocityHistory.size() > velocityHistorySize) {
-            velocityHistory.removeFirst();
-        }
-
-        double sum = 0.0;
-
-        for (double v : velocityHistory) {
-            sum += v;
-        }
-
-        averageVelocity = sum / velocityHistory.size();
     }
 
     double power;
+    double prevTargetTime = 0;
+    double prevTargetVelocity = 0;
     public void manageFlywheel(double commandedVelocity) {
 
         if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
@@ -125,13 +148,12 @@ public class Flywheel {
 
         velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
 
-        velo = (velo1 + velo2) / 2.0;
+        velo = velo1;
 
         updateVelocityAverage(velo);
 
         steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
     }
 
-
     public double getShooterPower(){return power;}
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java

@@ -104,7 +104,8 @@ public class Shooter {
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent(),
-                        voltage
+                        voltage,
+                        fly.getAverageVelocity()
                 );
 
                 fly.manageFlywheel(flywheelVelocity);
@@ -131,7 +132,8 @@ public class Shooter {
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent(),
-                        voltage
+                        voltage,
+                        fly.getAverageVelocity()
                 );
 
                 flywheelVelocity = commander.getPredictedRPM();
@@ -153,7 +155,8 @@ public class Shooter {
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent(),
-                        voltage
+                        voltage,
+                        fly.getAverageVelocity()
                 );
 
                 flywheelVelocity = commander.getPredictedRPM();
@@ -172,7 +175,8 @@ public class Shooter {
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent(),
-                        voltage
+                        voltage,
+                        fly.getAverageVelocity()
                 );
 
                 flywheelVelocity = commander.getPredictedRPM();
@@ -200,7 +204,8 @@ public class Shooter {
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent(),
-                        voltage
+                        voltage,
+                        fly.getAverageVelocity()
                 );
                 fly.manageFlywheel(flywheelVelocity);
                 fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java

@@ -274,22 +274,23 @@ public class SpindexerTransferIntake {
     private int greenTicks = 0;
     private int ballTicks = 0;
     private int holdBallsTicker = 0;
+    public static boolean intakeFull = true;
     public void update() {
 
-        TELE.addData("Sorted State", sortedIntakeStates);
+//        TELE.addData("Sorted State", sortedIntakeStates);
-        TELE.addData("Ball0", ballColors[0]);
+//        TELE.addData("Ball0", ballColors[0]);
-        TELE.addData("Ball1", ballColors[1]);
+//        TELE.addData("Ball1", ballColors[1]);
-        TELE.addData("Ball2", ballColors[2]);
+//        TELE.addData("Ball2", ballColors[2]);
-
+//
-        TELE.addData("Shoot0", shootOrder[0]);
+//        TELE.addData("Shoot0", shootOrder[0]);
-        TELE.addData("Shoot1", shootOrder[1]);
+//        TELE.addData("Shoot1", shootOrder[1]);
-        TELE.addData("Shoot2", shootOrder[2]);
+//        TELE.addData("Shoot2", shootOrder[2]);
-
+//
-        TELE.addData("Color0", ballColors[0]);
+//        TELE.addData("Color0", ballColors[0]);
-        TELE.addData("Color1", ballColors[1]);
+//        TELE.addData("Color1", ballColors[1]);
-        TELE.addData("Color2", ballColors[2]);
+//        TELE.addData("Color2", ballColors[2]);
-
+//
-        TELE.addData("Shoot State", shootState);
+//        TELE.addData("Shoot State", shootState);
         
         switch (mode) {
 
@@ -317,9 +318,13 @@ public class SpindexerTransferIntake {
 
                         if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
 
-                            setRapidMode(RapidMode.TRANSFER_OFF);
+                            holdBallsTicker++;
                         }
 
+                        if (holdBallsTicker > 10){
+                            setRapidMode(RapidMode.TRANSFER_OFF);
+                            holdBallsTicker = 0;
+                        }
 
                         break;
 
@@ -366,10 +371,12 @@ public class SpindexerTransferIntake {
                     case HOLD_BALLS:
 
                         if (robot.insideBeam.isPressed()
-                                && robot.outsideBeam.isPressed() && holdBallsTicker > 10) {
+                                && robot.outsideBeam.isPressed()) {
 
-                            robot.setIntakePower(0.1);
                             robot.setTransferPower(0);
+                            robot.setIntakePower(0.1);
+                            robot.setTransferServoPos(ServoPositions.transferServo_in);
+                            intakeFull = true;
 
                         } else {
                             holdBallsTicker++;
@@ -379,37 +386,42 @@ public class SpindexerTransferIntake {
 
                     case OPEN_GATE:
 
-                        robot.setRapidFireBlockerPos(
-                                ServoPositions.rapidFireBlocker_Open
-                        );
-
                         if (stateTime() >= 100) {
                             setRapidMode(RapidMode.SHOOT);
                         }
 
+                        robot.setTransferServoPos(ServoPositions.transferServo_in);
+
+                        robot.setRapidFireBlockerPos(
+                                ServoPositions.rapidFireBlocker_Open
+                        );
+
                         if (Shooter.manualFlywheel) {
                             robot.setTransferPower(NewShooterTest.transferPower);
+                            robot.setIntakePower(-NewShooterTest.transferPower);
                         } else {
                             robot.setTransferPower(commander.getTransferPow());
+                            robot.setIntakePower(-commander.getTransferPow());
                         }
 
                         break;
 
                     case SHOOT:
 
-                        robot.setTransferServoPos(
+                        if (stateTime() >= 500) {
-                                ServoPositions.transferServo_in
-                        );
-                        if (stateTime() >= 400) {
                             setRapidMode(RapidMode.INTAKE);
                         }
 
                         if (Shooter.manualFlywheel) {
                             robot.setTransferPower(NewShooterTest.transferPower);
+                            robot.setIntakePower(-NewShooterTest.transferPower);
                         } else {
                             robot.setTransferPower(commander.getTransferPow());
+                            robot.setIntakePower(-commander.getTransferPow());
                         }
 
+                        holdBallsTicker = 0;
+
                         break;
                 }
                 break;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java

@@ -20,7 +20,7 @@ public class Turret {
     private final double turretMin = 0.05;
     private final double turretMax = 0.95;
     public static boolean limelightUsed = true;
-    public static double B_PID_P = 0.00015, B_PID_I = 0.0, B_PID_D = 0.00001;
+    public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005;
     LLResult result;
     PIDController bearingPID;
     boolean bearingAligned = false;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java

@@ -8,88 +8,71 @@ public class VelocityCommander {
     public static double xAccK = 0.025; // TODO: Tune
     public static double yVelK = 0.05; // TODO: Tune
     public static double yAccK = 0.025; // TODO: Tune
+    public static boolean lockFront = false;
+    public static boolean lockBack = false;
+    public static int farBound = 140;
+    public static int closeBound = 110;
+    public static double errorHoodAdjustment = 0.0005;
     private double hoodPos = 0.88;
     private double transferPow = -1;
     private int velo = 0;
 
     public VelocityCommander() {}
 
-    private final double veloA = -2.703087757*Math.pow(10, -14);
+    final double veloA = -0.00000133612;
-    private final double veloB = 2.904756341*Math.pow(10, -11);
+    final double veloB = 0.000542733;
-    private final double veloC = -1.381814293*Math.pow(10, -8);
+    final double veloC = -0.0739531;
-    private final double veloD = 0.000003829224585;
+    final double veloD = 5.16759;
-    private final double veloE = -0.000684090204;
+    final double veloE = 62.45781;
-    private final double veloF = 0.0822754689;
-    private final double veloG = -6.743119277;
-    private final double veloH = 371.7359504;
-    private final double veloI = -13189.70958;
-    private final double veloJ = 272005.7124;
-    private final double veloK = -2474581.713;
     private double distToRPM (double dist){
-        double currentVelo = 0;
+        double currentVelo;
-        if (dist < 49) {
+        if (lockFront && dist > closeBound){
-            currentVelo = 2000;
+            dist = closeBound;
-        } else if (dist > 165){
+        } else if (lockBack && dist < farBound){
-            velo = 3760;
+            dist = farBound;
-        } else {
+        }
-            currentVelo = veloA*Math.pow(dist, 10) +
+        if (dist < 54) {
-                    veloB*Math.pow(dist, 9) +
+            velo = 2000;
-                    veloC*Math.pow(dist, 8) +
+        } else if (dist > 181){
-                    veloD*Math.pow(dist, 7) +
+            velo = 3600;
-                    veloE*Math.pow(dist, 6) +
+        } else {
-                    veloF*Math.pow(dist, 5) +
+            currentVelo = veloA*Math.pow(dist, 4) +
-                    veloG*Math.pow(dist, 4) +
+                    veloB*Math.pow(dist, 3) +
-                    veloH*Math.pow(dist, 3) +
+                    veloC*Math.pow(dist, 2) +
-                    veloI*Math.pow(dist, 2) +
+                    veloD*Math.pow(dist, 1) +
-                    veloJ*Math.pow(dist, 1) +
+                    veloE;
-                    veloK;
+            velo = 10 * Math.round((float) Math.max(200, Math.min(360, currentVelo)));
         }
-        velo = Math.round((float) Math.max(2000, Math.min(3760, currentVelo)));
         return velo;
     }
 
-    private final double hoodA = -4.3276177*Math.pow(10, -13);
+    final double hoodA = 9.04203*Math.pow(10, -8);
-    private final double hoodB = 2.68062979*Math.pow(10, -10);
+    final double hoodB = -0.0000204165;
-    private final double hoodC = -7.12859632*Math.pow(10, -8);
+    final double hoodC = -0.00252089;
-    private final double hoodD = 0.0000106010785;
+    final double hoodD = 1.06154;
-    private final double hoodE = -0.000960693973;
-    private final double hoodF = 0.0540375808;
-    private final double hoodG = -1.82724027;
-    private final double hoodH = 33.4797545;
-    private final double hoodI = -246.888632;
     private void distToHood (double dist){
-        if (dist > 112){
+        if (dist > 174){
-            hoodPos = 0.35;
+            hoodPos = 0.48;
-        } else if (dist < 49){
+        } else if (dist < 54){
             hoodPos = 0.88;
         } else {
-            hoodPos = hoodA*Math.pow(dist, 8) +
+            hoodPos = hoodA*Math.pow(dist, 3) +
-                    hoodB*Math.pow(dist, 7) +
+                    hoodB*Math.pow(dist, 2) +
-                    hoodC*Math.pow(dist, 6) +
+                    hoodC*Math.pow(dist, 1) +
-                    hoodD*Math.pow(dist, 5) +
+                    hoodD;
-                    hoodE*Math.pow(dist, 4) +
-                    hoodF*Math.pow(dist, 3) +
-                    hoodG*Math.pow(dist, 2) +
-                    hoodH*Math.pow(dist, 1) +
-                    hoodI;
-
-            hoodPos = Math.max(0.35, Math.min(0.88, hoodPos));
         }
+        hoodPos = Math.max(0.48, Math.min(0.88, hoodPos));
     }
     public double getHoodPredicted(){
         return hoodPos;
     }
 
     private void distToTransferPow(double dist, double voltage){
-        if (dist < 118){
+        if (dist < 140){
-            transferPow = -1;
+            transferPow = -0.8;
-        } else if (dist < 125){
-            transferPow = -0.7;
         } else {
             transferPow = -0.5;
         }
-
-//        transferPow = Math.max(-0.5, Math.min(-1, transferPow * (14/voltage)));
     }
     public double getTransferPow(){return transferPow;}
 
@@ -99,7 +82,7 @@ public class VelocityCommander {
     }
 
     double predictedDist = 0;
-    public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage) {
+    public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage, double velocity) {
 
         double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
         double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
@@ -110,6 +93,17 @@ public class VelocityCommander {
         distToHood(predictedDist);
         distToTransferPow(predictedDist, voltage);
         distToRPM(predictedDist);
+
+        hoodPos += adjustHood(predictedDist, velocity, velo);
+    }
+
+    public double adjustHood(double dist, double currentVelocity, double targetVelocity){
+        double error = targetVelocity - currentVelocity;
+        if (dist < farBound || error < 0){
+            error = 0;
+        }
+        System.out.println("Error "+ error);
+        return error * errorHoodAdjustment;
     }
 
     public double getPredictedRPM(){return velo;}