Made a seperate auto called TwentyBlueRightTest which includes all th…

…e robot movements to move the robot from the beginning to the front of their respective backdrop april tags. Miles needs to add the pixel dropper code and vision code in here.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/DriveForwardPID.java

@@ -14,6 +14,13 @@ public class DriveForwardPID {
     public static final double MIN_SPEED_INITIAL = 0.3; // was 0.25
     public static final double MIN_SPEED_FINAL = 0.25; // was 0.15
     public static final double acceptableError = .50; // in
+
+    public static final double MAX_SPEED_D = 0.3; // drive forward/reverse
+    public static final double RAMPS_UP_D = 6; // drive forward/reverse
+    public static final double RAMPS_DOWN_D = 8; // drive forward/reverse
+    public static final double MIN_SPEED_INITIAL_D = 0.25; // drive forward/reverse
+    public static final double MIN_SPEED_FINAL_D = 0.15; // drive forward/reverse
+    public static final double acceptableError_D = 1.0; // drive forward/reverse
     private final DcMotor paraRight;
     private final DcMotor paraLeft;
     private final DcMotor perp;
@@ -25,12 +32,22 @@ public static double rampDown(double distToTarget) {
         else return (MAX_SPEED - MIN_SPEED_FINAL) * (distToTarget / RAMPS_DOWN) + MIN_SPEED_FINAL;
     }
 
+    public static double rampDownForward(double distToTarget) {
+        if (distToTarget <= 0) return 0.0;
+        if (distToTarget >= RAMPS_DOWN_D) return MAX_SPEED_D;
+        else return (MAX_SPEED_D - MIN_SPEED_FINAL_D) * (distToTarget / RAMPS_DOWN_D) + MIN_SPEED_FINAL_D;
+    }
     // TODO: Migrate to Kotlin impl
     public static double rampUp(double distTravel) {
         if (distTravel <= 0) return MIN_SPEED_INITIAL;
         if (distTravel >= RAMPS_UP) return MAX_SPEED;
         return (MAX_SPEED - MIN_SPEED_INITIAL) * (distTravel / RAMPS_UP) + MIN_SPEED_INITIAL;
     }
+    public static double rampUpForward(double distTravel) {
+        if (distTravel <= 0) return MIN_SPEED_INITIAL_D;
+        if (distTravel >= RAMPS_UP_D) return MAX_SPEED_D;
+        return (MAX_SPEED_D - MIN_SPEED_INITIAL_D) * (distTravel / RAMPS_UP_D) + MIN_SPEED_INITIAL_D;
+    }
 
     public DriveForwardPID(RobotConfiguration robot) {
         this.robot = robot;
@@ -88,7 +105,7 @@ public void DriveForward(double target, Telemetry telemetry, double timeout) {
             double ldist = LeftOdoDist() - lbase;
             double avg = (rdist + ldist) / 2.0;
 
-            if (avg > target - acceptableError) break;
+            if (avg > target - acceptableError_D) break;
             double error = rdist - ldist;
 
             double currentTime = stopwatch.time();
@@ -97,7 +114,7 @@ public void DriveForward(double target, Telemetry telemetry, double timeout) {
             previousTime = currentTime;
 
             double correction = kp * error + ki * overall_error;
-            double speed = Math.min(rampUp(avg), rampDown(target - avg));
+            double speed = Math.min(rampUpForward(avg), rampDownForward(target - avg));
             driveMotors.frontLeft.setPower(speed + correction);
             driveMotors.backLeft.setPower(speed + correction);
             driveMotors.frontRight.setPower(speed - correction);
@@ -129,7 +146,7 @@ public void DriveReverse(double target, Telemetry telemetry, double timeout) {
             double ldist = lbase - LeftOdoDist();
             double avg = (rdist + ldist) / 2.0;
 
-            if (avg > target - acceptableError) break;
+            if (avg > target - acceptableError_D) break;
 
             double error = rdist - ldist;
 
@@ -138,15 +155,18 @@ public void DriveReverse(double target, Telemetry telemetry, double timeout) {
             overall_error += error * dt;
 
             double correction = kp * error + ki * overall_error;
-            double speed = -Math.min(rampUp(avg), rampDown(target - avg));
+            double speed = -Math.min(rampUpForward(avg), rampDownForward(target - avg));
             driveMotors.frontLeft.setPower(speed - correction);
             driveMotors.backLeft.setPower(speed - correction);
             driveMotors.frontRight.setPower(speed + correction);
             driveMotors.backRight.setPower(speed + correction);
+            //telemetry.addData("Target: ", target);
+
         }
 
         driveMotors.setAll(0);
         sumOfError = overall_error;
+
     }
 
     public void strafeRight(double target, Telemetry telemetry) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveForward.java

@@ -24,7 +24,7 @@ public void runOpMode() {
         waitForStart();
         if (!opModeIsActive()) return;
 
-        pidDrive.strafeLeft(24, telemetry);
+        pidDrive.DriveForward(24, telemetry);
 //        pidTurn.TurnRobot(360, telemetry);
 //        sleep(1000);
 //        pidTurn.TurnRobot(-360, telemetry);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java

@@ -258,11 +258,12 @@ public void runOpMode() throws InterruptedException {
             turnPID.TurnRobot(modifier * 100.0, telemetry);
             forwardPID.DriveReverse(19.0, telemetry, 5.0);
             // Left: 9.5", Center: 15", Right: 21.5"
+            Task navigateInFrontOfTag;
 
             scheduler.task(tracker.getTaskFactory());
             scheduler.task(trackerTagUpdate.updateTool(tracker));
 
-            Task navigateInFrontOfTag;
+
             switch (result) {
                 case Left:
                     navigateInFrontOfTag = newOdo.strafeLeft(
@@ -293,7 +294,6 @@ public void runOpMode() throws InterruptedException {
 
             // ewwwww
             scheduler.runToCompletion(this::opModeIsActive);
-
 //            robot.liftLeft().setTargetPosition(1000);
 
             // encoder ticks

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyBlueRightTest.java

@@ -0,0 +1,71 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName;
+import org.firstinspires.ftc.teamcode.Var;
+import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration;
+import org.firstinspires.ftc.teamcode.odo.DriveForwardPID;
+import org.firstinspires.ftc.teamcode.odo.TurnPID;
+
+@Autonomous
+public class TwentyBlueRightTest extends LinearOpMode{
+    private DriveForwardPID forwardPID;
+    private TurnPID turnPID;
+    private RobotConfiguration robot;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        RobotConfiguration robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap);
+        robot.clearEncoders();
+        DriveForwardPID drivePID = new DriveForwardPID(robot);
+        TurnPID turnPID = new TurnPID(robot);
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap);
+        waitForStart();
+        if (!opModeIsActive()) return;
+        /*Left spike marker BlueRightAuto
+        /drivePID.DriveReverse(21.0, telemetry);
+        turnPID.TurnRobot(45.0, telemetry);
+        drivePID.DriveReverse(3.0, telemetry);
+        turnPID.TurnRobot(-45.0, telemetry);
+        drivePID.strafeLeft(2, telemetry);
+        drivePID.DriveReverse(25, telemetry);
+        sleep(250);
+        turnPID.TurnRobot(95, telemetry);
+        drivePID.DriveReverse(80, telemetry);
+        drivePID.strafeRight(27, telemetry);*/
+
+        /*Center spike marker BlueRightAuto
+        drivePID.DriveReverse(27, telemetry);
+        drivePID.strafeLeft(7, telemetry);
+        drivePID.DriveReverse(24, telemetry);
+        turnPID.TurnRobot(95, telemetry);
+        drivePID.DriveReverse(87, telemetry);
+        drivePID.strafeRight(22, telemetry);*/
+
+        //Right spike marker BlueRightAuto
+        drivePID.DriveReverse(16.0, telemetry);
+        turnPID.TurnRobot(-60.0, telemetry);
+        turnPID.TurnRobot(60, telemetry);
+        drivePID.strafeRight(4.5, telemetry);
+        drivePID.DriveReverse(34, telemetry);
+        sleep(250);
+        turnPID.TurnRobot(90, telemetry);
+        drivePID.DriveReverse(78, telemetry);
+        drivePID.strafeRight(20.5, telemetry);
+
+        while (opModeIsActive()) {
+            telemetry.addData("LEFT  ", drivePID.LeftOdoDist());
+            telemetry.addData("RIGHT", drivePID.RightOdoDist());
+            telemetry.update();
+            sleep(20);
+        }
+
+
+
+    }
+}