Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Files at this revision

API Documentation at this revision

Comitter:
kyleliangus
Date:
Fri May 26 03:46:03 2017 +0000
Parent:
31:9b71b44e0867
Child:
33:68ce1f74ab5f
Commit message:
PID values still wonky. IR PID vs Encoder PID also wonky

Changed in this revision

irpair.cpp Show annotated file Show diff for this revision Revisions of this file
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/irpair.cpp	Wed May 24 21:47:21 2017 +0000
+++ b/irpair.cpp	Fri May 26 03:46:03 2017 +0000
@@ -1,28 +1,38 @@
 #include "irpair.h"
 #include "mbed.h"
 
-Ticker toggleIr;
+//Ticker toggleIr;
+
+#define dark_samples 10
+#define busy_wait 10
 
 void IRPair::calibrateSensor() {
 
     ir.write( 1 );
-    wait_us(70);
+    wait_us(55);
 
     for (int i = 0; i < samplesToTake; ++i) 
         sensorAvg += recv.read();
         
     ir.write( 0 );
     sensorAvg /= samplesToTake;
+    wait_us(5);
 }
 
-float IRPair::getSamples( int samples )
+double IRPair::getSamples( int samples )
 {
-    float z = 0;
+    double z = 0;
+    
+    ir.write( 0 );
+    //for( int i = 0; i < dark_samples; i++ )
+    //    z1 += recv.read();
+    
     ir.write( 1 );
-    wait_us(70);
+    wait_us(55);
     
     for( int i = 0; i < samples; ++i )
         z += recv.read();
     ir.write( 0 );
+    wait_us(5);
     return z / samples;
 }
\ No newline at end of file
--- a/main.cpp	Wed May 24 21:47:21 2017 +0000
+++ b/main.cpp	Fri May 26 03:46:03 2017 +0000
@@ -136,8 +136,8 @@
     right_motor.move(speed0);
     left_motor.move(speed1);
 
-    float ir1 = IRP_1.getSamples(50);
-    float ir4 = IRP_4.getSamples(50);
+    double ir1 = IRP_1.getSamples(50);
+    double ir4 = IRP_4.getSamples(50);
 
     if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4) {
         return;
@@ -340,6 +340,7 @@
 
 void nCellEncoderAndIR(double cellCount)
 {
+    //serial.printf("I'm inside IR!");
     double currentError = 0;
     double previousError = 0;
     double derivError = 0;
@@ -353,8 +354,9 @@
 
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
+    serial.printf("%d, %d\n", desiredCount0, desiredCount1 );
 
-    left_motor.forward(0.16);
+    left_motor.forward(0.15);
     right_motor.forward(0.15);
 
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
@@ -365,13 +367,16 @@
 
     int state = 0;
 
+    //serial.printf("Enter encoder while loop\n");
+    //serial.printf("%d, %d\n", encoder0.getPulses(), encoder1.getPulses() );
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
-        receiverTwoReading = IRP_2.getSamples(100);
-        receiverThreeReading = IRP_3.getSamples(100);
-        receiverOneReading = IRP_1.getSamples(100);
-        receiverFourReading = IRP_4.getSamples(100);
+        //serial.printf("Entered encoder while loop\n");
+        receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM );
+        receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM );
+        receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
+        receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
 
-        if(  receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ) {
+        if(  receiverOneReading > IRP_1.sensorAvg * 3 || receiverFourReading > IRP_4.sensorAvg * 3 ) {
             break;
         }
 
@@ -402,6 +407,7 @@
             blueLed.write(1);
         }
 
+        //serial.printf("Entering switch\n");
         switch(state) {
             case(0): { // both walls there
                 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL)  - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
@@ -420,6 +426,7 @@
                 break;
             }
         }
+        //serial.printf("Exiting switch");
 
         sumError += currentError;
         derivError = currentError - previousError;
@@ -431,6 +438,8 @@
             rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
             leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
         }
+        
+        //serial.printf("%f, %f\n", leftSpeed, rightSpeed);
         if (leftSpeed > 0.30) leftSpeed = 0.30;
         if (leftSpeed < 0) leftSpeed = 0;
         if (rightSpeed > 0.30) rightSpeed = 0.30;
@@ -438,7 +447,7 @@
 
         right_motor.forward(rightSpeed);
         left_motor.forward(leftSpeed);
-        pidOnEncoders();
+        //pidOnEncoders();
 
         previousError = currentError;
     }
@@ -446,7 +455,7 @@
         if (currDir % 4 == 0) {
             mouseY += 1;
         } else if (currDir % 4 == 1) {
-            mouseX + 1;
+            mouseX += 1;
         } else if (currDir % 4 == 2) {
             mouseY -= 1;
         } else if (currDir % 4 == 3) {
@@ -494,6 +503,7 @@
         }
     }
 
+    //serial.printf("What?\n");
     left_motor.brake();
     right_motor.brake();
 }
@@ -522,7 +532,7 @@
         goingToCenter = false;
         changeManhattanDistance(goingToCenter);
     } else if (!goingToCenter && (currentDistance == 0)) {
-        goingToCenter == true;
+        goingToCenter = true;
         changeManhattanDistance(goingToCenter);
     }
 
@@ -861,17 +871,19 @@
     wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
     visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
 
-    int prevEncoder0Count = 0;
-    int prevEncoder1Count = 0;
-    int currEncoder0Count = 0;
-    int currEncoder1Count = 0;
+    //int prevEncoder0Count = 0;
+    //int prevEncoder1Count = 0;
+    //int currEncoder0Count = 0;
+    //int currEncoder1Count = 0;
 
-    bool overrideFloodFill = false;
+    //bool overrideFloodFill = false;
     //right_motor.forward( 0.2 );
     //left_motor.forward( 0.2 );
     //turnRight180();
     //wait_ms(1500);
-    int nextMovement = 0;
+    //int nextMovement = 0;
+    //serial.printf("I'm about to enter!");
+    nCellEncoderAndIR(3);
     while (1) {
         // prevEncoder0Count = encoder0.getPulses();
 //        prevEncoder1Count = encoder1.getPulses();
@@ -999,7 +1011,8 @@
 
         ////////////////////////////////////////////////////////////////
 
-        //nCellEncoderAndIR(3);
+        //nCellEncoderAndIR(4);
+        //while(1);
         //break;
 
         //serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100));
@@ -1018,8 +1031,8 @@
         //pidOnEncoders();
         // moveForwardUntilWallIr();
         //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) ) ;
-        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 );
+        //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 );
 
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);
--- a/main.h	Wed May 24 21:47:21 2017 +0000
+++ b/main.h	Fri May 26 03:46:03 2017 +0000
@@ -9,7 +9,7 @@
 #include <utility>      // std::pair, std::make_pair
 
 #define PULSES 3520
-#define SAMPLE_NUM 100
+#define SAMPLE_NUM 40
 
 // Motors
 /*
@@ -167,10 +167,14 @@
 // #define IP_CONSTANT 8.85
 // #define II_CONSTANT 0.005
 // #define ID_CONSTANT 3.15
-#define IP_CONSTANT 8.2
-#define II_CONSTANT 0.06
-#define ID_CONSTANT 7.55
+//#define IP_CONSTANT 8.2
+//#define II_CONSTANT 0.06
+//#define ID_CONSTANT 7.55
  
+#define IP_CONSTANT 0.25
+#define II_CONSTANT 0
+#define ID_CONSTANT 0.00
+
 const int desiredCount180 = 2870;
 const int desiredCountR = 1700;
 const int desiredCountL = 1700;