The preloaded firmware shipped on the mBot.
Dependencies: mbed
Fork of Official_mBot by
main.cpp@1:ffd9a51e7d35, 2014-11-25 (annotated)
- Committer:
- jeffknaggs
- Date:
- Tue Nov 25 14:49:40 2014 +0000
- Revision:
- 1:ffd9a51e7d35
- Parent:
- 0:865d42c46692
Initial commit.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jeffknaggs | 0:865d42c46692 | 1 | /* |
jeffknaggs | 0:865d42c46692 | 2 | ** This software can be freely used, even comercially, as highlighted in the license. |
jeffknaggs | 0:865d42c46692 | 3 | ** |
jeffknaggs | 0:865d42c46692 | 4 | ** Copyright 2014 GHI Electronics, LLC |
jeffknaggs | 0:865d42c46692 | 5 | ** |
jeffknaggs | 0:865d42c46692 | 6 | ** Licensed under the Apache License, Version 2.0 (the "License"); |
jeffknaggs | 0:865d42c46692 | 7 | ** you may not use this file except in compliance with the License. |
jeffknaggs | 0:865d42c46692 | 8 | ** You may obtain a copy of the License at |
jeffknaggs | 0:865d42c46692 | 9 | ** |
jeffknaggs | 0:865d42c46692 | 10 | ** http://www.apache.org/licenses/LICENSE-2.0 |
jeffknaggs | 0:865d42c46692 | 11 | ** |
jeffknaggs | 0:865d42c46692 | 12 | ** Unless required by applicable law or agreed to in writing, software |
jeffknaggs | 0:865d42c46692 | 13 | ** distributed under the License is distributed on an "AS IS" BASIS, |
jeffknaggs | 0:865d42c46692 | 14 | ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
jeffknaggs | 0:865d42c46692 | 15 | ** See the License for the specific language governing permissions and |
jeffknaggs | 0:865d42c46692 | 16 | ** limitations under the License. |
jeffknaggs | 0:865d42c46692 | 17 | ** |
jeffknaggs | 0:865d42c46692 | 18 | **/ |
jeffknaggs | 0:865d42c46692 | 19 | |
jeffknaggs | 0:865d42c46692 | 20 | #include "mbed.h" |
jeffknaggs | 0:865d42c46692 | 21 | #include "mBot.h" |
jeffknaggs | 0:865d42c46692 | 22 | |
jeffknaggs | 0:865d42c46692 | 23 | #define MOTOR_SPEED 50 |
jeffknaggs | 0:865d42c46692 | 24 | // 0.5 was too sensitive, just spun on light table |
jeffknaggs | 0:865d42c46692 | 25 | // 0.9 was too sensitive for dark tables, with 10 bit precision |
jeffknaggs | 0:865d42c46692 | 26 | #define REFLECTIVITY_READING 0.950 |
jeffknaggs | 0:865d42c46692 | 27 | |
jeffknaggs | 0:865d42c46692 | 28 | bool OverEdge(bool * leftOverEdge, bool * rightOverEdge, int threshold_l, int threshold_r); |
jeffknaggs | 0:865d42c46692 | 29 | |
jeffknaggs | 0:865d42c46692 | 30 | |
jeffknaggs | 0:865d42c46692 | 31 | int base_reflective_l; |
jeffknaggs | 0:865d42c46692 | 32 | int base_reflective_r; |
jeffknaggs | 0:865d42c46692 | 33 | |
jeffknaggs | 0:865d42c46692 | 34 | DigitalOut LED[] = {(P1_19), (P0_19), (P0_20), (P0_2)}; |
jeffknaggs | 0:865d42c46692 | 35 | static const int LED_L1 = 0; |
jeffknaggs | 0:865d42c46692 | 36 | static const int LED_L2 = 1; |
jeffknaggs | 0:865d42c46692 | 37 | static const int LED_R1 = 2; |
jeffknaggs | 0:865d42c46692 | 38 | static const int LED_R2 = 3; |
jeffknaggs | 0:865d42c46692 | 39 | static const int LED_COUNT = 4; |
jeffknaggs | 0:865d42c46692 | 40 | |
jeffknaggs | 0:865d42c46692 | 41 | static int next_LED = 0; |
jeffknaggs | 0:865d42c46692 | 42 | void LEDfrontOn(){ |
jeffknaggs | 0:865d42c46692 | 43 | LED[LED_L1] = 1; |
jeffknaggs | 0:865d42c46692 | 44 | LED[LED_R1] = 1; |
jeffknaggs | 0:865d42c46692 | 45 | }; |
jeffknaggs | 0:865d42c46692 | 46 | void LEDfrontOff(){ |
jeffknaggs | 0:865d42c46692 | 47 | LED[LED_L1] = 0; |
jeffknaggs | 0:865d42c46692 | 48 | LED[LED_R1] = 0; |
jeffknaggs | 0:865d42c46692 | 49 | }; |
jeffknaggs | 0:865d42c46692 | 50 | void LEDbackOn(){ |
jeffknaggs | 0:865d42c46692 | 51 | LED[LED_L2] = 1; |
jeffknaggs | 0:865d42c46692 | 52 | LED[LED_R2] = 1; |
jeffknaggs | 0:865d42c46692 | 53 | }; |
jeffknaggs | 0:865d42c46692 | 54 | void LEDbackOff(){ |
jeffknaggs | 0:865d42c46692 | 55 | LED[LED_L2] = 0; |
jeffknaggs | 0:865d42c46692 | 56 | LED[LED_R2] = 0; |
jeffknaggs | 0:865d42c46692 | 57 | }; |
jeffknaggs | 0:865d42c46692 | 58 | void LEDsOff(){ |
jeffknaggs | 0:865d42c46692 | 59 | for (int ix = 0; ix < LED_COUNT; ix++) LED[ix] = 0; |
jeffknaggs | 0:865d42c46692 | 60 | } |
jeffknaggs | 0:865d42c46692 | 61 | |
jeffknaggs | 0:865d42c46692 | 62 | |
jeffknaggs | 0:865d42c46692 | 63 | DigitalOut Buzzer(P0_18); // 18 mbot, 14 lovemeter |
jeffknaggs | 0:865d42c46692 | 64 | void soundBuzzer(int Hz) |
jeffknaggs | 0:865d42c46692 | 65 | { |
jeffknaggs | 0:865d42c46692 | 66 | int ticks = Hz/64; |
jeffknaggs | 0:865d42c46692 | 67 | int tickCount = 0; |
jeffknaggs | 0:865d42c46692 | 68 | float frequency = 1/(float)Hz; |
jeffknaggs | 0:865d42c46692 | 69 | |
jeffknaggs | 0:865d42c46692 | 70 | while(tickCount < ticks) { |
jeffknaggs | 0:865d42c46692 | 71 | wait(frequency); |
jeffknaggs | 0:865d42c46692 | 72 | Buzzer = true; |
jeffknaggs | 0:865d42c46692 | 73 | wait(frequency); |
jeffknaggs | 0:865d42c46692 | 74 | Buzzer = false; |
jeffknaggs | 0:865d42c46692 | 75 | tickCount++; |
jeffknaggs | 0:865d42c46692 | 76 | } |
jeffknaggs | 0:865d42c46692 | 77 | } |
jeffknaggs | 0:865d42c46692 | 78 | void soundBreaks() { |
jeffknaggs | 0:865d42c46692 | 79 | for (int ix = 1; ix <= 10; ix++) { |
jeffknaggs | 0:865d42c46692 | 80 | soundBuzzer((ix * 100) + 1500); |
jeffknaggs | 0:865d42c46692 | 81 | } |
jeffknaggs | 0:865d42c46692 | 82 | } |
jeffknaggs | 0:865d42c46692 | 83 | void soundMotorStart() { |
jeffknaggs | 0:865d42c46692 | 84 | for (int ix = 1; ix <= 35; ix++) { |
jeffknaggs | 0:865d42c46692 | 85 | soundBuzzer(ix * 100 + 800); |
jeffknaggs | 0:865d42c46692 | 86 | } |
jeffknaggs | 0:865d42c46692 | 87 | } |
jeffknaggs | 0:865d42c46692 | 88 | void soundForward() { |
jeffknaggs | 0:865d42c46692 | 89 | soundBuzzer(1000); |
jeffknaggs | 0:865d42c46692 | 90 | } |
jeffknaggs | 0:865d42c46692 | 91 | |
jeffknaggs | 0:865d42c46692 | 92 | typedef DigitalIn Button_t; |
jeffknaggs | 0:865d42c46692 | 93 | Button_t btn1(P0_1, PullUp); |
jeffknaggs | 0:865d42c46692 | 94 | //Button_t btn2(P0_16, PullUp); |
jeffknaggs | 0:865d42c46692 | 95 | |
jeffknaggs | 0:865d42c46692 | 96 | |
jeffknaggs | 0:865d42c46692 | 97 | bool buttonIsUp(Button_t * button){ |
jeffknaggs | 0:865d42c46692 | 98 | return *button == 1; |
jeffknaggs | 0:865d42c46692 | 99 | }; |
jeffknaggs | 0:865d42c46692 | 100 | |
jeffknaggs | 0:865d42c46692 | 101 | mBot mybot; |
jeffknaggs | 0:865d42c46692 | 102 | void stopIfButtonPress(){ |
jeffknaggs | 0:865d42c46692 | 103 | if (!buttonIsUp(&btn1)){ |
jeffknaggs | 0:865d42c46692 | 104 | LEDsOff(); |
jeffknaggs | 0:865d42c46692 | 105 | mybot.SetReflectiveSensorState(0); |
jeffknaggs | 0:865d42c46692 | 106 | mybot.SetMotorSpeed(0,0); |
jeffknaggs | 0:865d42c46692 | 107 | deepsleep(); |
jeffknaggs | 0:865d42c46692 | 108 | } |
jeffknaggs | 0:865d42c46692 | 109 | } |
jeffknaggs | 0:865d42c46692 | 110 | void startTone() { |
jeffknaggs | 0:865d42c46692 | 111 | for (int ix = 1; ix <= 5; ix++) { |
jeffknaggs | 0:865d42c46692 | 112 | soundBuzzer(500); |
jeffknaggs | 0:865d42c46692 | 113 | } |
jeffknaggs | 0:865d42c46692 | 114 | } |
jeffknaggs | 0:865d42c46692 | 115 | void startUp(){ |
jeffknaggs | 0:865d42c46692 | 116 | LEDsOff(); |
jeffknaggs | 0:865d42c46692 | 117 | |
jeffknaggs | 0:865d42c46692 | 118 | next_LED = 0; LED[next_LED++] = 1; |
jeffknaggs | 0:865d42c46692 | 119 | startTone(); wait_ms(10); |
jeffknaggs | 0:865d42c46692 | 120 | if (next_LED >= LED_COUNT) next_LED = 0; LED[next_LED++] = 1; |
jeffknaggs | 0:865d42c46692 | 121 | startTone(); wait_ms(10); |
jeffknaggs | 0:865d42c46692 | 122 | if (next_LED >= LED_COUNT) next_LED = 0; LED[next_LED++] = 1; |
jeffknaggs | 0:865d42c46692 | 123 | startTone(); wait_ms(10); |
jeffknaggs | 0:865d42c46692 | 124 | if (next_LED >= LED_COUNT) next_LED = 0; LED[next_LED++] = 1; |
jeffknaggs | 0:865d42c46692 | 125 | startTone(); wait_ms(20); |
jeffknaggs | 0:865d42c46692 | 126 | if (next_LED >= LED_COUNT) next_LED = 0; LED[next_LED++] = 1; |
jeffknaggs | 0:865d42c46692 | 127 | startTone(); wait_ms(20); |
jeffknaggs | 0:865d42c46692 | 128 | if (next_LED >= LED_COUNT) next_LED = 0; LED[next_LED++] = 1; |
jeffknaggs | 0:865d42c46692 | 129 | startTone(); wait_ms(20); |
jeffknaggs | 0:865d42c46692 | 130 | if (next_LED >= LED_COUNT) next_LED = 0; LED[next_LED++] = 1; |
jeffknaggs | 0:865d42c46692 | 131 | startTone(); wait_ms(40); |
jeffknaggs | 0:865d42c46692 | 132 | if (next_LED >= LED_COUNT) next_LED = 0; LED[next_LED++] = 1; |
jeffknaggs | 0:865d42c46692 | 133 | startTone(); wait_ms(40); |
jeffknaggs | 0:865d42c46692 | 134 | if (next_LED < LED_COUNT && next_LED >= 0) LED[next_LED-1] = 0; |
jeffknaggs | 0:865d42c46692 | 135 | startTone(); |
jeffknaggs | 0:865d42c46692 | 136 | LEDfrontOn(); |
jeffknaggs | 0:865d42c46692 | 137 | LEDbackOff(); |
jeffknaggs | 0:865d42c46692 | 138 | } |
jeffknaggs | 0:865d42c46692 | 139 | int main() |
jeffknaggs | 0:865d42c46692 | 140 | { |
jeffknaggs | 0:865d42c46692 | 141 | int overleft; |
jeffknaggs | 0:865d42c46692 | 142 | int overright; |
jeffknaggs | 0:865d42c46692 | 143 | int left_temp; |
jeffknaggs | 0:865d42c46692 | 144 | int right_temp; |
jeffknaggs | 0:865d42c46692 | 145 | int LEDblinkCounter = 0; |
jeffknaggs | 0:865d42c46692 | 146 | startUp(); |
jeffknaggs | 0:865d42c46692 | 147 | // add time slicing for waits |
jeffknaggs | 0:865d42c46692 | 148 | mybot.SetMotorSpeed(MOTOR_SPEED, MOTOR_SPEED); |
jeffknaggs | 0:865d42c46692 | 149 | wait_ms(10); |
jeffknaggs | 0:865d42c46692 | 150 | while(1) |
jeffknaggs | 0:865d42c46692 | 151 | { |
jeffknaggs | 0:865d42c46692 | 152 | //wait_ms(5); |
jeffknaggs | 0:865d42c46692 | 153 | overleft = mybot.GetReflectiveReading(mybot.Left) > REFLECTIVITY_READING; |
jeffknaggs | 0:865d42c46692 | 154 | overright = mybot.GetReflectiveReading(mybot.Right)> REFLECTIVITY_READING; |
jeffknaggs | 0:865d42c46692 | 155 | stopIfButtonPress(); |
jeffknaggs | 0:865d42c46692 | 156 | if (LEDblinkCounter++ < 8000) { |
jeffknaggs | 0:865d42c46692 | 157 | if (LEDblinkCounter == 4000) { |
jeffknaggs | 0:865d42c46692 | 158 | LEDfrontOff(); |
jeffknaggs | 0:865d42c46692 | 159 | soundForward(); // slight wait here |
jeffknaggs | 0:865d42c46692 | 160 | } |
jeffknaggs | 0:865d42c46692 | 161 | } else { |
jeffknaggs | 0:865d42c46692 | 162 | LEDfrontOn(); |
jeffknaggs | 0:865d42c46692 | 163 | LEDblinkCounter = 0; |
jeffknaggs | 0:865d42c46692 | 164 | soundForward(); // slight wait here |
jeffknaggs | 0:865d42c46692 | 165 | } |
jeffknaggs | 0:865d42c46692 | 166 | |
jeffknaggs | 0:865d42c46692 | 167 | if( overleft || overright ) |
jeffknaggs | 0:865d42c46692 | 168 | { |
jeffknaggs | 0:865d42c46692 | 169 | |
jeffknaggs | 0:865d42c46692 | 170 | mybot.SetMotorSpeed(0,0); |
jeffknaggs | 0:865d42c46692 | 171 | LEDfrontOff(); |
jeffknaggs | 0:865d42c46692 | 172 | LEDbackOn(); |
jeffknaggs | 0:865d42c46692 | 173 | //wait_us(100); |
jeffknaggs | 0:865d42c46692 | 174 | if (overleft && overright) |
jeffknaggs | 0:865d42c46692 | 175 | { |
jeffknaggs | 0:865d42c46692 | 176 | left_temp = -30; |
jeffknaggs | 0:865d42c46692 | 177 | right_temp = 30; |
jeffknaggs | 0:865d42c46692 | 178 | } |
jeffknaggs | 0:865d42c46692 | 179 | else if (overleft) |
jeffknaggs | 0:865d42c46692 | 180 | { |
jeffknaggs | 0:865d42c46692 | 181 | left_temp = 60; |
jeffknaggs | 0:865d42c46692 | 182 | right_temp = -50; |
jeffknaggs | 0:865d42c46692 | 183 | } |
jeffknaggs | 0:865d42c46692 | 184 | else |
jeffknaggs | 0:865d42c46692 | 185 | { |
jeffknaggs | 0:865d42c46692 | 186 | left_temp = -50; |
jeffknaggs | 0:865d42c46692 | 187 | right_temp = 60; |
jeffknaggs | 0:865d42c46692 | 188 | } |
jeffknaggs | 0:865d42c46692 | 189 | |
jeffknaggs | 0:865d42c46692 | 190 | // back straight for a safety margin and comeabout |
jeffknaggs | 0:865d42c46692 | 191 | mybot.SetMotorSpeed(-80, -80); |
jeffknaggs | 0:865d42c46692 | 192 | wait_ms(325); |
jeffknaggs | 0:865d42c46692 | 193 | mybot.SetMotorSpeed(0,0); |
jeffknaggs | 0:865d42c46692 | 194 | wait_ms(5); |
jeffknaggs | 0:865d42c46692 | 195 | mybot.SetMotorSpeed(left_temp, right_temp); |
jeffknaggs | 0:865d42c46692 | 196 | wait_ms(300); |
jeffknaggs | 0:865d42c46692 | 197 | LEDbackOff(); |
jeffknaggs | 0:865d42c46692 | 198 | mybot.SetMotorSpeed(0,0); |
jeffknaggs | 0:865d42c46692 | 199 | LEDfrontOn(); |
jeffknaggs | 0:865d42c46692 | 200 | LEDblinkCounter = 0; |
jeffknaggs | 0:865d42c46692 | 201 | soundBreaks(); |
jeffknaggs | 0:865d42c46692 | 202 | mybot.SetMotorSpeed(MOTOR_SPEED,MOTOR_SPEED); |
jeffknaggs | 1:ffd9a51e7d35 | 203 | |
jeffknaggs | 0:865d42c46692 | 204 | |
jeffknaggs | 0:865d42c46692 | 205 | } |
jeffknaggs | 0:865d42c46692 | 206 | } |
jeffknaggs | 0:865d42c46692 | 207 | } |
jeffknaggs | 0:865d42c46692 | 208 |