GHI Electronics / Mbed 2 deprecated Official_mBot

The preloaded firmware shipped on the mBot.

Dependencies:   mbed

Fork of Official_mBot by Fred Parker

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?

UserRevisionLine numberNew 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