Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Files at this revision

API Documentation at this revision

Comitter:
christine222
Date:
Fri Jun 02 18:50:51 2017 +0000
Parent:
43:f22168a05c3e
Child:
45:8b0bee6baf38
Commit message:
rngesus

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	Sun May 28 10:09:56 2017 +0000
+++ b/main.cpp	Fri Jun 02 18:50:51 2017 +0000
@@ -38,9 +38,9 @@
 
 */
 
-#define IP_CONSTANT 2.00
+#define IP_CONSTANT 1.6
 #define II_CONSTANT 0.00001
-#define ID_CONSTANT 0.2
+#define ID_CONSTANT 0.24
 
 void pidOnEncoders();
 
@@ -64,7 +64,7 @@
     receiverFourReading = IRP_4.getSamples(100);
     right_motor.move(speed0);
     left_motor.move(speed1);
-    wait_us(60000);
+    wait_ms(100);
 
     double distance_to_go = (oneCellCount)*num;
 
@@ -143,7 +143,7 @@
 
     right_motor.move(speed0);
     left_motor.move(speed1);
-    wait_us(60000);
+    wait_us(100000);
 
     double distance_to_go = (oneCellCount)*num;
 
@@ -515,7 +515,7 @@
     
     left_motor.brake();
     right_motor.brake();
-    if (encoder0.getPulses() >= 0.5*desiredCount0 && encoder1.getPulses() >= 0.5*desiredCount1) {
+    if (encoder0.getPulses() >= 0.4*desiredCount0 && encoder1.getPulses() >= 0.4*desiredCount1) {
         if (currDir % 4 == 0) {
             mouseY += 1;
         } else if (currDir % 4 == 1) {
@@ -972,6 +972,14 @@
        //  prevEncoder0Count = encoder0.getPulses();
        // prevEncoder1Count = encoder1.getPulses();
 
+        // turnRight();
+        // wait_ms(2000);
+        // turnLeft();
+        // wait_ms(2000);
+        // turnRight();
+        // wait_ms(2000);
+        // turnRight();
+        // wait_ms(2000);
         
        if (!overrideFloodFill){
            nextMovement = chooseNextMovement();
@@ -981,8 +989,9 @@
                 //redLed.write(1);
                 //greenLed.write(0);
                 //blueLed.write(1);
-               encoderAfterTurn(1);
-               // nCellEncoderAndIR(1);
+               // encoderAfterTurn(1);
+               moveForwardEncoder(0.4);
+               nCellEncoderAndIR(0.6);
            }
            else if (nextMovement == 1){
                justTurned = true;
@@ -1000,10 +1009,54 @@
                justTurned = true;
                turn180();
            }
+       }else{
+        receiverOneReading = IRP_1.getSamples(100);
+        receiverTwoReading = IRP_2.getSamples(100);
+        receiverThreeReading = IRP_3.getSamples(100);
+        receiverFourReading = IRP_4.getSamples(100);
+
+        if(receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ){
+
+            int randomNum = rand() %2;
+            if(randomNum == 0){
+                turnRight();
+            }else{
+                turnLeft();
+            }
+        }else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
+            // both walls there
+            nCellEncoderAndIR(1);
+            redLed.write(1);
+            greenLed.write(0);
+            blueLed.write(1);
+        }else if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
+            // both sides gone
+
+            redLed.write(1);
+            greenLed.write(0);
+            blueLed.write(0);
+            // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass);
+            moveForwardEncoder(1);
+            continue;
+        } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone   RED
+            turnRight();
+            redLed.write(0);
+            greenLed.write(1);
+            blueLed.write(1);
+            // moveForwardEncoder(valToPass);
+            // break;
+        } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
+            turnLeft();
+            redLed.write(1);
+            greenLed.write(1);
+            blueLed.write(0);
+            // moveForwardEncoder(valToPass);
+            // break;
+        } 
        }
 
-       wait_ms(700);                          // reduce this once we fine tune this!
-        
+       wait_ms(500);                          // reduce this once we fine tune this!
+
 
         //////////////////////// TESTING CODE GOES BELOW ///////////////////////////
         
--- a/main.h	Sun May 28 10:09:56 2017 +0000
+++ b/main.h	Fri Jun 02 18:50:51 2017 +0000
@@ -173,18 +173,18 @@
 //#define ID_CONSTANT 7.55
 
 const int desiredCount180 = 3120;   // change accordingly to the terrain
-const int desiredCountR = 1390;
-const int desiredCountL = 1475;
+const int desiredCountR = 1340;
+const int desiredCountL = 1455;
 
 const int oneCellCount = 5480;
-const int oneCellCountMomentum = 4590;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
+const int oneCellCountMomentum = 4578;//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;
 double receiverThreeReading = 0.0;
 double receiverFourReading = 0.0;
 
-const double frontStop = 7.9;
+const double frontStop = 8.35;
 const double LRAvg = 3.5;
 
 float ir1base = 0.0;
@@ -272,7 +272,7 @@
  
     while(1) {
  
-        if(!(abs(error0) < 2) && !(abs(error1) < 2)) {
+        if(!(abs(error0) < 4) && !(abs(error1) < 4)) {
             count0 = encoder0.getPulses();
             count1 = encoder1.getPulses();