Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Files at this revision

API Documentation at this revision

Comitter:
sahilmgandhi
Date:
Wed May 17 02:01:34 2017 +0000
Parent:
19:7b66a518b6f8
Child:
21:9a6cb07bdcb6
Commit message:
added some init code to keep track of walls as moving. Turning works now (avg/4 gives good values so long as we are in the center)!; Also maybe we should start off facing back, and then turn 180 to get better average readings?

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Mon May 15 00:54:41 2017 +0000
+++ b/main.cpp	Wed May 17 02:01:34 2017 +0000
@@ -16,15 +16,15 @@
 */
 
 // Constants for when HIGH_PWM_VOLTAGE = 0.1
-#define IP_CONSTANT 8.9
-#define II_CONSTANT 1
-#define ID_CONSTANT 3.1
+#define IP_CONSTANT 8.85
+#define II_CONSTANT 0.005
+#define ID_CONSTANT 3.15
 
 const int desiredCountR = 1400;
 const int desiredCountL = 1475;
 
 const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4600;      // one cell count is actually approximately 5400, but this value is considering momentum!
+const int oneCellCountMomentum = 4700;      // one cell count is actually approximately 5400, but this value is considering momentum!
 
 float receiverOneReading = 0.0;
 float receiverTwoReading = 0.0;
@@ -78,6 +78,7 @@
     right_motor.brake();
     left_motor.brake();
     turnFlag = 0;           // zeroing out the flags!
+    currDir -= 1;
 }
 
 void turnRight()
@@ -98,7 +99,6 @@
     double error0 = count0 - desiredCount0;
     double error1 = count1 - desiredCount1;
 
-
     while(1) {
 
         if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
@@ -125,6 +125,7 @@
     right_motor.brake();
     left_motor.brake();
     turnFlag = 0;
+    currDir += 1;
 }
 
 void turnLeft180()
@@ -171,6 +172,7 @@
 
     right_motor.brake();
     left_motor.brake();
+    currDir -= 2;
 }
 
 void turnRight180()
@@ -216,23 +218,30 @@
     }
     right_motor.brake();
     left_motor.brake();
+    currDir += 2;
 }
 
 void moveForwardCellEncoder(double cellNum){
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
 
-    left_motor.forward(0.11);
-    right_motor.forward(0.10);
+    left_motor.forward(0.125);
+    right_motor.forward(0.095);
     wait_ms(1);
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
-        receiverTwoReading = IRP_2.getSamples(5);
-        receiverThreeReading = IRP_3.getSamples(5);
-        if (receiverThreeReading < 0.0007f)
+        receiverTwoReading = IRP_2.getSamples(100);
+        receiverThreeReading = IRP_3.getSamples(100);
+        // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading);
+        if (receiverThreeReading < IRP_3.sensorAvg/4){
+            // redLed.write(1);
+            // blueLed.write(0);
             turnFlag |= RIGHT_FLAG;
-        else if (receiverTwoReading < 0.0009f)
+        }
+        else if (receiverTwoReading < IRP_2.sensorAvg/4){
+            // redLed.write(0);
+            // blueLed.write(1);
             turnFlag |= LEFT_FLAG;
-        // serial.printf("Encoder0 count is: %d, Encoder1 count is: %d, desiredEncoder0 = %d, desiredEncoder1 = %d\n", encoder0.getPulses(), encoder1.getPulses(), desiredCount0, desiredCount1);
+        }
         pidOnEncoders();
     }
 
@@ -266,7 +275,7 @@
     double HIGH_PWM_VOLTAGE = 0.1;
 
     double rightSpeed = 0.1;
-    double leftSpeed = 0.1;
+    double leftSpeed = 0.12;
 
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
@@ -279,7 +288,7 @@
         } else {
             blueLed.write(1);
         }
-
+        sumError += currentError;
         currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
         derivError = currentError - previousError;
         double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
@@ -305,8 +314,6 @@
         ir3 = IRP_3.getSamples( SAMPLE_NUM );
 
     }
-    //sensor1Turn = IR_Sensor1.read();
-    //sensor4Turn = IR_Sensor4.read();
 
     //backward();
     wait_ms(40);
@@ -369,8 +376,8 @@
     count0 = encoder0.getPulses();
     count1 = encoder1.getPulses();
     int diff = count0 - count1;
-    double kp = 0.000075;
-    double kd = 0.000135;
+    double kp = 0.00011;
+    double kd = 0.00014;
     int prev = 0;
 
     int counter = 0;
@@ -385,12 +392,12 @@
         double d = kppart + kdpart;
         
         //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
-        if( x < diff - 50 ) // count1 is bigger, right wheel pushed forward
+        if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
         {
             left_motor.move( -d );
             right_motor.move( d );
         }
-        else if( x > diff + 50 )
+        else if( x > diff + 40 )
         {
             left_motor.move( -d );
             right_motor.move( d );
@@ -407,7 +414,7 @@
     }
 }
 
-void oneCellEncoderAndIR(){
+void oneCellEncoderAndIR(double cellCount){
     double currentError = 0;
     double previousError = 0;
     double derivError = 0;
@@ -417,17 +424,31 @@
     double rightSpeed = 0.10;
     double leftSpeed = 0.10;
 
-    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum;
-    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum;
+    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
+    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
 
-    left_motor.forward(0.11);
+    left_motor.forward(0.125);
     right_motor.forward(0.10);
 
+    float receiverTwoReading = 0.0;
+    float receiverThreeReading = 0.0;
+
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
         
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
-        currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
+        receiverTwoReading = IRP_2.getSamples(100);
+        receiverThreeReading = IRP_3.getSamples(100);
+        if (receiverTwoReading <= IRP_2.sensorAvg/4)
+            currentError = receiverThreeReading - IRP_3.sensorAvg;
+        else if (receiverThreeReading <= IRP_3.sensorAvg/4)
+            currentError = receiverTwoReading - IRP_2.sensorAvg;
+        // else if (receiverTwoReading <= IRP_2.sensorAvg/2 && receiverThreeReading <= IRP_3.sensorAvg/2)       // scenario when both left and right wall missing, use encoders only
+        //     moveForwardCellEncoder(2);
+        else
+            currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg));
+        
+        sumError += currentError;
         derivError = currentError - previousError;
         double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
         if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
@@ -515,18 +536,18 @@
     //left_motor.forward( 0.2 );
 
     while (1) {
-        oneCellEncoderAndIR();
-        // moveForwardCellEncoder(1);
-        // wait(0.5);
-        // handleTurns();
-        // wait(0.5);
-        // moveForwardCellEncoder(1);
-        // wait(0.5);
-        // handleTurns();
+        // oneCellEncoderAndIR(5);
+        // break;
+        moveForwardCellEncoder(1);
+        wait(0.5);
+        handleTurns();
+        wait(0.5);
+        moveForwardCellEncoder(1);
+        wait(0.5);
+        handleTurns();
         break;
-        // moveForwardOneCellEncoder();
         //pidOnEncoders();
-       //moveForwardUntilWallIr();
+       // moveForwardUntilWallIr();
         //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
         //serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
         // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
@@ -535,14 +556,5 @@
 
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);
-//        redLed.write(0);
-//        wait_ms(1000);
-//        redLed.write(1);
-//        greenLed.write(0);
-//        wait_ms(1000);
-//        greenLed.write(1);
-//        blueLed.write(0);
-//        wait_ms(1000);
-//        blueLed.write(1);
     }
 }
--- a/main.h	Mon May 15 00:54:41 2017 +0000
+++ b/main.h	Wed May 17 02:01:34 2017 +0000
@@ -85,4 +85,31 @@
 QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
 QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
 
+#define F_WALL 0x1
+#define L_WALL 0x2
+#define R_WALL 0x4
+#define B_WALL 0x8
+
+int currDir = 100;              // modulo this to keep track of the current direction of the mouse!
+int wallArray[16][16] = {0};    // array to keep track of the walls
+int mouseLoc[16][16] = {0};     // array to keep track of the mouse's current location
+int manhattanDist[16][16] = {
+                                  {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
+                                  {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
+                                  {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
+                                  {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
+                                  {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
+                                  {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
+                                  {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
+                                  {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
+                                  {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
+                                  {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
+                                  {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
+                                  {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
+                                  {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
+                                  {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
+                                  {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
+                                  {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
+                          };
+
 #endif
\ No newline at end of file