Diff: Buggy.cpp

Revision:

5:a33f016d5962

Parent:

4:2d939ef2b09c

Child:

6:edbaeaaf08bb

--- a/Buggy.cpp	Sun Aug 06 20:13:33 2017 +0000
+++ b/Buggy.cpp	Mon Aug 07 18:57:55 2017 +0000
@@ -26,6 +26,9 @@
 void SendSonarTrigger();
 void onButtonA(MicroBitEvent);
 void onButtonB(MicroBitEvent);
+void PrintSonarTiming(void);
+void RunBasicBuggyMotorTest(unsigned int Voltage, unsigned int Time_ms, unsigned int Pause_ms);
+void TestSonar();
 
 /*******************************
 Global variables for the microbit runtime
@@ -492,4 +495,49 @@
     {
         MotorTestGo = true;
     }
-} 
\ No newline at end of file
+} 
+
+void SelfTest()
+{
+    int Counter = 3;
+    unsigned int LastSystemTime = 0;
+    
+    //Display start message using the LEDs
+    //uBit.display.scroll("Hello World");
+    //Instead, display 3, 2, 1, 0
+    uBit.display.printAsync(Counter);
+    LastSystemTime = uBit.systemTime();
+    while (Counter > 0)
+    {
+        if( (uBit.systemTime()-LastSystemTime) > 1000)
+        {    
+            //uBit.serial.printf("Counter=%d, ", Counter);
+            LastSystemTime = uBit.systemTime();
+            Counter = Counter - 1;
+            uBit.display.printAsync(Counter);
+        }
+    }
+        
+    while(1)
+    {
+        //Run the sonar test for 10 secs
+        LastSystemTime = uBit.systemTime();
+        while( (uBit.systemTime()-LastSystemTime) < 10000)
+        {    
+            TestSonar();
+        }
+        //Display 0
+        uBit.display.printAsync(Counter);
+        //Run the motor test
+        RunBasicBuggyMotorTest(800, 1000, 1000);
+    }
+}
+
+void DisplaySonarTiming()
+{
+    while(1)
+    {
+        PrintSonarTiming();
+    }
+    
+}