Diff: main.cpp

Revision:

2:47b7a55b0805

Parent:

1:8c05eb53f714

Child:

3:c3a42966b47c

--- a/main.cpp	Sun Jul 16 20:23:27 2017 +0000
+++ b/main.cpp	Mon Jul 31 21:48:36 2017 +0000
@@ -23,404 +23,39 @@
 DEALINGS IN THE SOFTWARE.
 */
 
-#include "MicroBit.h"
+#include "Buggy.h"
 
-#define BUGGY_HALTED    0
-#define BUGGY_RUNNING   1
-#define BUGGY_PAUSED    2
 
 void onButtonA(MicroBitEvent);
 int main();
-void MoveForward(unsigned int Voltage, unsigned int Time_ms);
-void MoveBackward(unsigned int Voltage, unsigned int Time_ms);
-void RotateClockwise(unsigned int Voltage, unsigned int Time_ms);
-void RotateAnticlockwise(unsigned int Voltage, unsigned int Time_ms);
-void PauseLastCommand();
-void ContinueLastCommand();
-bool hasLastCommandCompleted();
-
-MicroBit uBit;
+void SelfTest();
+void DisplaySonarTiming();
 
 MicroBitButton buttonA(MICROBIT_PIN_BUTTON_A, MICROBIT_ID_BUTTON_A);
 MicroBitButton buttonB(MICROBIT_PIN_BUTTON_B, MICROBIT_ID_BUTTON_B);
 
-MicroBitPin MotorSpeed_L(MICROBIT_ID_IO_P0, MICROBIT_PIN_P0, PIN_CAPABILITY_ANALOG);
-MicroBitPin MotorDirection_L(MICROBIT_ID_IO_P8, MICROBIT_PIN_P8, PIN_CAPABILITY_DIGITAL );
-
-MicroBitPin MotorSpeed_R(MICROBIT_ID_IO_P1, MICROBIT_PIN_P1, PIN_CAPABILITY_ANALOG);
-MicroBitPin MotorDirection_R(MICROBIT_ID_IO_P12, MICROBIT_PIN_P12, PIN_CAPABILITY_DIGITAL );
-
-//Sonar trigger and echo
-MicroBitPin Echo(MICROBIT_ID_IO_P13, MICROBIT_PIN_P13, PIN_CAPABILITY_DIGITAL);
-MicroBitPin Trig(MICROBIT_ID_IO_P13, MICROBIT_PIN_P13, PIN_CAPABILITY_DIGITAL);
-
-MicroBitMessageBus bus; 
-unsigned long StartEdge = 0;
-unsigned long Duration = 0;
-int SendPulseRequest = 0;
-int Triggered = 0;
-int TargetMotorRunTime = 0;
-int CurrentMotorRunTime = 0;
-unsigned int TimerStart = 0;
-unsigned int LastMotorVoltage_L = 0;
-unsigned int LastMotorVoltage_R = 0;
-unsigned int LastMotorDirection_L = 0;
-unsigned int LastMotorDirection_R = 0;
-int BuggyState = BUGGY_HALTED;
-
 
 void onButtonA(MicroBitEvent evt) 
 { 
     if(evt.value == MICROBIT_BUTTON_EVT_CLICK)
     {
-        SendPulseRequest = 1;
+
     }
 } 
 
-void onEchoHi(MicroBitEvent evt) 
-{ 
-    StartEdge = evt.timestamp; 
-    Triggered++;
-} 
 
-void onEchoLo(MicroBitEvent evt)
-{
-    Duration = evt.timestamp - StartEdge;
-    Triggered++;
-}
-
-void onEchoPulse(MicroBitEvent evt)
-{
-    Duration = evt.timestamp;
-    Triggered++;
-}
-
-bool hasLastCommandCompleted()
-{
-    if(BuggyState == BUGGY_RUNNING)
-    {
-        CurrentMotorRunTime = uBit.systemTime() - TimerStart;
-        if(CurrentMotorRunTime >= TargetMotorRunTime)
-        {
-            //Stop the motors by writing a 0 voltage and setting direction to forward.
-            MotorDirection_L.setDigitalValue(0);
-            MotorDirection_R.setDigitalValue(0);
-            MotorSpeed_L.setAnalogValue(0);  
-            MotorSpeed_R.setAnalogValue(0);
-            
-            //Update the buggy state to show that it is now halted
-            BuggyState = BUGGY_HALTED;
-            
-            //completed
-            return true;
-        }
-        else
-        {
-            //return not completed
-            return false;
-        }
-    }
-    
-    //return no last command was running
-    return false; 
-}
-
-void PauseLastCommand()
-{
-    //Only do this if the buggy is actually running
-    if(BuggyState == BUGGY_RUNNING)
-    {
-        //Store the amount of elapsed time before the command is paused.
-        CurrentMotorRunTime = uBit.systemTime() - TimerStart;
-        //Stop the motors by writing a 0 voltage and setting direction to forward.
-        MotorDirection_L.setDigitalValue(0);
-        MotorDirection_R.setDigitalValue(0);
-        MotorSpeed_L.setAnalogValue(0);  
-        MotorSpeed_R.setAnalogValue(0);
-        
-        //Update the buggy state to show that it is now halted
-        BuggyState = BUGGY_PAUSED;
-    }
-}
-
-void ContinueLastCommand()
-{
-    //Only do this if the buggy is currently halted we have not yet reached the target run time
-    if( (BuggyState == BUGGY_PAUSED) && (TargetMotorRunTime > CurrentMotorRunTime) )
-    {
-        //Update the target motor run time to only run for the incomplete time period and zero CurrentMotorRunTime
-        TargetMotorRunTime = TargetMotorRunTime - CurrentMotorRunTime;
-        CurrentMotorRunTime = 0;
-        //Set the voltage of the motors as per the stored last voltage and direction command.
-        MotorDirection_L.setDigitalValue(LastMotorDirection_L);
-        MotorDirection_R.setDigitalValue(LastMotorDirection_R);
-        MotorSpeed_L.setAnalogValue(LastMotorVoltage_L);  
-        MotorSpeed_R.setAnalogValue(LastMotorVoltage_R); 
-        
-        //Start the timer
-        TimerStart = uBit.systemTime();
-        
-        //Update the buggy state to show that it is now running
-        BuggyState = BUGGY_RUNNING;
-    }
-}
-
-void MoveForward(unsigned int Voltage, unsigned int Time_ms)
-{
-    //Initialise the variables for tracking the travel progress
-    TargetMotorRunTime = Time_ms;
-    CurrentMotorRunTime = 0;
-    
-    //Limit the voltage to 1024, which is the max A2D output value.
-    if(Voltage > 1024)
-    {
-        Voltage = 1024;
-    }
-    
-    //Set the direction of the motors to forward
-    MotorDirection_L.setDigitalValue(0);
-    MotorDirection_R.setDigitalValue(0);
-    //Set the voltage of the motors as per the user request 
-    MotorSpeed_L.setAnalogValue(Voltage);  
-    MotorSpeed_R.setAnalogValue(Voltage); 
-    
-    //Store the command being used in case the command has to be paused and resumed later
-    LastMotorVoltage_L = Voltage;
-    LastMotorVoltage_R = Voltage;
-    LastMotorDirection_L = 0;
-    LastMotorDirection_R = 0;
-    //Start the timer
-    TimerStart = uBit.systemTime();
-    
-    //Update the buggy state to show that it is now running
-    BuggyState = BUGGY_RUNNING;
-}
-
-void MoveBackward(unsigned int Voltage, unsigned int Time_ms)
-{
-    int ReverseVoltage;
-    //Initialise the variables for tracking the travel progress
-    TargetMotorRunTime = Time_ms;
-    CurrentMotorRunTime = 0;
-    
-    //Limit the voltage to 1024, which is the max A2D output value.
-    if(Voltage > 1024)
-    {
-        Voltage = 1024;
-    }
-    
-    //In reverse, 0 is actually max speed and 1024 is stopped.
-    ReverseVoltage = 1024 - Voltage;
-    
-    //Set the direction of the motors to reverse
-    MotorDirection_L.setDigitalValue(1);
-    MotorDirection_R.setDigitalValue(1);
-    //Set the voltage of the motors as per the user request (1024 - value) 
-    MotorSpeed_L.setAnalogValue(ReverseVoltage);  
-    MotorSpeed_R.setAnalogValue(ReverseVoltage); 
-    
-    //Store the command being used in case the command has to be paused and resumed later
-    LastMotorVoltage_L = ReverseVoltage;
-    LastMotorVoltage_R = ReverseVoltage;
-    LastMotorDirection_L = 1;
-    LastMotorDirection_R = 1;
-    //Start the timer
-    TimerStart = uBit.systemTime();
-    
-    //Update the buggy state to show that it is now running
-    BuggyState = BUGGY_RUNNING;
-}
-void RotateClockwise(unsigned int Voltage, unsigned int Time_ms)
-{
-    int ReverseVoltage;
-    //Initialise the variables for tracking the travel progress
-    TargetMotorRunTime = Time_ms;
-    CurrentMotorRunTime = 0;
-    
-    //Limit the voltage to 1024, which is the max A2D output value.
-    if(Voltage > 1024)
-    {
-        Voltage = 1024;
-    }
-    
-    //In reverse, 0 is actually max speed and 1024 is stopped.
-    ReverseVoltage = 1024 - Voltage;
-    
-    //Set the motor direction: left-forward, right-backward
-    MotorDirection_L.setDigitalValue(0);
-    MotorDirection_R.setDigitalValue(1);
-    //Set the voltage of the motors as per the user request (1024 - value) 
-    MotorSpeed_L.setAnalogValue(Voltage);  
-    MotorSpeed_R.setAnalogValue(ReverseVoltage); 
-    
-    //Store the command being used in case the command has to be paused and resumed later
-    LastMotorVoltage_L = Voltage;
-    LastMotorVoltage_R = ReverseVoltage;
-    LastMotorDirection_L = 0;
-    LastMotorDirection_R = 1;
-    //Start the timer
-    TimerStart = uBit.systemTime();
-    
-    //Update the buggy state to show that it is now running
-    BuggyState = BUGGY_RUNNING;
-}
-
-void RotateAnticlockwise(unsigned int Voltage, unsigned int Time_ms)
-{
-    int ReverseVoltage;
-    //Initialise the variables for tracking the travel progress
-    TargetMotorRunTime = Time_ms;
-    CurrentMotorRunTime = 0;
-    
-    //Limit the voltage to 1024, which is the max A2D output value.
-    if(Voltage > 1024)
-    {
-        Voltage = 1024;
-    }
-    
-    //In reverse, 0 is actually max speed and 1024 is stopped.
-    ReverseVoltage = 1024 - Voltage;
-    
-    //Set the motor direction: left-backward, right-forward
-    MotorDirection_L.setDigitalValue(1);
-    MotorDirection_R.setDigitalValue(0);
-    //Set the voltage of the motors as per the user request (1024 - value) 
-    MotorSpeed_L.setAnalogValue(ReverseVoltage);  
-    MotorSpeed_R.setAnalogValue(Voltage); 
-    
-    //Store the command being used in case the command has to be paused and resumed later
-    LastMotorVoltage_L = ReverseVoltage;
-    LastMotorVoltage_R = Voltage;
-    LastMotorDirection_L = 1;
-    LastMotorDirection_R = 0;
-    //Start the timer
-    TimerStart = uBit.systemTime();
-    
-    //Update the buggy state to show that it is now running
-    BuggyState = BUGGY_RUNNING;
-}
 
 
 int main()
-{
-    /*initialise local variables*/
-    unsigned long LastSystemTime = 0;   //To keep track of the clock
-    int EchoValue = 5;
-    int PressedCount = 0;
-    int ValueChange = 0;
-    
+{    
     //Initialise the micro:bit runtime.
-    uBit.init();
-    
-    //Display start message using the LEDs
-    uBit.display.scroll("H");
-    /*The sw MicroBitButtons instantiated above also have events assigned to them. This line will 
-    suppress MICROBIT_BUTTON_EVT_CLICK and MICROBIT_BUTTON_EVT_LONG_CLICK events on the button, otherwise
-    2 events will be flagged*/
-    buttonA.setEventConfiguration(MICROBIT_BUTTON_SIMPLE_EVENTS);
-    
-    //Listen out for the button A press event
-    uBit.messageBus.listen(MICROBIT_ID_BUTTON_A, MICROBIT_BUTTON_EVT_CLICK, onButtonA, MESSAGE_BUS_LISTENER_IMMEDIATE);
-    
-    //Set the triggerpin low
-   // Trig.setDigitalValue(0);
-    
-/*    Echo.eventOn(MICROBIT_PIN_EVENT_ON_PULSE);
-    Echo.eventOn(MICROBIT_PIN_EVENT_ON_EDGE);
-    uBit.messageBus.listen(MICROBIT_ID_IO_P13, MICROBIT_PIN_EVT_RISE, onEchoHi, MESSAGE_BUS_LISTENER_IMMEDIATE);
-    uBit.messageBus.listen(MICROBIT_ID_IO_P13, MICROBIT_PIN_EVT_FALL, onEchoLo, MESSAGE_BUS_LISTENER_IMMEDIATE);
-    uBit.messageBus.listen(MICROBIT_ID_IO_P13, MICROBIT_PIN_EVT_PULSE_HI, onEchoPulse, MESSAGE_BUS_LISTENER_IMMEDIATE);
- */   
-     
-    /*Because this pin is being used as both input and output, the configuring of the pin as an input
-    must be done first because once the output has changed, it will not work - possible uBit bug*/
-    EchoValue = Echo.getDigitalValue();
-    uBit.display.print(EchoValue);
+    InitialiseBuggy();
     
-    while(1)
-    {
-        MoveBackward(900, 5000);
-        do
-        {
-            if(buttonA.isPressed())
-            {
-                PauseLastCommand();
-            }
-            else
-            {
-                ContinueLastCommand();
-            }
-        }while( hasLastCommandCompleted() == false );
-        wait_ms(1000);
-        
-        MoveForward(900, 5000);
-        do
-        {
-            if(buttonA.isPressed())
-            {
-                PauseLastCommand();
-            }
-            else
-            {
-                ContinueLastCommand();
-            }
-        }while( hasLastCommandCompleted() == false );
-        wait_ms(1000);
-        
-        RotateClockwise(900, 5000);
-        do
-        {
-            if(buttonA.isPressed())
-            {
-                PauseLastCommand();
-            }
-            else
-            {
-                ContinueLastCommand();
-            }
-        }while( hasLastCommandCompleted() == false );
-        wait_ms(1000);
-        
-        RotateAnticlockwise(900, 5000);
-        do
-        {
-            if(buttonA.isPressed())
-            {
-                PauseLastCommand();
-            }
-            else
-            {
-                ContinueLastCommand();
-            }
-        }while( hasLastCommandCompleted() == false );
-        wait_ms(1000);
- 
- #ifdef   HVHVHVHV    
-        EchoValue = Echo.getDigitalValue();
-        if(EchoValue > ValueChange) 
-        {
-            ValueChange = EchoValue;    
-        }
-       /* if( (uBit.systemTime()-LastSystemTime) > 1000)
-        {    
-            //uBit.serial.printf("Duration=%d, ", Duration);
-            LastSystemTime = uBit.systemTime();
-        }*/
-        
-        if(SendPulseRequest == 1)
-        {
-            SendPulseRequest = 0;
-            Trig.setDigitalValue(1);
-            wait_us(50);
-            Trig.setDigitalValue(0);
-            //uBit.display.printAsync(++PressedCount);
-        }
-        if(ValueChange>0)
-        {
-            uBit.display.printAsync(ValueChange);
-        }
-#endif
-    }
+    //Run self test forever
+    //SelfTest();
+    //Print the sonar time
+    DisplaySonarTiming();
+    
 
     // If main exits, there may still be other fibers running or registered event handlers etc.
     // Simply release this fiber, which will mean we enter the scheduler. Worse case, we then
@@ -428,3 +63,47 @@
     release_fiber();
 }
 
+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();
+    }
+    
+}