Micromouse 18 / Mbed 2 deprecated encoder_v2_12-3

Scaled IR readings by 10^5 so also changed thresholds. Implemented and tested LED multiplexing Working on a new turning scheme involving IR readings

Dependencies:   QEI RemoteIR mbed

Fork of encoder_v2 by Micromouse 18

Files at this revision

API Documentation at this revision

Revision 18:14bf07a27998, committed 2017-12-09

Comitter:
kensterino
Date:
Sat Dec 09 03:37:08 2017 +0000
Parent:
17:2b519a746a1e
Commit message:
works but make sure the fucking battery is charged

Changed in this revision

PID_control.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/PID_control.cpp	Sat Dec 09 01:13:47 2017 +0000
+++ b/PID_control.cpp	Sat Dec 09 03:37:08 2017 +0000
@@ -157,6 +157,11 @@
         }
 }
 
+bool corner() {
+    if (RS_reading > 1078000000 && LS_reading > 1078000000 && RF_reading > 1080000000 && LF_reading > 1080000000)
+        return true;
+    else return false;
+    }
 
 void forward()
 {
@@ -226,6 +231,7 @@
         printf(" + right: %f\n " , high_speed);
         printf("\n");
         //wait(2);
+        resetEncoders();
         }
     if (IRError < -30) {
         double high_speed = i_speed - IR_correction;
@@ -243,9 +249,10 @@
         printf(" - left: %f\n " , high_speed);
         printf(" - right: %f\n " , low_speed);
         printf("\n");
+        resetEncoders();
         //wait(2);
     }
-    if (IRError < 30 && IRError > -30) {
+    if (IRError <= 30 && IRError >= -30) {
        forward();
        printf("forward\n");
        printf("\n");
@@ -322,10 +329,11 @@
 }
 
 void turnRight() {
+    //double scale = RF_reading / LF_reading;
     m_Left_F.write(0.5);
     m_Right_B.write(0.5);
     resetEncoders();
-    wait(0.1);
+    wait(0.1); //*scale);
     
     }
 
@@ -437,9 +445,9 @@
         {
             printf(" 1 stop\n");
             stop(); //stop the rat
-            wait(1);    //delay optional
+            //wait(1);    //delay optional
             while(1){
-                forwardSlow();
+                //forwardSlow();
                 if (RF_reading > threshold && LF_reading > threshold)
                 {
                     printf(" 2 stop\n");
@@ -535,16 +543,18 @@
     }
     
 void afterTurnForward() {
+    resetEncoders();
     Timer turn;
     turn.start();
     int current = turn.read_ms();
-    while ((turn.read_ms() - current) < 200)
-            { m_Right_F.write(i_speed);
-            m_Left_F.write(i_speed);
+    while ((turn.read_ms() - current) < 500)
+            { forward();
             printf("%f\n", turn.read_ms());
              }
     printf("out of after\n");
     stop();
+    //resetEncoders();
+    wait(1);
     }
     
 void LeftOrRight() {
@@ -573,32 +583,26 @@
     //testStop();
     //turnAround(); 
     
-    while (1) { 
+    while (1) {
+         
         IRForward(); 
-        /*if (testStop()) {
-            backUpForTurn();
-            turnAround();
-            stop();   
-            wait(1); 
-            afterTurnForward();
-            wait(0.5); 
-        }
-        */
         if (testStop()) {
             backUpForTurn();
             LeftOrRight();
             stop();
-            wait(1); 
+            wait(0.05); 
             afterTurnForward();
-            wait(0.5);
+            //wait(0.5);
             stop();
-            //break;
             }
-        
+            
+        /*if (corner) {     //TEST CORNER() test
+            turnAround();
+            stop();
+            break;
+            }
+        */       
+           //printf("%d\n", RF_reading);      
         }
-        
-    stop();
+    
 }
-    
-
-    
\ No newline at end of file