Revision 36:9c4cc9944b69, committed 2017-05-27

Comitter:

kyleliangus

Date:

Sat May 27 02:40:10 2017 +0000

Parent:

35:a5bd9ef82210

Child:

37:3dcc95e9321c

Commit message:

Fixed Right turn, left turn inconsistent

Changed in this revision

--- a/main.cpp	Sat May 27 00:41:19 2017 +0000
+++ b/main.cpp	Sat May 27 02:40:10 2017 +0000
@@ -190,6 +190,7 @@
     return;
 }
 
+/*
 void moveForwardUntilWallIr()
 {
     double currentError = 0;
@@ -254,7 +255,7 @@
         right_motor.brake();
     }
 }
-
+*/
 void printDipFlag()
 {
     if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
@@ -349,18 +350,18 @@
     double derivError = 0;
     double sumError = 0;
 
-    double HIGH_PWM_VOLTAGE_R = 0.15;
-    double HIGH_PWM_VOLTAGE_L = 0.16;
+    double HIGH_PWM_VOLTAGE_R = 0.11;
+    double HIGH_PWM_VOLTAGE_L = 0.11;
 
-    double rightSpeed = 0.15;
-    double leftSpeed = 0.16;
+    double rightSpeed = 0.11;
+    double leftSpeed = 0.11;
 
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
     serial.printf("%d, %d\n", desiredCount0, desiredCount1 );
 
-    left_motor.forward(0.15);
-    right_motor.forward(0.15);
+    left_motor.forward(0.11);
+    right_motor.forward(0.11);
 
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
@@ -802,6 +803,18 @@
     }
 }
 
+void waitButton4()
+{
+    //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
+    int init_val = dipFlags & BUTTON4_FLAG;
+    while( (dipFlags & BUTTON4_FLAG) == init_val )
+    {
+        // do nothing until ready
+    }
+    //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
+    wait( 2 );
+}
+
 int main()
 {
     //Set highest bandwidth.
@@ -857,13 +870,7 @@
     dipButton3.fall(&disableButton3);
     dipButton4.fall(&disableButton4);
 
-    serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
-    while( (dipFlags & BUTTON4_FLAG) == 0 )
-    {
-        // do nothing until ready
-    }
-    serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
-    wait( 2 );
+    //waitButton4();
 
     // init the wall, and mouse loc arrays:
     wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
@@ -875,9 +882,9 @@
     //int currEncoder1Count = 0;
 
     //bool overrideFloodFill = false;
-    right_motor.forward( 0.2 );
-    left_motor.forward( 0.2 );
-    //turnRight180();
+    right_motor.forward( 0.11 );
+    left_motor.forward( 0.11 );
+    //turn180();
     //wait_ms(1500);
     //int nextMovement = 0;
     while (1) {
@@ -902,7 +909,7 @@
 //            }
 //            else if (nextMovement == 4){
 //                justTurned = true;
-//                turnRight180();
+//                turn180();
 //            }
 //        }
 //        else{
@@ -927,30 +934,22 @@
 
 
         //////////////////////// TESTING CODE GOES BELOW ///////////////////////////
-        nCellEncoderAndIR(4);
-        wait_ms(200);
-        turnRight();
-        wait_ms(200);
-        nCellEncoderAndIR(3);
-        wait_ms(200);
-        turnRight();
-        wait_ms(200);
-        nCellEncoderAndIR(2);
-        wait_ms(200);
-        turnRight();
-        wait_ms(200);
-        nCellEncoderAndIR(1);
-        wait_ms(200);
+        
+        turnLeft();
+        wait_ms(300);
+        turnLeft();
+        wait_ms(300);
+        turnLeft();
+        wait_ms(300);
         turnLeft();
-        wait_ms(200);
-        nCellEncoderAndIR(1);
-        wait_ms(200);
-        turnLeft();
-        wait_ms(200);
-        nCellEncoderAndIR(1);
-        wait_ms(200);
-        turnRight180();
-        break;
+        /*
+        wait_ms(300);
+        turn180();
+        wait_ms(300);
+        turn180();
+        wait_ms(300);*/
+        
+        waitButton4();
         // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\n",encoder0.getPulses(), encoder1.getPulses() );
         // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
         // serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );

--- 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;