Andrew Oyaide / StudentBuggyProject

Starting point for the student buggy project

Dependencies:   microbit

Fork of microbit-hello-world by micro:bit

Buggy.cpp@6:edbaeaaf08bb, 2017-08-12 (annotated)

Committer:
OyaideA
Date:
Sat Aug 12 21:46:41 2017 +0000
Revision:
6:edbaeaaf08bb
Parent:
5:a33f016d5962 
Updated the comments in Buggy.h to explain what each function does

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 4:2d939ef2b09c 27 void onButtonA(MicroBitEvent);
OyaideA 4:2d939ef2b09c 28 void onButtonB(MicroBitEvent);
OyaideA 5:a33f016d5962 29 void PrintSonarTiming(void);
OyaideA 5:a33f016d5962 30 void RunBasicBuggyMotorTest(unsigned int Voltage, unsigned int Time_ms, unsigned int Pause_ms);
OyaideA 5:a33f016d5962 31 void TestSonar();
OyaideA 2:47b7a55b0805 32
OyaideA 2:47b7a55b0805 33 /*******************************
OyaideA 2:47b7a55b0805 34 Global variables for the microbit runtime
OyaideA 2:47b7a55b0805 35 ******************************/
OyaideA 2:47b7a55b0805 36 MicroBit uBit;
OyaideA 2:47b7a55b0805 37 MicroBitPin MotorSpeed_L(MICROBIT_ID_IO_P0, MICROBIT_PIN_P0, PIN_CAPABILITY_ANALOG);
OyaideA 2:47b7a55b0805 38 MicroBitPin MotorDirection_L(MICROBIT_ID_IO_P8, MICROBIT_PIN_P8, PIN_CAPABILITY_DIGITAL );
OyaideA 2:47b7a55b0805 39 MicroBitPin MotorSpeed_R(MICROBIT_ID_IO_P1, MICROBIT_PIN_P1, PIN_CAPABILITY_ANALOG);
OyaideA 2:47b7a55b0805 40 MicroBitPin MotorDirection_R(MICROBIT_ID_IO_P12, MICROBIT_PIN_P12, PIN_CAPABILITY_DIGITAL );
OyaideA 2:47b7a55b0805 41 //Note: The buggy has both the trigger and echo signals on the same microbit pin
OyaideA 2:47b7a55b0805 42 MicroBitPin Sonar(MICROBIT_ID_IO_P13, MICROBIT_PIN_P13, PIN_CAPABILITY_DIGITAL);
OyaideA 4:2d939ef2b09c 43 MicroBitButton buttonA(MICROBIT_PIN_BUTTON_A, MICROBIT_ID_BUTTON_A);
OyaideA 4:2d939ef2b09c 44 MicroBitButton buttonB(MICROBIT_PIN_BUTTON_B, MICROBIT_ID_BUTTON_B);
OyaideA 2:47b7a55b0805 45
OyaideA 2:47b7a55b0805 46
OyaideA 2:47b7a55b0805 47 /**********************************
OyaideA 2:47b7a55b0805 48 Global variables for the buggy interface
OyaideA 2:47b7a55b0805 49 *************************************/
OyaideA 2:47b7a55b0805 50 unsigned long SonarReturnPulseWidth = 0xFFFFFFFF;
OyaideA 2:47b7a55b0805 51 int TargetMotorRunTime = 0;
OyaideA 2:47b7a55b0805 52 int CurrentMotorRunTime = 0;
OyaideA 2:47b7a55b0805 53 unsigned int TimerStart = 0;
OyaideA 2:47b7a55b0805 54 unsigned int LastMotorVoltage_L = 0;
OyaideA 2:47b7a55b0805 55 unsigned int LastMotorVoltage_R = 0;
OyaideA 2:47b7a55b0805 56 unsigned int LastMotorDirection_L = 0;
OyaideA 2:47b7a55b0805 57 unsigned int LastMotorDirection_R = 0;
OyaideA 2:47b7a55b0805 58 int BuggyState = BUGGY_HALTED;
OyaideA 2:47b7a55b0805 59 Ticker SonarTriggerTimer;
OyaideA 4:2d939ef2b09c 60 int MotorTestIndex = 0;
OyaideA 4:2d939ef2b09c 61 bool MotorTestGo = false;
OyaideA 2:47b7a55b0805 62
OyaideA 2:47b7a55b0805 63 /*****************************************************************************
OyaideA 2:47b7a55b0805 64 Start of function definitions
OyaideA 2:47b7a55b0805 65 *****************************************************************************/
OyaideA 2:47b7a55b0805 66 void InitialiseBuggy()
OyaideA 2:47b7a55b0805 67 {
OyaideA 2:47b7a55b0805 68 //Initialise the micro:bit runtime.
OyaideA 2:47b7a55b0805 69 uBit.init();
OyaideA 2:47b7a55b0805 70
OyaideA 2:47b7a55b0805 71 //setup the message bus to listen for events from the ultrasonic sensor input
OyaideA 2:47b7a55b0805 72 uBit.messageBus.listen(MICROBIT_ID_IO_P13, MICROBIT_PIN_EVT_PULSE_HI, onSonarEchoPulse, MESSAGE_BUS_LISTENER_IMMEDIATE);
OyaideA 4:2d939ef2b09c 73 /*The sw MicroBitButtons instantiated above also have events assigned to them. This line will
OyaideA 4:2d939ef2b09c 74 suppress MICROBIT_BUTTON_EVT_CLICK and MICROBIT_BUTTON_EVT_LONG_CLICK events on the button, otherwise
OyaideA 4:2d939ef2b09c 75 2 events will be flagged*/
OyaideA 4:2d939ef2b09c 76 buttonA.setEventConfiguration(MICROBIT_BUTTON_SIMPLE_EVENTS);
OyaideA 4:2d939ef2b09c 77 buttonB.setEventConfiguration(MICROBIT_BUTTON_SIMPLE_EVENTS);
OyaideA 4:2d939ef2b09c 78 //Listen out for the button A press event
OyaideA 4:2d939ef2b09c 79 uBit.messageBus.listen(MICROBIT_ID_BUTTON_A, MICROBIT_BUTTON_EVT_CLICK, onButtonA, MESSAGE_BUS_LISTENER_IMMEDIATE);
OyaideA 4:2d939ef2b09c 80 uBit.messageBus.listen(MICROBIT_ID_BUTTON_B, MICROBIT_BUTTON_EVT_CLICK, onButtonB, MESSAGE_BUS_LISTENER_IMMEDIATE);
OyaideA 4:2d939ef2b09c 81
OyaideA 2:47b7a55b0805 82 //Attach the SendSonarTrigger function to the timer and configure for it to trigger every 0.2 secs.
OyaideA 2:47b7a55b0805 83 SonarTriggerTimer.attach(&SendSonarTrigger, 0.2);
OyaideA 2:47b7a55b0805 84 }
OyaideA 2:47b7a55b0805 85
OyaideA 2:47b7a55b0805 86 void SendSonarTrigger()
OyaideA 2:47b7a55b0805 87 {
OyaideA 2:47b7a55b0805 88 //Turn the monitoring of the event off whilst the trigger pulse is sent.
OyaideA 2:47b7a55b0805 89 Sonar.eventOn(MICROBIT_PIN_EVENT_NONE);
OyaideA 2:47b7a55b0805 90 /*Set the trigger to high for 10 microseconds, then back to a low. This
OyaideA 2:47b7a55b0805 91 setDigitalValue function also changes the pin to an output pin*/
OyaideA 2:47b7a55b0805 92 Sonar.setDigitalValue(1);
OyaideA 2:47b7a55b0805 93 wait_us(10);
OyaideA 2:47b7a55b0805 94 Sonar.setDigitalValue(0);
OyaideA 2:47b7a55b0805 95
OyaideA 2:47b7a55b0805 96 //Call this function to turn the sonar pin back to an input pin to await the echo.
OyaideA 2:47b7a55b0805 97 Sonar.getDigitalValue();
OyaideA 2:47b7a55b0805 98 //Turn the event back on so that this function responds again
OyaideA 2:47b7a55b0805 99 Sonar.eventOn(MICROBIT_PIN_EVENT_ON_PULSE);
OyaideA 2:47b7a55b0805 100
OyaideA 2:47b7a55b0805 101 return;
OyaideA 2:47b7a55b0805 102
OyaideA 2:47b7a55b0805 103 }
OyaideA 2:47b7a55b0805 104
OyaideA 2:47b7a55b0805 105 void onSonarEchoPulse(MicroBitEvent evt)
OyaideA 2:47b7a55b0805 106 {
OyaideA 2:47b7a55b0805 107 //Read the pulse wdith of the returned echo. This is returned in the timestamp of the event.
OyaideA 2:47b7a55b0805 108 SonarReturnPulseWidth = evt.timestamp;
OyaideA 2:47b7a55b0805 109
OyaideA 2:47b7a55b0805 110 return;
OyaideA 2:47b7a55b0805 111 }
OyaideA 2:47b7a55b0805 112
OyaideA 2:47b7a55b0805 113 unsigned int GetSonarTime_us()
OyaideA 2:47b7a55b0805 114 {
OyaideA 2:47b7a55b0805 115 return (unsigned int)SonarReturnPulseWidth;
OyaideA 2:47b7a55b0805 116 }
OyaideA 2:47b7a55b0805 117
OyaideA 2:47b7a55b0805 118 bool hasLastCommandCompleted()
OyaideA 2:47b7a55b0805 119 {
OyaideA 2:47b7a55b0805 120 if(BuggyState == BUGGY_RUNNING)
OyaideA 2:47b7a55b0805 121 {
OyaideA 2:47b7a55b0805 122 CurrentMotorRunTime = uBit.systemTime() - TimerStart;
OyaideA 2:47b7a55b0805 123 if(CurrentMotorRunTime >= TargetMotorRunTime)
OyaideA 2:47b7a55b0805 124 {
OyaideA 2:47b7a55b0805 125 //Stop the motors by writing a 0 voltage and setting direction to forward.
OyaideA 2:47b7a55b0805 126 MotorDirection_L.setDigitalValue(0);
OyaideA 2:47b7a55b0805 127 MotorDirection_R.setDigitalValue(0);
OyaideA 2:47b7a55b0805 128 MotorSpeed_L.setAnalogValue(0);
OyaideA 2:47b7a55b0805 129 MotorSpeed_R.setAnalogValue(0);
OyaideA 2:47b7a55b0805 130
OyaideA 2:47b7a55b0805 131 //Update the buggy state to show that it is now halted
OyaideA 2:47b7a55b0805 132 BuggyState = BUGGY_HALTED;
OyaideA 2:47b7a55b0805 133
OyaideA 2:47b7a55b0805 134 //completed
OyaideA 2:47b7a55b0805 135 return true;
OyaideA 2:47b7a55b0805 136 }
OyaideA 2:47b7a55b0805 137 else
OyaideA 2:47b7a55b0805 138 {
OyaideA 2:47b7a55b0805 139 //return not completed
OyaideA 2:47b7a55b0805 140 return false;
OyaideA 2:47b7a55b0805 141 }
OyaideA 2:47b7a55b0805 142 }
OyaideA 2:47b7a55b0805 143
OyaideA 2:47b7a55b0805 144 //return no last command was running
OyaideA 2:47b7a55b0805 145 return false;
OyaideA 2:47b7a55b0805 146 }
OyaideA 2:47b7a55b0805 147
OyaideA 2:47b7a55b0805 148 void PauseLastCommand()
OyaideA 2:47b7a55b0805 149 {
OyaideA 2:47b7a55b0805 150 //Only do this if the buggy is actually running
OyaideA 2:47b7a55b0805 151 if(BuggyState == BUGGY_RUNNING)
OyaideA 2:47b7a55b0805 152 {
OyaideA 2:47b7a55b0805 153 //Store the amount of elapsed time before the command is paused.
OyaideA 2:47b7a55b0805 154 CurrentMotorRunTime = uBit.systemTime() - TimerStart;
OyaideA 2:47b7a55b0805 155 //Stop the motors by writing a 0 voltage and setting direction to forward.
OyaideA 2:47b7a55b0805 156 MotorDirection_L.setDigitalValue(0);
OyaideA 2:47b7a55b0805 157 MotorDirection_R.setDigitalValue(0);
OyaideA 2:47b7a55b0805 158 MotorSpeed_L.setAnalogValue(0);
OyaideA 2:47b7a55b0805 159 MotorSpeed_R.setAnalogValue(0);
OyaideA 2:47b7a55b0805 160
OyaideA 2:47b7a55b0805 161 //Update the buggy state to show that it is now halted
OyaideA 2:47b7a55b0805 162 BuggyState = BUGGY_PAUSED;
OyaideA 2:47b7a55b0805 163 }
OyaideA 2:47b7a55b0805 164 }
OyaideA 2:47b7a55b0805 165
OyaideA 2:47b7a55b0805 166 void ContinueLastCommand()
OyaideA 2:47b7a55b0805 167 {
OyaideA 2:47b7a55b0805 168 //Only do this if the buggy is currently halted we have not yet reached the target run time
OyaideA 2:47b7a55b0805 169 if( (BuggyState == BUGGY_PAUSED) && (TargetMotorRunTime > CurrentMotorRunTime) )
OyaideA 2:47b7a55b0805 170 {
OyaideA 2:47b7a55b0805 171 //Update the target motor run time to only run for the incomplete time period and zero CurrentMotorRunTime
OyaideA 2:47b7a55b0805 172 TargetMotorRunTime = TargetMotorRunTime - CurrentMotorRunTime;
OyaideA 2:47b7a55b0805 173 CurrentMotorRunTime = 0;
OyaideA 2:47b7a55b0805 174 //Set the voltage of the motors as per the stored last voltage and direction command.
OyaideA 2:47b7a55b0805 175 MotorDirection_L.setDigitalValue(LastMotorDirection_L);
OyaideA 2:47b7a55b0805 176 MotorDirection_R.setDigitalValue(LastMotorDirection_R);
OyaideA 2:47b7a55b0805 177 MotorSpeed_L.setAnalogValue(LastMotorVoltage_L);
OyaideA 2:47b7a55b0805 178 MotorSpeed_R.setAnalogValue(LastMotorVoltage_R);
OyaideA 2:47b7a55b0805 179
OyaideA 2:47b7a55b0805 180 //Start the timer
OyaideA 2:47b7a55b0805 181 TimerStart = uBit.systemTime();
OyaideA 2:47b7a55b0805 182
OyaideA 2:47b7a55b0805 183 //Update the buggy state to show that it is now running
OyaideA 2:47b7a55b0805 184 BuggyState = BUGGY_RUNNING;
OyaideA 2:47b7a55b0805 185 }
OyaideA 2:47b7a55b0805 186 }
OyaideA 2:47b7a55b0805 187
OyaideA 2:47b7a55b0805 188 void MoveBuggy(int Command, unsigned int Voltage, unsigned int Time_ms)
OyaideA 2:47b7a55b0805 189 {
OyaideA 2:47b7a55b0805 190 //Initialise the variables for tracking the travel progress
OyaideA 2:47b7a55b0805 191 TargetMotorRunTime = Time_ms;
OyaideA 2:47b7a55b0805 192 CurrentMotorRunTime = 0;
OyaideA 2:47b7a55b0805 193
OyaideA 2:47b7a55b0805 194 //Limit the voltage to 1024, which is the max A2D output value.
OyaideA 2:47b7a55b0805 195 if(Voltage > 1024)
OyaideA 2:47b7a55b0805 196 {
OyaideA 2:47b7a55b0805 197 Voltage = 1024;
OyaideA 2:47b7a55b0805 198 }
OyaideA 2:47b7a55b0805 199
OyaideA 2:47b7a55b0805 200 switch(Command)
OyaideA 2:47b7a55b0805 201 {
OyaideA 2:47b7a55b0805 202 case MOVE_FORWARD:
OyaideA 2:47b7a55b0805 203 //Set the motor voltage as requested by the user
OyaideA 2:47b7a55b0805 204 LastMotorVoltage_L = Voltage;
OyaideA 2:47b7a55b0805 205 LastMotorVoltage_R = Voltage;
OyaideA 2:47b7a55b0805 206 //Set motor direction to forward
OyaideA 2:47b7a55b0805 207 LastMotorDirection_L = 0;
OyaideA 2:47b7a55b0805 208 LastMotorDirection_R = 0;
OyaideA 2:47b7a55b0805 209 break;
OyaideA 2:47b7a55b0805 210
OyaideA 2:47b7a55b0805 211 case MOVE_BACKWARD:
OyaideA 2:47b7a55b0805 212 /*Set the voltage of the motors as per the user request (1024 - value)
OyaideA 2:47b7a55b0805 213 In reverse, 0 is actually max speed and 1024 is stopped.*/
OyaideA 2:47b7a55b0805 214 LastMotorVoltage_L = 1024 - Voltage;
OyaideA 2:47b7a55b0805 215 LastMotorVoltage_R = 1024 - Voltage;
OyaideA 2:47b7a55b0805 216 //Set the motor direction to reverse
OyaideA 2:47b7a55b0805 217 LastMotorDirection_L = 1;
OyaideA 2:47b7a55b0805 218 LastMotorDirection_R = 1;
OyaideA 2:47b7a55b0805 219 break;
OyaideA 2:47b7a55b0805 220
OyaideA 2:47b7a55b0805 221 case ROTATE_CLOCKWISE:
OyaideA 2:47b7a55b0805 222 /*Set the voltage of the motors as per the user request (1024 - value)
OyaideA 2:47b7a55b0805 223 In reverse, 0 is actually max speed and 1024 is stopped.*/
OyaideA 2:47b7a55b0805 224 LastMotorVoltage_L = Voltage;
OyaideA 2:47b7a55b0805 225 LastMotorVoltage_R = 1024 - Voltage;
OyaideA 2:47b7a55b0805 226 //Set the direction to left wheel forwards and right wheel backward
OyaideA 2:47b7a55b0805 227 LastMotorDirection_L = 0;
OyaideA 2:47b7a55b0805 228 LastMotorDirection_R = 1;
OyaideA 2:47b7a55b0805 229 break;
OyaideA 2:47b7a55b0805 230
OyaideA 2:47b7a55b0805 231 case ROTATE_ANTICLOCKWISE:
OyaideA 2:47b7a55b0805 232 /*Set the voltage of the motors as per the user request (1024 - value)
OyaideA 2:47b7a55b0805 233 In reverse, 0 is actually max speed and 1024 is stopped.*/
OyaideA 2:47b7a55b0805 234 LastMotorVoltage_L = 1024 - Voltage;
OyaideA 2:47b7a55b0805 235 LastMotorVoltage_R = Voltage;
OyaideA 2:47b7a55b0805 236 //Set the direction to left wheel backwards and right wheel forward
OyaideA 2:47b7a55b0805 237 LastMotorDirection_L = 1;
OyaideA 2:47b7a55b0805 238 LastMotorDirection_R = 0;
OyaideA 2:47b7a55b0805 239 break;
OyaideA 2:47b7a55b0805 240
OyaideA 2:47b7a55b0805 241 default:
OyaideA 2:47b7a55b0805 242
OyaideA 2:47b7a55b0805 243 break;
OyaideA 2:47b7a55b0805 244 }
OyaideA 2:47b7a55b0805 245
OyaideA 2:47b7a55b0805 246 //Set the direction of the motors as per the command
OyaideA 2:47b7a55b0805 247 MotorDirection_L.setDigitalValue(LastMotorDirection_L);
OyaideA 2:47b7a55b0805 248 MotorDirection_R.setDigitalValue(LastMotorDirection_R);
OyaideA 2:47b7a55b0805 249 wait_ms(1);
OyaideA 2:47b7a55b0805 250 //Set the voltage of the motors as per the user request
OyaideA 2:47b7a55b0805 251 MotorSpeed_L.setAnalogValue(LastMotorVoltage_L);
OyaideA 2:47b7a55b0805 252 MotorSpeed_R.setAnalogValue(LastMotorVoltage_R);
OyaideA 2:47b7a55b0805 253
OyaideA 2:47b7a55b0805 254 //Start the timer
OyaideA 2:47b7a55b0805 255 TimerStart = uBit.systemTime();
OyaideA 2:47b7a55b0805 256
OyaideA 2:47b7a55b0805 257 //Update the buggy state to show that it is now running
OyaideA 2:47b7a55b0805 258 BuggyState = BUGGY_RUNNING;
OyaideA 2:47b7a55b0805 259
OyaideA 2:47b7a55b0805 260 return;
OyaideA 2:47b7a55b0805 261 }
OyaideA 2:47b7a55b0805 262
OyaideA 2:47b7a55b0805 263
OyaideA 2:47b7a55b0805 264 void RunBasicBuggyMotorTest(unsigned int Voltage, unsigned int Time_ms, unsigned int Pause_ms)
OyaideA 2:47b7a55b0805 265 {
OyaideA 2:47b7a55b0805 266 //Move the buggy forward
OyaideA 2:47b7a55b0805 267 MoveBuggy(MOVE_FORWARD, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 268 //wait and check if the command has completed
OyaideA 2:47b7a55b0805 269 do
OyaideA 2:47b7a55b0805 270 {
OyaideA 2:47b7a55b0805 271 //sleep whilst we wait for the command to complete
OyaideA 2:47b7a55b0805 272 uBit.sleep(Time_ms);
OyaideA 2:47b7a55b0805 273 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 274 //Pause before doing the next command
OyaideA 2:47b7a55b0805 275 wait_ms(Pause_ms);
OyaideA 2:47b7a55b0805 276
OyaideA 2:47b7a55b0805 277 //Move the buggy bacward
OyaideA 2:47b7a55b0805 278 MoveBuggy(MOVE_BACKWARD, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 279 //wait and check if the command has completed
OyaideA 2:47b7a55b0805 280 do
OyaideA 2:47b7a55b0805 281 {
OyaideA 2:47b7a55b0805 282 //sleep whilst we wait for the command to complete
OyaideA 2:47b7a55b0805 283 uBit.sleep(Time_ms);
OyaideA 2:47b7a55b0805 284 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 285 //Pause before doing the next command
OyaideA 2:47b7a55b0805 286 wait_ms(Pause_ms);
OyaideA 2:47b7a55b0805 287
OyaideA 2:47b7a55b0805 288 //Rotate the buggy clockwise
OyaideA 2:47b7a55b0805 289 MoveBuggy(ROTATE_CLOCKWISE, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 290 //wait and check if the command has completed
OyaideA 2:47b7a55b0805 291 do
OyaideA 2:47b7a55b0805 292 {
OyaideA 2:47b7a55b0805 293 //sleep whilst we wait for the command to complete
OyaideA 2:47b7a55b0805 294 uBit.sleep(Time_ms);
OyaideA 2:47b7a55b0805 295 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 296 //Pause before doing the next command
OyaideA 2:47b7a55b0805 297 wait_ms(Pause_ms);
OyaideA 2:47b7a55b0805 298
OyaideA 2:47b7a55b0805 299 //Rotate the buggy anticloclwise
OyaideA 2:47b7a55b0805 300 MoveBuggy(ROTATE_ANTICLOCKWISE, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 301 //wait and check if the command has completed
OyaideA 2:47b7a55b0805 302 do
OyaideA 2:47b7a55b0805 303 {
OyaideA 2:47b7a55b0805 304 //sleep whilst we wait for the command to complete
OyaideA 2:47b7a55b0805 305 uBit.sleep(Time_ms);
OyaideA 2:47b7a55b0805 306 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 307 //Pause before doing the next command
OyaideA 2:47b7a55b0805 308 wait_ms(Pause_ms);
OyaideA 2:47b7a55b0805 309
OyaideA 2:47b7a55b0805 310 return;
OyaideA 2:47b7a55b0805 311 }
OyaideA 2:47b7a55b0805 312
OyaideA 2:47b7a55b0805 313 void TestAntiCollision(unsigned int Voltage, unsigned int Time_ms, unsigned int SonarTime_us)
OyaideA 2:47b7a55b0805 314 {
OyaideA 2:47b7a55b0805 315 MoveBuggy(MOVE_FORWARD, Voltage, Time_ms);
OyaideA 2:47b7a55b0805 316 do
OyaideA 2:47b7a55b0805 317 {
OyaideA 2:47b7a55b0805 318 if(GetSonarTime_us() < SonarTime_us)
OyaideA 2:47b7a55b0805 319 {
OyaideA 2:47b7a55b0805 320 PauseLastCommand();
OyaideA 2:47b7a55b0805 321 }
OyaideA 2:47b7a55b0805 322 else
OyaideA 2:47b7a55b0805 323 {
OyaideA 2:47b7a55b0805 324 ContinueLastCommand();
OyaideA 2:47b7a55b0805 325 }
OyaideA 2:47b7a55b0805 326 }while( hasLastCommandCompleted() == false );
OyaideA 2:47b7a55b0805 327
OyaideA 2:47b7a55b0805 328 return;
OyaideA 2:47b7a55b0805 329
OyaideA 2:47b7a55b0805 330 }
OyaideA 2:47b7a55b0805 331
OyaideA 2:47b7a55b0805 332 void TestSonar()
OyaideA 2:47b7a55b0805 333 {
OyaideA 2:47b7a55b0805 334 //Local variables
OyaideA 2:47b7a55b0805 335 MicroBitImage SonarImage;
OyaideA 2:47b7a55b0805 336 unsigned int SonarDistance;
OyaideA 2:47b7a55b0805 337 int NumPixels, x, y;
OyaideA 2:47b7a55b0805 338 int MaxDisplayDist = 100;
OyaideA 2:47b7a55b0805 339 int WholeRows, RemainderColumns;
OyaideA 2:47b7a55b0805 340
OyaideA 2:47b7a55b0805 341 //Compute the distance in cm. the 58 is taken from the datasheet
OyaideA 2:47b7a55b0805 342 SonarDistance = GetSonarTime_us()/58;
OyaideA 2:47b7a55b0805 343 //limit to 1m
OyaideA 2:47b7a55b0805 344 if(SonarDistance > MaxDisplayDist)
OyaideA 2:47b7a55b0805 345 {
OyaideA 2:47b7a55b0805 346 SonarDistance = MaxDisplayDist;
OyaideA 2:47b7a55b0805 347 }
OyaideA 2:47b7a55b0805 348
OyaideA 2:47b7a55b0805 349 //Convert the distance to the number of pixels to light
OyaideA 2:47b7a55b0805 350 NumPixels = (SonarDistance*25)/MaxDisplayDist;
OyaideA 2:47b7a55b0805 351 //Convert into the number of whole pixel rows and remainder columns to light
OyaideA 2:47b7a55b0805 352 WholeRows = NumPixels/5;
OyaideA 2:47b7a55b0805 353 RemainderColumns = NumPixels%5;
OyaideA 2:47b7a55b0805 354
OyaideA 2:47b7a55b0805 355 //First fill the whole rows
OyaideA 2:47b7a55b0805 356 for(y=0; y<WholeRows; y++)
OyaideA 2:47b7a55b0805 357 {
OyaideA 2:47b7a55b0805 358 for(x=0; x<5; x++)
OyaideA 2:47b7a55b0805 359 {
OyaideA 2:47b7a55b0805 360 uBit.display.image.setPixelValue(x, y, 200);
OyaideA 2:47b7a55b0805 361 }
OyaideA 2:47b7a55b0805 362 }
OyaideA 2:47b7a55b0805 363
OyaideA 2:47b7a55b0805 364 //fill the partial row
OyaideA 2:47b7a55b0805 365 if(WholeRows < 5)
OyaideA 2:47b7a55b0805 366 {
OyaideA 2:47b7a55b0805 367 for(x=0; x<RemainderColumns; x++)
OyaideA 2:47b7a55b0805 368 {
OyaideA 2:47b7a55b0805 369 uBit.display.image.setPixelValue(x, y, 200);
OyaideA 2:47b7a55b0805 370 }
OyaideA 2:47b7a55b0805 371 }
OyaideA 2:47b7a55b0805 372
OyaideA 2:47b7a55b0805 373 //Fill the remaining pixels in the partial row with 0
OyaideA 2:47b7a55b0805 374 for( ; x<5; x++)
OyaideA 2:47b7a55b0805 375 {
OyaideA 2:47b7a55b0805 376 uBit.display.image.setPixelValue(x, y, 0);
OyaideA 2:47b7a55b0805 377 }
OyaideA 2:47b7a55b0805 378 //Continue from the next row
OyaideA 2:47b7a55b0805 379 y++;
OyaideA 2:47b7a55b0805 380 for( ; y<5; y++)
OyaideA 2:47b7a55b0805 381 {
OyaideA 2:47b7a55b0805 382 for(x=0; x<5; x++)
OyaideA 2:47b7a55b0805 383 {
OyaideA 2:47b7a55b0805 384 uBit.display.image.setPixelValue(x, y, 0);
OyaideA 2:47b7a55b0805 385 }
OyaideA 2:47b7a55b0805 386 }
OyaideA 2:47b7a55b0805 387
OyaideA 2:47b7a55b0805 388 return;
OyaideA 2:47b7a55b0805 389 }
OyaideA 2:47b7a55b0805 390
OyaideA 2:47b7a55b0805 391 void PrintSonarTiming(void)
OyaideA 2:47b7a55b0805 392 {
OyaideA 2:47b7a55b0805 393 //Local variables
OyaideA 2:47b7a55b0805 394 int SonarTime;
OyaideA 2:47b7a55b0805 395
OyaideA 2:47b7a55b0805 396 //read the latest sonar time
OyaideA 2:47b7a55b0805 397 SonarTime = GetSonarTime_us();
OyaideA 2:47b7a55b0805 398 //Print the time in micro secs
OyaideA 2:47b7a55b0805 399 uBit.display.printAsync(SonarTime);
OyaideA 2:47b7a55b0805 400 uBit.serial.printf("Time = %d us\n\r", SonarTime);
OyaideA 2:47b7a55b0805 401 }
OyaideA 4:2d939ef2b09c 402
OyaideA 4:2d939ef2b09c 403 void MotorSpeedCharacterisation(void)
OyaideA 4:2d939ef2b09c 404 {
OyaideA 4:2d939ef2b09c 405 int Voltage;
OyaideA 4:2d939ef2b09c 406 char OutBuf[7];
OyaideA 4:2d939ef2b09c 407 int Counter;
OyaideA 4:2d939ef2b09c 408 unsigned int LastSystemTime;
OyaideA 4:2d939ef2b09c 409
OyaideA 4:2d939ef2b09c 410 //do this forever
OyaideA 4:2d939ef2b09c 411 while(1)
OyaideA 4:2d939ef2b09c 412 {
OyaideA 4:2d939ef2b09c 413 if(MotorTestIndex < 10) //Move forward tests
OyaideA 4:2d939ef2b09c 414 {
OyaideA 4:2d939ef2b09c 415 Voltage = (MotorTestIndex * 100) + 100;
OyaideA 4:2d939ef2b09c 416 sprintf(OutBuf, "F%d", Voltage);
OyaideA 4:2d939ef2b09c 417 }
OyaideA 4:2d939ef2b09c 418 else if(MotorTestIndex < 20) //Rotate clockwise test
OyaideA 4:2d939ef2b09c 419 {
OyaideA 4:2d939ef2b09c 420 Voltage = ((MotorTestIndex-10) * 100) + 100;
OyaideA 4:2d939ef2b09c 421 sprintf(OutBuf, "C%d", Voltage);
OyaideA 4:2d939ef2b09c 422 }
OyaideA 4:2d939ef2b09c 423 else //Rotate anticlockwise tests
OyaideA 4:2d939ef2b09c 424 {
OyaideA 4:2d939ef2b09c 425 Voltage = ((MotorTestIndex-20) * 100) + 100;
OyaideA 4:2d939ef2b09c 426 sprintf(OutBuf, "A%d", Voltage);
OyaideA 4:2d939ef2b09c 427 }
OyaideA 4:2d939ef2b09c 428 //Display the current target test
OyaideA 4:2d939ef2b09c 429 uBit.display.print(OutBuf);
OyaideA 4:2d939ef2b09c 430
OyaideA 4:2d939ef2b09c 431 //If button B has been pressed
OyaideA 4:2d939ef2b09c 432 if(MotorTestGo == true)
OyaideA 4:2d939ef2b09c 433 {
OyaideA 4:2d939ef2b09c 434 //do the algorithm for counting down from 3
OyaideA 4:2d939ef2b09c 435 Counter = 3;
OyaideA 4:2d939ef2b09c 436 LastSystemTime = 0;
OyaideA 4:2d939ef2b09c 437 uBit.display.printAsync(Counter);
OyaideA 4:2d939ef2b09c 438 LastSystemTime = uBit.systemTime();
OyaideA 4:2d939ef2b09c 439 while (Counter > 0)
OyaideA 4:2d939ef2b09c 440 {
OyaideA 4:2d939ef2b09c 441 if( (uBit.systemTime()-LastSystemTime) > 1000)
OyaideA 4:2d939ef2b09c 442 {
OyaideA 4:2d939ef2b09c 443 LastSystemTime = uBit.systemTime();
OyaideA 4:2d939ef2b09c 444 Counter = Counter - 1;
OyaideA 4:2d939ef2b09c 445 uBit.display.printAsync(Counter);
OyaideA 4:2d939ef2b09c 446 }
OyaideA 4:2d939ef2b09c 447 }
OyaideA 4:2d939ef2b09c 448
OyaideA 4:2d939ef2b09c 449 //run the selected motor characterisation test
OyaideA 4:2d939ef2b09c 450 if(MotorTestIndex < 10) //Move forward tests
OyaideA 4:2d939ef2b09c 451 {
OyaideA 4:2d939ef2b09c 452 MoveBuggy(MOVE_FORWARD, Voltage, 1000);
OyaideA 4:2d939ef2b09c 453 do
OyaideA 4:2d939ef2b09c 454 {
OyaideA 4:2d939ef2b09c 455 //Nothing
OyaideA 4:2d939ef2b09c 456 }while( hasLastCommandCompleted() == false );
OyaideA 4:2d939ef2b09c 457 }
OyaideA 4:2d939ef2b09c 458 else if(MotorTestIndex < 20) //Rotate clockwise test
OyaideA 4:2d939ef2b09c 459 {
OyaideA 4:2d939ef2b09c 460 MoveBuggy(ROTATE_CLOCKWISE, Voltage, 500);
OyaideA 4:2d939ef2b09c 461 do
OyaideA 4:2d939ef2b09c 462 {
OyaideA 4:2d939ef2b09c 463 //Nothing
OyaideA 4:2d939ef2b09c 464 }while( hasLastCommandCompleted() == false );
OyaideA 4:2d939ef2b09c 465 }
OyaideA 4:2d939ef2b09c 466 else //Rotate anticlockwise tests
OyaideA 4:2d939ef2b09c 467 {
OyaideA 4:2d939ef2b09c 468 MoveBuggy(ROTATE_ANTICLOCKWISE, Voltage, 500);
OyaideA 4:2d939ef2b09c 469 do
OyaideA 4:2d939ef2b09c 470 {
OyaideA 4:2d939ef2b09c 471 //Nothing
OyaideA 4:2d939ef2b09c 472 }while( hasLastCommandCompleted() == false );
OyaideA 4:2d939ef2b09c 473 }
OyaideA 4:2d939ef2b09c 474
OyaideA 4:2d939ef2b09c 475 MotorTestGo = false;
OyaideA 4:2d939ef2b09c 476 }
OyaideA 4:2d939ef2b09c 477 }
OyaideA 4:2d939ef2b09c 478 }
OyaideA 4:2d939ef2b09c 479
OyaideA 4:2d939ef2b09c 480 void onButtonA(MicroBitEvent evt)
OyaideA 4:2d939ef2b09c 481 {
OyaideA 4:2d939ef2b09c 482 if(evt.value == MICROBIT_BUTTON_EVT_CLICK)
OyaideA 4:2d939ef2b09c 483 {
OyaideA 4:2d939ef2b09c 484 MotorTestIndex++;
OyaideA 4:2d939ef2b09c 485 if(MotorTestIndex > 30)
OyaideA 4:2d939ef2b09c 486 {
OyaideA 4:2d939ef2b09c 487 MotorTestIndex = 0;
OyaideA 4:2d939ef2b09c 488 }
OyaideA 4:2d939ef2b09c 489 }
OyaideA 4:2d939ef2b09c 490 }
OyaideA 4:2d939ef2b09c 491
OyaideA 4:2d939ef2b09c 492 void onButtonB(MicroBitEvent evt)
OyaideA 4:2d939ef2b09c 493 {
OyaideA 4:2d939ef2b09c 494 if(evt.value == MICROBIT_BUTTON_EVT_CLICK)
OyaideA 4:2d939ef2b09c 495 {
OyaideA 4:2d939ef2b09c 496 MotorTestGo = true;
OyaideA 4:2d939ef2b09c 497 }
OyaideA 5:a33f016d5962 498 }
OyaideA 5:a33f016d5962 499
OyaideA 5:a33f016d5962 500 void SelfTest()
OyaideA 5:a33f016d5962 501 {
OyaideA 5:a33f016d5962 502 int Counter = 3;
OyaideA 5:a33f016d5962 503 unsigned int LastSystemTime = 0;
OyaideA 5:a33f016d5962 504
OyaideA 5:a33f016d5962 505 //Display start message using the LEDs
OyaideA 5:a33f016d5962 506 //uBit.display.scroll("Hello World");
OyaideA 5:a33f016d5962 507 //Instead, display 3, 2, 1, 0
OyaideA 5:a33f016d5962 508 uBit.display.printAsync(Counter);
OyaideA 5:a33f016d5962 509 LastSystemTime = uBit.systemTime();
OyaideA 5:a33f016d5962 510 while (Counter > 0)
OyaideA 5:a33f016d5962 511 {
OyaideA 5:a33f016d5962 512 if( (uBit.systemTime()-LastSystemTime) > 1000)
OyaideA 5:a33f016d5962 513 {
OyaideA 5:a33f016d5962 514 //uBit.serial.printf("Counter=%d, ", Counter);
OyaideA 5:a33f016d5962 515 LastSystemTime = uBit.systemTime();
OyaideA 5:a33f016d5962 516 Counter = Counter - 1;
OyaideA 5:a33f016d5962 517 uBit.display.printAsync(Counter);
OyaideA 5:a33f016d5962 518 }
OyaideA 5:a33f016d5962 519 }
OyaideA 5:a33f016d5962 520
OyaideA 5:a33f016d5962 521 while(1)
OyaideA 5:a33f016d5962 522 {
OyaideA 5:a33f016d5962 523 //Run the sonar test for 10 secs
OyaideA 5:a33f016d5962 524 LastSystemTime = uBit.systemTime();
OyaideA 5:a33f016d5962 525 while( (uBit.systemTime()-LastSystemTime) < 10000)
OyaideA 5:a33f016d5962 526 {
OyaideA 5:a33f016d5962 527 TestSonar();
OyaideA 5:a33f016d5962 528 }
OyaideA 5:a33f016d5962 529 //Display 0
OyaideA 5:a33f016d5962 530 uBit.display.printAsync(Counter);
OyaideA 5:a33f016d5962 531 //Run the motor test
OyaideA 6:edbaeaaf08bb 532 RunBasicBuggyMotorTest(500, 1000, 1000);
OyaideA 5:a33f016d5962 533 }
OyaideA 5:a33f016d5962 534 }
OyaideA 5:a33f016d5962 535
OyaideA 5:a33f016d5962 536 void DisplaySonarTiming()
OyaideA 5:a33f016d5962 537 {
OyaideA 5:a33f016d5962 538 while(1)
OyaideA 5:a33f016d5962 539 {
OyaideA 5:a33f016d5962 540 PrintSonarTiming();
OyaideA 5:a33f016d5962 541 }
OyaideA 5:a33f016d5962 542
OyaideA 5:a33f016d5962 543 }