Andrew Oyaide / StudentBuggyProject

Starting point for the student buggy project

Dependencies:   microbit

Fork of microbit-hello-world by micro:bit

Buggy.cpp@4:2d939ef2b09c, 2017-08-06 (annotated)

Committer:
OyaideA
Date:
Sun Aug 06 20:13:33 2017 +0000
Revision:
4:2d939ef2b09c
Parent:
2:47b7a55b0805
Child:
5:a33f016d5962
Now includes motor characterisation code

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