Starting point for the student buggy project
Fork of microbit-hello-world by
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?
User | Revision | Line number | New 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 | } |