Diff: main.h

Revision:

39:058fb32c24e0

Parent:

38:fe05f93009a2

Child:

40:465d2b565977

--- a/main.h	Sat May 27 21:29:55 2017 +0000
+++ b/main.h	Sun May 28 03:42:59 2017 +0000
@@ -172,67 +172,76 @@
 //#define II_CONSTANT 0.06
 //#define ID_CONSTANT 7.55
 
-const int desiredCount180 = 3380;   // change accordingly to the terrain
-const int desiredCountR = 1600;
-const int desiredCountL = 1600;
- 
+const int desiredCount180 = 2900;   // change accordingly to the terrain
+const int desiredCountR = 1490;
+const int desiredCountL = 1500;
+
 const int oneCellCount = 5400;
-const int oneCellCountMomentum = 5070;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
- 
+const int oneCellCountMomentum = 4630;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
+// const int oneCellCountMomentum = 4400;
 double receiverOneReading = 0.0;
 double receiverTwoReading = 0.0;
 double receiverThreeReading = 0.0;
 double receiverFourReading = 0.0;
 
+const double frontStop = 7.2;
+const double LRAvg = 3.5;
+
 float ir1base = 0.0;
 float ir2base = 0.0;
 float ir3base = 0.0;
 float ir4base = 0.0;
 
 float averageDivUpper = 0.5;
-                          
+
 inline void turnLeft()
 {
     double speed0 = 0.11;
     double speed1 = -0.12;    // change back to 0.13 if turns stop working, testing something out!
- 
-    double kp = 0.000082;
- 
+
+    //double kp = 0.000082;
+    double kp = 0.00010;
+
     int counter = 0;
     int initial0 = encoder0.getPulses();
     int initial1 = encoder1.getPulses();
- 
+
     int desiredCount0 = initial0 - desiredCountL; // left wheel
     int desiredCount1 = initial1 + desiredCountL; // right wheel
- 
+
     int count0 = initial0;
     int count1 = initial1;
- 
+
     double error0 = desiredCount0 - count0; // is negative
     double error1 = desiredCount1 - count1; // is positive
- 
+
     while(1) {
  
-        if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
+        if(!(abs(error0) < 4) && !(abs(error1) < 4)) {
             count0 = encoder0.getPulses();
             count1 = encoder1.getPulses();
-            
+
             error0 = desiredCount0 - count0; // is negative
             error1 = desiredCount1 - count1; // is positive
-  
+
             right_motor.move(error1*kp);
             left_motor.move(error0*kp);
             counter = 0;
         } else {
             counter++;
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+
+            error0 = desiredCount0 - count0; // is negative
+            error1 = desiredCount1 - count1; // is positive
             right_motor.brake();
             left_motor.brake();
         }
-        if (counter > 60) {
+        if (counter > 100) {
             break;
         }
     }
- 
+
     right_motor.brake();
     left_motor.brake();
     turnFlag = 0;           // zeroing out the flags!
@@ -245,7 +254,8 @@
     double speed0 = -0.11;
     double speed1 = 0.12;     // change back to 0.13 if turns stop working, testing something out!
  
-    double kp = 0.00009;
+//    double kp = 0.00009;
+    double kp = 0.0002;
  
     int counter = 0;
     int initial0 = encoder0.getPulses();
@@ -262,7 +272,7 @@
  
     while(1) {
  
-        if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
+        if(!(abs(error0) < 2) && !(abs(error1) < 2)) {
             count0 = encoder0.getPulses();
             count1 = encoder1.getPulses();
  
@@ -274,10 +284,15 @@
             counter = 0;
         } else {
             counter++;
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+ 
+            error0 = desiredCount0 - count0; // is positive
+            error1 = desiredCount1 - count1; // is negative
             right_motor.brake();
             left_motor.brake();
         }
-        if (counter > 60) {
+        if (counter > 100) {
             break;
         }
     }
@@ -293,7 +308,8 @@
     double speed0 = -0.10;
     double speed1 = 0.11;
     
-    double kp = 0.000055;
+//    double kp = 0.000055;
+    double kp = 0.00006;
  
     int counter = 0;
     int initial0 = encoder0.getPulses();
@@ -322,10 +338,17 @@
             counter = 0;
         } else {
             counter++;
+            
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+ 
+            error0 = desiredCount0 - count0;
+            error1 = desiredCount1 - count1;
+            
             right_motor.brake();
             left_motor.brake();
         }
-        if (counter > 60) {
+        if (counter > 150) {
             break;
         }
     }