Diff: main.h

Revision:

36:9c4cc9944b69

Parent:

35:a5bd9ef82210

Child:

37:3dcc95e9321c

diff -r a5bd9ef82210 -r 9c4cc9944b69 main.h
--- a/main.h	Sat May 27 00:41:19 2017 +0000
+++ b/main.h	Sat May 27 02:40:10 2017 +0000
@@ -10,6 +10,7 @@
 
 #define PULSES 3520
 #define SAMPLE_NUM 40
+#define WHEEL_SPEED 0.10
 
 // Motors
 /*
@@ -171,12 +172,12 @@
 //#define II_CONSTANT 0.06
 //#define ID_CONSTANT 7.55
 
-const int desiredCount180 = 3000;   // change accordingly to the terrain
-const int desiredCountR = 1700;
-const int desiredCountL = 1700;
+const int desiredCount180 = 3200;   // change accordingly to the terrain
+const int desiredCountR = 1600;
+const int desiredCountL = 1590;
  
 const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4570;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
+const int oneCellCountMomentum = 4800;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
  
 double receiverOneReading = 0.0;
 double receiverTwoReading = 0.0;
@@ -190,44 +191,41 @@
 
 float ir4base = 0.0;
 
-float initAverageL = 8.25;
-float averageDivL = 27.8; //blue
-float initAverageR = 8.75; //4.5
-float averageDivR = 28.5; //red
 float averageDivUpper = 0.5;
-
-float noWallR = 0.007;
-float noWallL = 0.007;
                           
 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.000085;
+ 
     int counter = 0;
     int initial0 = encoder0.getPulses();
     int initial1 = encoder1.getPulses();
  
-    int desiredCount0 = initial0 - desiredCountL;
-    int desiredCount1 = initial1 + desiredCountL;
+    int desiredCount0 = initial0 - desiredCountL; // left wheel
+    int desiredCount1 = initial1 + desiredCountL; // right wheel
  
     int count0 = initial0;
     int count1 = initial1;
  
-    double error0 = count0 - desiredCount0;
-    double error1 = count1 - desiredCount1;
+    double error0 = desiredCount0 - count0; // is negative
+    double error1 = desiredCount1 - count1; // is positive
  
     while(1) {
  
-        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+        if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
             count0 = encoder0.getPulses();
             count1 = encoder1.getPulses();
+            
+            error0 = desiredCount0 - count0; // is negative
+            error1 = desiredCount1 - count1; // is positive
  
-            error0 = count0 - desiredCount0;
-            error1 = count1 - desiredCount1;
+            //serial.printf("%f, %f\n", error0, error1 );
  
-            right_motor.move(speed0);
-            left_motor.move(speed1);
+            right_motor.move(error1*kp);
+            left_motor.move(error0*kp);
             counter = 0;
         } else {
             counter++;
@@ -252,30 +250,34 @@
     double speed0 = -0.11;
     double speed1 = 0.12;     // change back to 0.13 if turns stop working, testing something out!
  
+    double kp = 0.00009;
+ 
     int counter = 0;
     int initial0 = encoder0.getPulses();
     int initial1 = encoder1.getPulses();
  
-    int desiredCount0 = initial0 + desiredCountR;
-    int desiredCount1 = initial1 - desiredCountR;
+    int desiredCount0 = initial0 + desiredCountR; // left wheel
+    int desiredCount1 = initial1 - desiredCountR; // right wheel
  
     int count0 = initial0;
     int count1 = initial1;
  
-    double error0 = count0 - desiredCount0;
-    double error1 = count1 - desiredCount1;
+    double error0 = desiredCount0 - count0; // is positive
+    double error1 = desiredCount1 - count1; // is negative
  
     while(1) {
  
-        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+        if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
             count0 = encoder0.getPulses();
             count1 = encoder1.getPulses();
  
-            error0 = count0 - desiredCount0;
-            error1 = count1 - desiredCount1;
+            error0 = desiredCount0 - count0; // is positive
+            error1 = desiredCount1 - count1; // is negative
  
-            right_motor.move(speed0);
-            left_motor.move(speed1);
+            //serial.printf("%f, %f\n", error0, error1 );
+ 
+            right_motor.move(error1*kp);
+            left_motor.move(error0*kp);
             counter = 0;
         } else {
             counter++;
@@ -294,7 +296,7 @@
     currDir += 1;
 }
 
-inline void turnRight180()
+inline void turn180()
 {
     double speed0 = -0.15;
     double speed1 = 0.16;