Starting point for the student buggy project
Fork of microbit-hello-world by
Buggy.cpp@5:a33f016d5962, 2017-08-07 (annotated)
- Committer:
- OyaideA
- Date:
- Mon Aug 07 18:57:55 2017 +0000
- Revision:
- 5:a33f016d5962
- Parent:
- 4:2d939ef2b09c
- Child:
- 6:edbaeaaf08bb
Now with a cleaned up main.cpp
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 | 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 | 5:a33f016d5962 | 532 | RunBasicBuggyMotorTest(800, 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 | } |