more stuff

2 files changed, 53 insertions, 26 deletions

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java
index 2fe7d0c..c4b2160 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java
@@ -88,19 +88,19 @@ public class TankbotAutonomousv1 extends LinearOpMode {
 
     static final double COUNTS_PER_MOTOR_REV = 560;    // HD Hex 1:20
     static final double DRIVE_GEAR_REDUCTION = 1.0;     // This is < 1.0 if geared UP
-    static final double WHEEL_DIAMETER_INCHES = 4.0;     // For figuring circumference
     static final double INCH_PER_REV = 19.5 / 2.54;
-    static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
-            (INCH_PER_REV);
+    static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / INCH_PER_REV;
 
     // These constants define the desired driving/control characteristics
     // The can/should be tweaked to suite the specific robot drive train.
-    static final double DRIVE_SPEED = 0.2;     // Nominal speed for better accuracy.
-    static final double TURN_SPEED = 0.2;     // Our motors are really fast
+    static final double DRIVE_SPEED = 0.40;     // Nominal speed for better accuracy.
+    static final double TURN_SPEED = 0.20;     // Our motors are really fast
 
-    static final double HEADING_THRESHOLD = 1.0;
-    static final double P_TURN_COEFF = 0.08;     // Larger is more responsive, but also less stable
-    static final double P_DRIVE_COEFF = 0.03;     // Larger is more responsive, but also less stable
+    static final double HEADING_THRESHOLD = 0.75;
+    static final double P_TURN_COEFF = 0.07;     // Larger is more responsive, but also less stable
+    static final double P_DRIVE_COEFF = 0.01;     // Larger is more responsive, but also less stable
+    static final double D_TURN_COEFF = .019;
+    static final double D_DRIVE_COEFF = .001;
 
     /* Which way the robot is pointing, and which way gravity pulls, as measured by the gyro.
        Use gyroUpdate() to update
@@ -225,7 +225,7 @@ public class TankbotAutonomousv1 extends LinearOpMode {
         //robot.IntakeMotor.setPower(.5);
         if(sample_pos >= 0) {
             /* knock sample off */
-            double sample_distances[] = { 35, 30, 35 };
+            double sample_distances[] = { 32, 30, 32 };
             gyroDriveRelative(DRIVE_SPEED, sample_distances[sample_pos], 0);
             gyroDriveRelative(DRIVE_SPEED, -sample_distances[sample_pos], 0);
         }
@@ -237,11 +237,11 @@ public class TankbotAutonomousv1 extends LinearOpMode {
             case 0:
                 /* reverse the previous turn (we will be facing depot directly) */
                 gyroTurnRelative(TURN_SPEED, (sample_pos < 0 ? 0 : -sample_angles[sample_pos]));
-                gyroDriveRelative(DRIVE_SPEED, 12, 0);
-                gyroTurnRelative(TURN_SPEED, -80);
+                gyroDriveRelative(DRIVE_SPEED * .75, 10, 0);
+                gyroTurnRelative(TURN_SPEED, -85);
                 gyroDriveRelative(DRIVE_SPEED, 20, 0);
-                gyroTurnRelative(TURN_SPEED, (45 + 10));
-                gyroDriveRelative(DRIVE_SPEED, 16, 0);
+                gyroTurnRelative(TURN_SPEED, (45 - 5));
+                gyroDriveRelative(DRIVE_SPEED, 14, 0);
                 gyroTurnRelative(TURN_SPEED, 90);
                 gyroDriveRelative(DRIVE_SPEED, 35, 0);
                 break;
@@ -253,9 +253,9 @@ public class TankbotAutonomousv1 extends LinearOpMode {
         }
 
         /* Eject marker */
-        robot.IntakeMotor.setPower(-.5);
-        sleep(300);
-        gyroDriveRelative(DRIVE_SPEED * 2, -8, 0);
+        robot.IntakeMotor.setPower(-.75);
+        sleep(2000);
+        gyroDriveRelative(DRIVE_SPEED * 2, -20, 0);
 
         /* TODO: drive to crater */
 
@@ -307,6 +307,8 @@ public class TankbotAutonomousv1 extends LinearOpMode {
         // Ensure that the opmode is still active
         if (opModeIsActive()) {
 
+            resetDerivative();
+
             // Determine new target position, and pass to motor controller
             moveCounts = (int)(distance * COUNTS_PER_INCH);
             newLeftTarget = robot.DriveLeft.getCurrentPosition() + moveCounts;
@@ -330,7 +332,7 @@ public class TankbotAutonomousv1 extends LinearOpMode {
 
                 // adjust relative speed based on heading error.
                 error = getError(angle);
-                steer = getSteer(error, P_DRIVE_COEFF);
+                steer = getSteer(error, P_DRIVE_COEFF, D_DRIVE_COEFF);
 
                 // if driving in reverse, the motor correction also needs to be reversed
                 if (distance < 0)
@@ -390,8 +392,10 @@ public class TankbotAutonomousv1 extends LinearOpMode {
      */
     public void gyroTurn (  double speed, double angle) {
 
+        resetDerivative();
+
         // keep looping while we are still active, and not on heading.
-        while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF)) {
+        while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF, D_TURN_COEFF)) {
             // Update telemetry & Allow time for other processes to run.
             telemetry.update();
         }
@@ -399,7 +403,9 @@ public class TankbotAutonomousv1 extends LinearOpMode {
 
     public void gyroTurnRelative(double speed, double dtheta)
     {
-        gyroTurn(speed, getHeading(true) + dtheta);
+        double angle = getHeading(true) + dtheta;
+        gyroTurn(speed, angle);
+        gyroHold(speed, angle, .25);
     }
 
     /**
@@ -414,13 +420,15 @@ public class TankbotAutonomousv1 extends LinearOpMode {
      */
     public void gyroHold( double speed, double angle, double holdTime) {
 
+        resetDerivative();
+
         ElapsedTime holdTimer = new ElapsedTime();
 
         // keep looping while we have time remaining.
         holdTimer.reset();
         while (opModeIsActive() && (holdTimer.time() < holdTime)) {
             // Update telemetry & Allow time for other processes to run.
-            onHeading(speed, angle, P_TURN_COEFF);
+            onHeading(speed, angle, P_TURN_COEFF, D_TURN_COEFF);
             telemetry.update();
         }
 
@@ -440,7 +448,7 @@ public class TankbotAutonomousv1 extends LinearOpMode {
      * @return
      */
 
-    boolean onHeading(double speed, double angle, double PCoeff) {
+    boolean onHeading(double speed, double angle, double PCoeff, double DCoeff) {
         double   error ;
         double   steer ;
         boolean  onTarget = false ;
@@ -457,7 +465,7 @@ public class TankbotAutonomousv1 extends LinearOpMode {
             onTarget = true;
         }
         else {
-            steer = getSteer(error, PCoeff);
+            steer = getSteer(error, PCoeff, DCoeff);
             leftSpeed  = speed * steer;
             rightSpeed   = -leftSpeed;
         }
@@ -491,14 +499,33 @@ public class TankbotAutonomousv1 extends LinearOpMode {
         return robotError;
     }
 
+    boolean have_deriv = false;
+    double last_error;
+    long last_timestamp; /* nanoseconds */
+    private void resetDerivative()
+    {
+        have_deriv = false;
+    }
+
     /**
      * returns desired steering force.  +/- 1 range.  +ve = steer left
      * @param error   Error angle in robot relative degrees
      * @param PCoeff  Proportional Gain Coefficient
      * @return
      */
-    public double getSteer(double error, double PCoeff) {
-        return Range.clip(error * PCoeff, -1, 1);
+    public double getSteer(double error, double PCoeff, double DCoeff) {
+        double steer = error * PCoeff;
+        long time_now = System.nanoTime();
+        if(have_deriv && last_timestamp != time_now)
+        {
+            double dxdt = (error - last_error) / (time_now - last_timestamp);
+            steer += DCoeff * dxdt;
+        }
+        steer = Range.clip(steer, -1, 1);
+        have_deriv = true;
+        last_error = error;
+        last_timestamp = time_now;
+        return steer;
     }
 
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java
index 00af146..fac3eb4 100755
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java
@@ -193,9 +193,9 @@ public class TankbotDriver extends LinearOpMode {
             boolean no_lift = false;
 
             if(gamepad1.a)
-                dlift += 2240/12;
+                dlift += 2240/5;
             else if(gamepad1.y)
-                dlift -= 2240/12;
+                dlift -= 2240/5;
             else
                 no_lift = true;