Fabio Faria / Mbed OS ISR_Mini-explorer_Rangefinder

Preliminary sample code for using the rangefinder.

Dependencies:   ISR_Mini-explorer_tmp2

Diff: main.cpp

Revision:
2:d4d7d6d4a827
Parent:
1:8fd592bac4de
--- a/main.cpp	Wed May 02 14:05:07 2018 +0000
+++ b/main.cpp	Fri May 11 19:22:44 2018 +0000
@@ -6,6 +6,9 @@
 int main()
 {
     initRobot(124);
+    X=20;
+    Y=20;
+    THETA=0;
 
     while(1) {
         if( read_rangefinder() == 0 ) {
@@ -14,6 +17,11 @@
             }
             pc.printf("\n");
         }
+        else
+        {
+            pc.printf("Rangefinder reading failed!\n");
+            q_led_red_rea = 0; // Turns ON rear red LED.
+        }
 
         wait(0.5);
     }