Andrew Oyaide / StudentBuggyProject

Starting point for the student buggy project

Dependencies:   microbit

Fork of microbit-hello-world by micro:bit

Buggy.cpp@2:47b7a55b0805, 2017-07-31 (annotated)

Committer:
OyaideA
Date:
Mon Jul 31 21:48:36 2017 +0000
Revision:
2:47b7a55b0805
Child:
4:2d939ef2b09c
Project includes self test and code for ultrasonic characterisation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OyaideA 2:47b7a55b0805 1 /***********************************************************************
OyaideA 2:47b7a55b0805 2 This file contains the different function calls that can be used to control
OyaideA 2:47b7a55b0805 3 the buggy and read and control the ultrasonic sensor on the buggy
OyaideA 2:47b7a55b0805 4 *************************************************************************/
OyaideA 2:47b7a55b0805 5 /************
OyaideA 2:47b7a55b0805 6 Includes
OyaideA 2:47b7a55b0805 7 **************/
OyaideA 2:47b7a55b0805 8 #include "Buggy.h"
OyaideA 2:47b7a55b0805 9 #include <stdio.h>
OyaideA 2:47b7a55b0805 10
OyaideA 2:47b7a55b0805 11 /******************
OyaideA 2:47b7a55b0805 12 Definition of constants
OyaideA 2:47b7a55b0805 13 ******************/
OyaideA 2:47b7a55b0805 14 #define BUGGY_HALTED 0
OyaideA 2:47b7a55b0805 15 #define BUGGY_RUNNING 1
OyaideA 2:47b7a55b0805 16 #define BUGGY_PAUSED 2
OyaideA 2:47b7a55b0805 17
OyaideA 2:47b7a55b0805 18 /*******************************
OyaideA 2:47b7a55b0805 19 Local function declarations
OyaideA 2:47b7a55b0805 20 *******************************/
OyaideA 2:47b7a55b0805 21 void onSonarEchoPulse(MicroBitEvent evt);
OyaideA 2:47b7a55b0805 22 void MoveForward(unsigned int Voltage, unsigned int Time_ms);
OyaideA 2:47b7a55b0805 23 void MoveBackward(unsigned int Voltage, unsigned int Time_ms);
OyaideA 2:47b7a55b0805 24 void RotateClockwise(unsigned int Voltage, unsigned int Time_ms);
OyaideA 2:47b7a55b0805 25 void RotateAnticlockwise(unsigned int Voltage, unsigned int Time_ms);
OyaideA 2:47b7a55b0805 26 void SendSonarTrigger();
OyaideA 2:47b7a55b0805 27
OyaideA 2:47b7a55b0805 28 /*******************************
OyaideA 2:47b7a55b0805 29 Global variables for the microbit runtime
OyaideA 2:47b7a55b0805 30 ******************************/
OyaideA 2:47b7a55b0805 31 MicroBit uBit;
OyaideA 2:47b7a55b0805 32 MicroBitPin MotorSpeed_L(MICROBIT_ID_IO_P0, MICROBIT_PIN_P0, PIN_CAPABILITY_ANALOG);
OyaideA 2:47b7a55b0805 33 MicroBitPin MotorDirection_L(MICROBIT_ID_IO_P8, MICROBIT_PIN_P8, PIN_CAPABILITY_DIGITAL );
OyaideA 2:47b7a55b0805 34 MicroBitPin MotorSpeed_R(MICROBIT_ID_IO_P1, MICROBIT_PIN_P1, PIN_CAPABILITY_ANALOG);
OyaideA 2:47b7a55b0805 35 MicroBitPin MotorDirection_R(MICROBIT_ID_IO_P12, MICROBIT_PIN_P12, PIN_CAPABILITY_DIGITAL );
OyaideA 2:47b7a55b0805 36 //Note: The buggy has both the trigger and echo signals on the same microbit pin
OyaideA 2:47b7a55b0805 37 MicroBitPin Sonar(MICROBIT_ID_IO_P13, MICROBIT_PIN_P13, PIN_CAPABILITY_DIGITAL);
OyaideA 2:47b7a55b0805 38
OyaideA 2:47b7a55b0805 39
OyaideA 2:47b7a55b0805 40 /**********************************
OyaideA 2:47b7a55b0805 41 Global variables for the buggy interface
OyaideA 2:47b7a55b0805 42 *************************************/
OyaideA 2:47b7a55b0805 43 unsigned long SonarReturnPulseWidth = 0xFFFFFFFF;
OyaideA 2:47b7a55b0805 44 int TargetMotorRunTime = 0;
OyaideA 2:47b7a55b0805 45 int CurrentMotorRunTime = 0;
OyaideA 2:47b7a55b0805 46 unsigned int TimerStart = 0;
OyaideA 2:47b7a55b0805 47 unsigned int LastMotorVoltage_L = 0;
OyaideA 2:47b7a55b0805 48 unsigned int LastMotorVoltage_R = 0;
OyaideA 2:47b7a55b0805 49 unsigned int LastMotorDirection_L = 0;
OyaideA 2:47b7a55b0805 50 unsigned int LastMotorDirection_R = 0;
OyaideA 2:47b7a55b0805 51 int BuggyState = BUGGY_HALTED;
OyaideA 2:47b7a55b0805 52 Ticker SonarTriggerTimer;
OyaideA 2:47b7a55b0805 53
OyaideA 2:47b7a55b0805 54 /*****************************************************************************
OyaideA 2:47b7a55b0805 55 Start of function definitions
OyaideA 2:47b7a55b0805 56 *****************************************************************************/
OyaideA 2:47b7a55b0805 57 void InitialiseBuggy()
OyaideA 2:47b7a55b0805 58 {
OyaideA 2:47b7a55b0805 59 //Initialise the micro:bit runtime.
OyaideA 2:47b7a55b0805 60 uBit.init();
OyaideA 2:47b7a55b0805 61
OyaideA 2:47b7a55b0805 62 //setup the message bus to listen for events from the ultrasonic sensor input
OyaideA 2:47b7a55b0805 63 uBit.messageBus.listen(MICROBIT_ID_IO_P13, MICROBIT_PIN_EVT_PULSE_HI, onSonarEchoPulse, MESSAGE_BUS_LISTENER_IMMEDIATE);
OyaideA 2:47b7a55b0805 64
OyaideA 2:47b7a55b0805 65 //Attach the SendSonarTrigger function to the timer and configure for it to trigger every 0.2 secs.
OyaideA 2:47b7a55b0805 66 SonarTriggerTimer.attach(&SendSonarTrigger, 0.2);
OyaideA 2:47b7a55b0805 67 }
OyaideA 2:47b7a55b0805 68
OyaideA 2:47b7a55b0805 69 void SendSonarTrigger()
OyaideA 2:47b7a55b0805 70 {
OyaideA 2:47b7a55b0805 71 //Turn the monitoring of the event off whilst the trigger pulse is sent.
OyaideA 2:47b7a55b0805 72 Sonar.eventOn(MICROBIT_PIN_EVENT_NONE);
OyaideA 2:47b7a55b0805 73 /*Set the trigger to high for 10 microseconds, then back to a low. This
OyaideA 2:47b7a55b0805 74 setDigitalValue function also changes the pin to an output pin*/
OyaideA 2:47b7a55b0805 75 Sonar.setDigitalValue(1);
OyaideA 2:47b7a55b0805 76 wait_us(10);
OyaideA 2:47b7a55b0805 77 Sonar.setDigitalValue(0);
OyaideA 2:47b7a55b0805 78
OyaideA 2:47b7a55b0805 79 //Call this function to turn the sonar pin back to an input pin to await the echo.
OyaideA 2:47b7a55b0805 80 Sonar.getDigitalValue();
OyaideA 2:47b7a55b0805 81 //Turn the event back on so that this function responds again
OyaideA 2:47b7a55b0805 82 Sonar.eventOn(MICROBIT_PIN_EVENT_ON_PULSE);
OyaideA 2:47b7a55b0805 83
OyaideA 2:47b7a55b0805 84 return;
OyaideA 2:47b7a55b0805 85
OyaideA 2:47b7a55b0805 86 }
OyaideA 2:47b7a55b0805 87
OyaideA 2:47b7a55b0805 88 void onSonarEchoPulse(MicroBitEvent evt)
OyaideA 2:47b7a55b0805 89 {
OyaideA 2:47b7a55b0805 90 //Read the pulse wdith of the returned echo. This is returned in the timestamp of the event.
OyaideA 2:47b7a55b0805 91 SonarReturnPulseWidth = evt.timestamp;
OyaideA 2:47b7a55b0805 92
OyaideA 2:47b7a55b0805 93 return;
OyaideA 2:47b7a55b0805 94 }
OyaideA 2:47b7a55b0805 95
OyaideA 2:47b7a55b0805 96 unsigned int GetSonarTime_us()
OyaideA 2:47b7a55b0805 97 {
OyaideA 2:47b7a55b0805 98 return (unsigned int)SonarReturnPulseWidth;
OyaideA 2:47b7a55b0805 99 }
OyaideA 2:47b7a55b0805 100
OyaideA 2:47b7a55b0805 101 bool hasLastCommandCompleted()
OyaideA 2:47b7a55b0805 102 {
OyaideA 2:47b7a55b0805 103 if(BuggyState == BUGGY_RUNNING)
OyaideA 2:47b7a55b0805 104 {
OyaideA 2:47b7a55b0805 105 CurrentMotorRunTime = uBit.systemTime() - TimerStart;
OyaideA 2:47b7a55b0805 106 if(CurrentMotorRunTime >= TargetMotorRunTime)
OyaideA 2:47b7a55b0805 107 {
OyaideA 2:47b7a55b0805 108 //Stop the motors by writing a 0 voltage and setting direction to forward.
OyaideA 2:47b7a55b0805 109 MotorDirection_L.setDigitalValue(0);
OyaideA 2:47b7a55b0805 110 MotorDirection_R.setDigitalValue(0);
OyaideA 2:47b7a55b0805 111 MotorSpeed_L.setAnalogValue(0);
OyaideA 2:47b7a55b0805 112 MotorSpeed_R.setAnalogValue(0);
OyaideA 2:47b7a55b0805 113
OyaideA 2:47b7a55b0805 114 //Update the buggy state to show that it is now halted
OyaideA 2:47b7a55b0805 115 BuggyState = BUGGY_HALTED;
OyaideA 2:47b7a55b0805 116
OyaideA 2:47b7a55b0805 117 //completed
OyaideA 2:47b7a55b0805 118 return true;
OyaideA 2:47b7a55b0805 119 }
OyaideA 2:47b7a55b0805 120 else
OyaideA 2:47b7a55b0805 121 {
OyaideA 2:47b7a55b0805 122 //return not completed
OyaideA 2:47b7a55b0805 123 return false;
OyaideA 2:47b7a55b0805 124 }
OyaideA 2:47b7a55b0805 125 }
OyaideA 2:47b7a55b0805 126
OyaideA 2:47b7a55b0805 127 //return no last command was running
OyaideA 2:47b7a55b0805 128 return false;
OyaideA 2:47b7a55b0805 129 }
OyaideA 2:47b7a55b0805 130
OyaideA 2:47b7a55b0805 131 void PauseLastCommand()
OyaideA 2:47b7a55b0805 132 {
OyaideA 2:47b7a55b0805 133 //Only do this if the buggy is actually running
OyaideA 2:47b7a55b0805 134 if(BuggyState == BUGGY_RUNNING)
OyaideA 2:47b7a55b0805 135 {
OyaideA 2:47b7a55b0805 136 //Store the amount of elapsed time before the command is paused.
OyaideA 2:47b7a55b0805 137 CurrentMotorRunTime = uBit.systemTime() - TimerStart;
OyaideA 2:47b7a55b0805 138 //Stop the motors by writing a 0 voltage and setting direction to forward.
OyaideA 2:47b7a55b0805 139 MotorDirection_L.setDigitalValue(0);
OyaideA 2:47b7a55b0805 140 MotorDirection_R.setDigitalValue(0);
OyaideA 2:47b7a55b0805 141 MotorSpeed_L.setAnalogValue(0);
OyaideA 2:47b7a55b0805 142 MotorSpeed_R.setAnalogValue(0);
OyaideA 2:47b7a55b0805 143
OyaideA 2:47b7a55b0805 144 //Update the buggy state to show that it is now halted
OyaideA 2:47b7a55b0805 145 BuggyState = BUGGY_PAUSED;
OyaideA 2:47b7a55b0805 146 }
OyaideA 2:47b7a55b0805 147 }
OyaideA 2:47b7a55b0805 148
OyaideA 2:47b7a55b0805 149 void ContinueLastCommand()
OyaideA 2:47b7a55b0805 150 {
OyaideA 2:47b7a55b0805 151 //Only do this if the buggy is currently halted we have not yet reached the target run time
OyaideA 2:47b7a55b0805 152 if( (BuggyState == BUGGY_PAUSED) && (TargetMotorRunTime > CurrentMotorRunTime) )
OyaideA 2:47b7a55b0805 153 {
OyaideA 2:47b7a55b0805 154 //Update the target motor run time to only run for the incomplete time period and zero CurrentMotorRunTime
OyaideA 2:47b7a55b0805 155 TargetMotorRunTime = TargetMotorRunTime - CurrentMotorRunTime;
OyaideA 2:47b7a55b0805 156 CurrentMotorRunTime = 0;
OyaideA 2:47b7a55b0805 157 //Set the voltage of the motors as per the stored last voltage and direction command.
OyaideA 2:47b7a55b0805 158 MotorDirection_L.setDigitalValue(LastMotorDirection_L);
OyaideA 2:47b7a55b0805 159 MotorDirection_R.setDigitalValue(LastMotorDirection_R);
OyaideA 2:47b7a55b0805 160 MotorSpeed_L.setAnalogValue(LastMotorVoltage_L);
OyaideA 2:47b7a55b0805 161 MotorSpeed_R.setAnalogValue(LastMotorVoltage_R);
OyaideA 2:47b7a55b0805 162
OyaideA 2:47b7a55b0805 163 //Start the timer
OyaideA 2:47b7a55b0805 164 TimerStart = uBit.systemTime();
OyaideA 2:47b7a55b0805 165
OyaideA 2:47b7a55b0805 166 //Update the buggy state to show that it is now running
OyaideA 2:47b7a55b0805 167 BuggyState = BUGGY_RUNNING;
OyaideA 2:47b7a55b0805 168 }
OyaideA 2:47b7a55b0805 169 }
OyaideA 2:47b7a55b0805 170
OyaideA 2:47b7a55b0805 171 void MoveBuggy(int Command, unsigned int Voltage, unsigned int Time_ms)
OyaideA 2:47b7a55b0805 172 {
OyaideA 2:47b7a55b0805 173 //Initialise the variables for tracking the travel progress
OyaideA 2:47b7a55b0805 174 TargetMotorRunTime = Time_ms;
OyaideA 2:47b7a55b0805 175 CurrentMotorRunTime = 0;
OyaideA 2:47b7a55b0805 176
OyaideA 2:47b7a55b0805 177 //Limit the voltage to 1024, which is the max A2D output value.
OyaideA 2:47b7a55b0805 178 if(Voltage > 1024)
OyaideA 2:47b7a55b0805 179 {
OyaideA 2:47b7a55b0805 180 Voltage = 1024;
OyaideA 2:47b7a55b0805 181 }
OyaideA 2:47b7a55b0805 182
OyaideA 2:47b7a55b0805 183 switch(Command)
OyaideA 2:47b7a55b0805 184 {
OyaideA 2:47b7a55b0805 185 case MOVE_FORWARD:
OyaideA 2:47b7a55b0805 186 //Set the motor voltage as requested by the user
OyaideA 2:47b7a55b0805 187 LastMotorVoltage_L = Voltage;
OyaideA 2:47b7a55b0805 188 LastMotorVoltage_R = Voltage;
OyaideA 2:47b7a55b0805 189 //Set motor direction to forward
OyaideA 2:47b7a55b0805 190 LastMotorDirection_L = 0;
OyaideA 2:47b7a55b0805 191 LastMotorDirection_R = 0;
OyaideA 2:47b7a55b0805 192 break;
OyaideA 2:47b7a55b0805 193
OyaideA 2:47b7a55b0805 194 case MOVE_BACKWARD:
OyaideA 2:47b7a55b0805 195 /*Set the voltage of the motors as per the user request (1024 - value)
OyaideA 2:47b7a55b0805 196 In reverse, 0 is actually max speed and 1024 is stopped.*/
OyaideA 2:47b7a55b0805 197 LastMotorVoltage_L = 1024 - Voltage;
OyaideA 2:47b7a55b0805 198 LastMotorVoltage_R = 1024 - Voltage;
OyaideA 2:47b7a55b0805 199 //Set the motor direction to reverse
OyaideA 2:47b7a55b0805 200 LastMotorDirection_L = 1;
OyaideA 2:47b7a55b0805 201 LastMotorDirection_R = 1;
OyaideA 2:47b7a55b0805 202 break;
OyaideA 2:47b7a55b0805 203
OyaideA 2:47b7a55b0805 204 case ROTATE_CLOCKWISE:
OyaideA 2:47b7a55b0805 205 /*Set the voltage of the motors as per the user request (1024 - value)
OyaideA 2:47b7a55b0805 206 In reverse, 0 is actually max speed and 1024 is stopped.*/
OyaideA 2:47b7a55b0805 207 LastMotorVoltage_L = Voltage;
OyaideA 2:47b7a55b0805 208 LastMotorVoltage_R = 1024 - Voltage;
OyaideA 2:47b7a55b0805 209 //Set the direction to left wheel forwards and right wheel backward
OyaideA 2:47b7a55b0805 210 LastMotorDirection_L = 0;
OyaideA 2:47b7a55b0805 211 LastMotorDirection_R = 1;
OyaideA 2:47b7a55b0805 212 break;
OyaideA 2:47b7a55b0805 213
OyaideA 2:47b7a55b0805 214 case ROTATE_ANTICLOCKWISE:
OyaideA 2:47b7a55b0805 215 /*Set the voltage of the motors as per the user request (1024 - value)
OyaideA 2:47b7a55b0805 216 In reverse, 0 is actually max speed and 1024 is stopped.*/
OyaideA 2:47b7a55b0805 217 LastMotorVoltage_L = 1024 - Voltage;
OyaideA 2:47b7a55b0805 218 LastMotorVoltage_R = Voltage;
OyaideA 2:47b7a55b0805 219 //Set the direction to left wheel backwards and right wheel forward
OyaideA 2:47b7a55b0805 220 LastMotorDirection_L = 1;
OyaideA 2:47b7a55b0805 221 LastMotorDirection_R = 0;
OyaideA 2:47b7a55b0805 222 break;
OyaideA 2:47b7a55b0805 223
OyaideA 2:47b7a55b0805 224 default:
OyaideA 2:47b7a55b0805 225
OyaideA 2:47b7a55b0805 226 break;
OyaideA 2:47b7a55b0805 227 }
OyaideA 2:47b7a55b0805 228
OyaideA 2:47b7a55b0805 229 //Set the direction of the motors as per the command
OyaideA 2:47b7a55b0805 230 MotorDirection_L.setDigitalValue(LastMotorDirection_L);
OyaideA 2:47b7a55b0805 231 MotorDirection_R.setDigitalValue(LastMotorDirection_R);
OyaideA 2:47b7a55b0805 232 wait_ms(1);
OyaideA 2:47b7a55b0805 233 //Set the voltage of the motors as per the user request
OyaideA 2:47b7a55b0805 234 MotorSpeed_L.setAnalogValue(LastMotorVoltage_L);
OyaideA 2:47b7a55b0805 235 MotorSpeed_R.setAnalogValue(LastMotorVoltage_R);
OyaideA 2:47b7a55b0805 236
OyaideA 2:47b7a55b0805 237 //Start the timer
OyaideA 2:47b7a55b0805 238 TimerStart = uBit.systemTime();
OyaideA 2:47b7a55b0805 239
OyaideA 2:47b7a55b0805 240 //Update the buggy state to show that it is now running
OyaideA 2:47b7a55b0805 241 BuggyState = BUGGY_RUNNING;
OyaideA 2:47b7a55b0805 242
OyaideA 2:47b7a55b0805 243 return;
OyaideA 2:47b7a55b0805 244 }
OyaideA 2:47b7a55b0805 245
OyaideA 2:47b7a55b0805 246
OyaideA 2:47b7a55b0805 247 void RunBasicBuggyMotorTest(unsigned int Voltage, unsigned int Time_ms, unsigned int Pause_ms)
OyaideA 2:47b7a55b0805 248 {
OyaideA 2:47b7a55b0805 249 //Move the buggy forward
OyaideA 2:47b7a55b0805 250 MoveBuggy(MOVE_FORWARD, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 251 //wait and check if the command has completed
OyaideA 2:47b7a55b0805 252 do
OyaideA 2:47b7a55b0805 253 {
OyaideA 2:47b7a55b0805 254 //sleep whilst we wait for the command to complete
OyaideA 2:47b7a55b0805 255 uBit.sleep(Time_ms);
OyaideA 2:47b7a55b0805 256 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 257 //Pause before doing the next command
OyaideA 2:47b7a55b0805 258 wait_ms(Pause_ms);
OyaideA 2:47b7a55b0805 259
OyaideA 2:47b7a55b0805 260 //Move the buggy bacward
OyaideA 2:47b7a55b0805 261 MoveBuggy(MOVE_BACKWARD, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 262 //wait and check if the command has completed
OyaideA 2:47b7a55b0805 263 do
OyaideA 2:47b7a55b0805 264 {
OyaideA 2:47b7a55b0805 265 //sleep whilst we wait for the command to complete
OyaideA 2:47b7a55b0805 266 uBit.sleep(Time_ms);
OyaideA 2:47b7a55b0805 267 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 268 //Pause before doing the next command
OyaideA 2:47b7a55b0805 269 wait_ms(Pause_ms);
OyaideA 2:47b7a55b0805 270
OyaideA 2:47b7a55b0805 271 //Rotate the buggy clockwise
OyaideA 2:47b7a55b0805 272 MoveBuggy(ROTATE_CLOCKWISE, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 273 //wait and check if the command has completed
OyaideA 2:47b7a55b0805 274 do
OyaideA 2:47b7a55b0805 275 {
OyaideA 2:47b7a55b0805 276 //sleep whilst we wait for the command to complete
OyaideA 2:47b7a55b0805 277 uBit.sleep(Time_ms);
OyaideA 2:47b7a55b0805 278 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 279 //Pause before doing the next command
OyaideA 2:47b7a55b0805 280 wait_ms(Pause_ms);
OyaideA 2:47b7a55b0805 281
OyaideA 2:47b7a55b0805 282 //Rotate the buggy anticloclwise
OyaideA 2:47b7a55b0805 283 MoveBuggy(ROTATE_ANTICLOCKWISE, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 284 //wait and check if the command has completed
OyaideA 2:47b7a55b0805 285 do
OyaideA 2:47b7a55b0805 286 {
OyaideA 2:47b7a55b0805 287 //sleep whilst we wait for the command to complete
OyaideA 2:47b7a55b0805 288 uBit.sleep(Time_ms);
OyaideA 2:47b7a55b0805 289 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 290 //Pause before doing the next command
OyaideA 2:47b7a55b0805 291 wait_ms(Pause_ms);
OyaideA 2:47b7a55b0805 292
OyaideA 2:47b7a55b0805 293 return;
OyaideA 2:47b7a55b0805 294 }
OyaideA 2:47b7a55b0805 295
OyaideA 2:47b7a55b0805 296 void TestAntiCollision(unsigned int Voltage, unsigned int Time_ms, unsigned int SonarTime_us)
OyaideA 2:47b7a55b0805 297 {
OyaideA 2:47b7a55b0805 298 MoveBuggy(MOVE_FORWARD, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 299 do
OyaideA 2:47b7a55b0805 300 {
OyaideA 2:47b7a55b0805 301 if(GetSonarTime_us() < SonarTime_us)
OyaideA 2:47b7a55b0805 302 {
OyaideA 2:47b7a55b0805 303 PauseLastCommand();
OyaideA 2:47b7a55b0805 304 }
OyaideA 2:47b7a55b0805 305 else
OyaideA 2:47b7a55b0805 306 {
OyaideA 2:47b7a55b0805 307 ContinueLastCommand();
OyaideA 2:47b7a55b0805 308 }
OyaideA 2:47b7a55b0805 309 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 310
OyaideA 2:47b7a55b0805 311 return;
OyaideA 2:47b7a55b0805 312
OyaideA 2:47b7a55b0805 313 }
OyaideA 2:47b7a55b0805 314
OyaideA 2:47b7a55b0805 315 void TestSonar()
OyaideA 2:47b7a55b0805 316 {
OyaideA 2:47b7a55b0805 317 //Local variables
OyaideA 2:47b7a55b0805 318 MicroBitImage SonarImage;
OyaideA 2:47b7a55b0805 319 unsigned int SonarDistance;
OyaideA 2:47b7a55b0805 320 int NumPixels, x, y;
OyaideA 2:47b7a55b0805 321 int MaxDisplayDist = 100;
OyaideA 2:47b7a55b0805 322 int WholeRows, RemainderColumns;
OyaideA 2:47b7a55b0805 323
OyaideA 2:47b7a55b0805 324 //Compute the distance in cm. the 58 is taken from the datasheet
OyaideA 2:47b7a55b0805 325 SonarDistance = GetSonarTime_us()/58;
OyaideA 2:47b7a55b0805 326 //limit to 1m
OyaideA 2:47b7a55b0805 327 if(SonarDistance > MaxDisplayDist)
OyaideA 2:47b7a55b0805 328 {
OyaideA 2:47b7a55b0805 329 SonarDistance = MaxDisplayDist;
OyaideA 2:47b7a55b0805 330 }
OyaideA 2:47b7a55b0805 331
OyaideA 2:47b7a55b0805 332 //Convert the distance to the number of pixels to light
OyaideA 2:47b7a55b0805 333 NumPixels = (SonarDistance*25)/MaxDisplayDist;
OyaideA 2:47b7a55b0805 334 //Convert into the number of whole pixel rows and remainder columns to light
OyaideA 2:47b7a55b0805 335 WholeRows = NumPixels/5;
OyaideA 2:47b7a55b0805 336 RemainderColumns = NumPixels%5;
OyaideA 2:47b7a55b0805 337
OyaideA 2:47b7a55b0805 338 //First fill the whole rows
OyaideA 2:47b7a55b0805 339 for(y=0; y<WholeRows; y++)
OyaideA 2:47b7a55b0805 340 {
OyaideA 2:47b7a55b0805 341 for(x=0; x<5; x++)
OyaideA 2:47b7a55b0805 342 {
OyaideA 2:47b7a55b0805 343 uBit.display.image.setPixelValue(x, y, 200);
OyaideA 2:47b7a55b0805 344 }
OyaideA 2:47b7a55b0805 345 }
OyaideA 2:47b7a55b0805 346
OyaideA 2:47b7a55b0805 347 //fill the partial row
OyaideA 2:47b7a55b0805 348 if(WholeRows < 5)
OyaideA 2:47b7a55b0805 349 {
OyaideA 2:47b7a55b0805 350 for(x=0; x<RemainderColumns; x++)
OyaideA 2:47b7a55b0805 351 {
OyaideA 2:47b7a55b0805 352 uBit.display.image.setPixelValue(x, y, 200);
OyaideA 2:47b7a55b0805 353 }
OyaideA 2:47b7a55b0805 354 }
OyaideA 2:47b7a55b0805 355
OyaideA 2:47b7a55b0805 356 //Fill the remaining pixels in the partial row with 0
OyaideA 2:47b7a55b0805 357 for( ; x<5; x++)
OyaideA 2:47b7a55b0805 358 {
OyaideA 2:47b7a55b0805 359 uBit.display.image.setPixelValue(x, y, 0);
OyaideA 2:47b7a55b0805 360 }
OyaideA 2:47b7a55b0805 361 //Continue from the next row
OyaideA 2:47b7a55b0805 362 y++;
OyaideA 2:47b7a55b0805 363 for( ; y<5; y++)
OyaideA 2:47b7a55b0805 364 {
OyaideA 2:47b7a55b0805 365 for(x=0; x<5; x++)
OyaideA 2:47b7a55b0805 366 {
OyaideA 2:47b7a55b0805 367 uBit.display.image.setPixelValue(x, y, 0);
OyaideA 2:47b7a55b0805 368 }
OyaideA 2:47b7a55b0805 369 }
OyaideA 2:47b7a55b0805 370
OyaideA 2:47b7a55b0805 371 return;
OyaideA 2:47b7a55b0805 372 }
OyaideA 2:47b7a55b0805 373
OyaideA 2:47b7a55b0805 374 void PrintSonarTiming(void)
OyaideA 2:47b7a55b0805 375 {
OyaideA 2:47b7a55b0805 376 //Local variables
OyaideA 2:47b7a55b0805 377 int SonarTime;
OyaideA 2:47b7a55b0805 378
OyaideA 2:47b7a55b0805 379 //read the latest sonar time
OyaideA 2:47b7a55b0805 380 SonarTime = GetSonarTime_us();
OyaideA 2:47b7a55b0805 381 //Print the time in micro secs
OyaideA 2:47b7a55b0805 382 uBit.display.printAsync(SonarTime);
OyaideA 2:47b7a55b0805 383 uBit.serial.printf("Time = %d us\n\r", SonarTime);
OyaideA 2:47b7a55b0805 384 }