BULME_BERTL14 / Mbed 2 deprecated func_Bertl

Bertl Robot with fiunctions

Dependencies:   mbed HCSR

main.cpp@7:cb6947e1f1d3, 2015-02-05 (annotated)

Committer:
bulmecisco
Date:
Thu Feb 05 12:37:47 2015 +0000
Revision:
7:cb6947e1f1d3
Parent:
6:be710baf53ec
Child:
9:8e3392380915
FrontIsClearU for ultra sonic sensor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Enenkel 0:0b7c22955b8c 1 /***********************************
bulmecisco 6:be710baf53ec 2 * name: func_Bertl v 0.3
bulmecisco 4:a975caedface 3 * author: PE HTL BULME
bulmecisco 4:a975caedface 4 * email: pe@bulme.at
bulmecisco 4:a975caedface 5 * description:
bulmecisco 5:7b091d085c70 6 * functions for Bertl The Robot
bulmecisco 4:a975caedface 7 *
bulmecisco 4:a975caedface 8 *************************************/
Enenkel 0:0b7c22955b8c 9 #include "mbed.h"
bulmecisco 4:a975caedface 10 #include "config.h"
bulmecisco 4:a975caedface 11
bulmecisco 6:be710baf53ec 12 bool Start();
bulmecisco 5:7b091d085c70 13 void ShutOff();
bulmecisco 5:7b091d085c70 14 void Move();
bulmecisco 4:a975caedface 15 bool FrontIsClear();
bulmecisco 5:7b091d085c70 16 bool MoveMeasure(int time, int& left, int& right);
bulmecisco 5:7b091d085c70 17 bool MoveTicks(int ticks);
bulmecisco 5:7b091d085c70 18 void TurnBack();
bulmecisco 5:7b091d085c70 19 void TurnLeftTicks(int ticks);
bulmecisco 5:7b091d085c70 20 void TurnLeftTime(int ms);
bulmecisco 5:7b091d085c70 21 void TurnLeft();
bulmecisco 5:7b091d085c70 22 void TurnLedOn(int16_t led);
bulmecisco 5:7b091d085c70 23 void TurnLedOff(int16_t led);
bulmecisco 5:7b091d085c70 24 void MoveMore(int anzahl);
bulmecisco 5:7b091d085c70 25 void Back();
bulmecisco 5:7b091d085c70 26 bool WaitUntilButtonPressed();
bulmecisco 6:be710baf53ec 27
bulmecisco 6:be710baf53ec 28 // Eigene Funktionsdefinitionen hier
bulmecisco 7:cb6947e1f1d3 29 bool FrontIsClearU()
bulmecisco 7:cb6947e1f1d3 30 {
bulmecisco 7:cb6947e1f1d3 31 int dist = 0;
bulmecisco 7:cb6947e1f1d3 32 usensor.start();
bulmecisco 7:cb6947e1f1d3 33 wait_ms(10);
bulmecisco 7:cb6947e1f1d3 34 dist=usensor.get_dist_cm();
bulmecisco 7:cb6947e1f1d3 35 if(dist < 5)
bulmecisco 7:cb6947e1f1d3 36 return false;
bulmecisco 7:cb6947e1f1d3 37 else
bulmecisco 7:cb6947e1f1d3 38 return true;
bulmecisco 7:cb6947e1f1d3 39
bulmecisco 7:cb6947e1f1d3 40 // pc.printf("\r\nCount =%d",count);
bulmecisco 7:cb6947e1f1d3 41 // pc.printf("Distance: %d",dist );
bulmecisco 7:cb6947e1f1d3 42 }
bulmecisco 5:7b091d085c70 43 // ************* Hauptprogramm ************
bulmecisco 5:7b091d085c70 44 int main()
bulmecisco 5:7b091d085c70 45 {
bulmecisco 7:cb6947e1f1d3 46 int dist = 0;
bulmecisco 7:cb6947e1f1d3 47 unsigned char count=0;
bulmecisco 7:cb6947e1f1d3 48 /* while(count < 20) {
bulmecisco 7:cb6947e1f1d3 49 usensor.start();
bulmecisco 7:cb6947e1f1d3 50 wait_ms(500);
bulmecisco 7:cb6947e1f1d3 51 dist=usensor.get_dist_cm();
bulmecisco 7:cb6947e1f1d3 52 pc.printf("\r\nCount =%d",count);
bulmecisco 7:cb6947e1f1d3 53 pc.printf("Distance: %d",dist );
bulmecisco 7:cb6947e1f1d3 54 count++;
bulmecisco 5:7b091d085c70 55
bulmecisco 7:cb6947e1f1d3 56 }
bulmecisco 7:cb6947e1f1d3 57 */ // while(!Start()) {} // wait to start
bulmecisco 7:cb6947e1f1d3 58 wait_ms(200);
bulmecisco 7:cb6947e1f1d3 59 while(FrontIsClearU())
bulmecisco 7:cb6947e1f1d3 60 {
bulmecisco 7:cb6947e1f1d3 61 MoveTicks(40);
bulmecisco 7:cb6947e1f1d3 62 }
bulmecisco 5:7b091d085c70 63 TurnLeft();
bulmecisco 7:cb6947e1f1d3 64 while(FrontIsClearU())
bulmecisco 7:cb6947e1f1d3 65 {
bulmecisco 7:cb6947e1f1d3 66 MoveTicks(40);
bulmecisco 7:cb6947e1f1d3 67 }
bulmecisco 5:7b091d085c70 68
bulmecisco 7:cb6947e1f1d3 69 // TurnLeftTicks(2000); // Anzahl der MotorSensorwerte
bulmecisco 6:be710baf53ec 70
bulmecisco 5:7b091d085c70 71 ShutOff();
bulmecisco 5:7b091d085c70 72 return 0;
bulmecisco 5:7b091d085c70 73 }
bulmecisco 5:7b091d085c70 74
bulmecisco 5:7b091d085c70 75
bulmecisco 5:7b091d085c70 76 bool Start()
bulmecisco 5:7b091d085c70 77 {
bulmecisco 5:7b091d085c70 78 DEBUG_PRINT("Debug level: %d", (int) DEBUG);
bulmecisco 5:7b091d085c70 79 i2c.frequency(40000); // I2C Frequenz 40kHz
bulmecisco 5:7b091d085c70 80
bulmecisco 5:7b091d085c70 81 while(WaitUntilButtonPressed()) { }
bulmecisco 5:7b091d085c70 82 return true;
bulmecisco 5:7b091d085c70 83 }
bulmecisco 5:7b091d085c70 84
bulmecisco 4:a975caedface 85
bulmecisco 4:a975caedface 86 void ShutOff()
bulmecisco 4:a975caedface 87 {
bulmecisco 4:a975caedface 88 MotorR_FORWARD = MotorL_FORWARD = 0; // motor OFF
bulmecisco 4:a975caedface 89 MotorR_EN=MotorL_EN=0; // motor disable
bulmecisco 4:a975caedface 90 }
Enenkel 0:0b7c22955b8c 91
bulmecisco 4:a975caedface 92 void Move()
bulmecisco 3:c7e5419fa980 93 {
bulmecisco 4:a975caedface 94 int count=0, left=0, right=0; // initialise variables
bulmecisco 4:a975caedface 95 MotorR_EN=MotorL_EN=1; // both motor ENABLE
bulmecisco 4:a975caedface 96
bulmecisco 4:a975caedface 97 while(count<1000) {
bulmecisco 4:a975caedface 98 MotorR_FORWARD = MotorL_FORWARD = 1;// both motor forward ON
bulmecisco 4:a975caedface 99 LED_D10 = SensorL; // LED D10 blinks
bulmecisco 4:a975caedface 100 if(SensorL == 1)
bulmecisco 4:a975caedface 101 left++;
bulmecisco 4:a975caedface 102 if(SensorR == 1)
bulmecisco 4:a975caedface 103 right++;
bulmecisco 4:a975caedface 104 LED_D13 = SensorR; // LED D13 blinks
bulmecisco 4:a975caedface 105 count++;
bulmecisco 4:a975caedface 106 wait_ms(1); // wait for 1 ms
bulmecisco 4:a975caedface 107 }
bulmecisco 4:a975caedface 108 //DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);
bulmecisco 4:a975caedface 109
bulmecisco 4:a975caedface 110 MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off
bulmecisco 4:a975caedface 111 MotorR_EN=MotorL_EN=0;
bulmecisco 4:a975caedface 112 wait_ms(250);
bulmecisco 4:a975caedface 113 }
bulmecisco 3:c7e5419fa980 114
bulmecisco 4:a975caedface 115 bool FrontIsClear()
bulmecisco 4:a975caedface 116 {
bulmecisco 4:a975caedface 117 char cmd[3]; // array for I2C
bulmecisco 4:a975caedface 118 int16_t btns;
bulmecisco 4:a975caedface 119 bool wert;
bulmecisco 4:a975caedface 120
bulmecisco 4:a975caedface 121 cmd[0] = 0x06;
bulmecisco 4:a975caedface 122 cmd[1] = 0x00;
bulmecisco 4:a975caedface 123 i2c.write(addr, cmd, 2); // define Port0 = Out
bulmecisco 4:a975caedface 124
bulmecisco 4:a975caedface 125 cmd[0]=0x01;
bulmecisco 4:a975caedface 126 i2c.write(addr, cmd, 1);
bulmecisco 4:a975caedface 127 i2c.read(addr|1, cmd, 1);
bulmecisco 4:a975caedface 128 btns = cmd[0];
bulmecisco 4:a975caedface 129 if( btns & (BTN_FL|BTN_FM|BTN_FR))
bulmecisco 4:a975caedface 130 wert = false;
bulmecisco 4:a975caedface 131 else
bulmecisco 4:a975caedface 132 wert = true;
bulmecisco 4:a975caedface 133 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
bulmecisco 4:a975caedface 134 return wert;
bulmecisco 3:c7e5419fa980 135 }
bulmecisco 3:c7e5419fa980 136
bulmecisco 6:be710baf53ec 137 /*
bulmecisco 6:be710baf53ec 138 use in main():
bulmecisco 6:be710baf53ec 139 int left=0, right=0;
bulmecisco 6:be710baf53ec 140
bulmecisco 6:be710baf53ec 141 MoveMeasure(1000, left, right);
bulmecisco 6:be710baf53ec 142 pc.printf("\r\nleft= %d right= %d\r\n", left, right);
bulmecisco 6:be710baf53ec 143 */
bulmecisco 6:be710baf53ec 144 bool MoveMeasure(int time, int& left, int& right)
bulmecisco 4:a975caedface 145 {
bulmecisco 4:a975caedface 146 int count=0; //, left=0, right=0;
bulmecisco 4:a975caedface 147
bulmecisco 4:a975caedface 148 MotorR_EN=MotorL_EN=1;
bulmecisco 4:a975caedface 149 MotorR_FORWARD = MotorL_FORWARD = 1;
bulmecisco 1:f2d7bec926ce 150
bulmecisco 6:be710baf53ec 151 while(count<time) {
bulmecisco 4:a975caedface 152 MotorR_FORWARD = MotorL_FORWARD = 1;
bulmecisco 4:a975caedface 153 LED_D10 = SensorL;
bulmecisco 4:a975caedface 154 if(SensorL == 1)
bulmecisco 4:a975caedface 155 left++;
bulmecisco 4:a975caedface 156 if(SensorR == 1)
bulmecisco 4:a975caedface 157 right++;
bulmecisco 4:a975caedface 158 LED_D13 = SensorR;
bulmecisco 4:a975caedface 159 count++;
bulmecisco 4:a975caedface 160 wait_ms(1);
bulmecisco 3:c7e5419fa980 161 }
bulmecisco 4:a975caedface 162 DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);
bulmecisco 4:a975caedface 163
bulmecisco 4:a975caedface 164 MotorR_FORWARD = MotorL_FORWARD = 0;
bulmecisco 4:a975caedface 165 MotorR_EN=MotorL_EN=0;
bulmecisco 4:a975caedface 166 wait_ms(500);
bulmecisco 4:a975caedface 167 return true;
bulmecisco 1:f2d7bec926ce 168 }
bulmecisco 1:f2d7bec926ce 169
bulmecisco 4:a975caedface 170 bool MoveTicks(int ticks)
bulmecisco 4:a975caedface 171 {
bulmecisco 4:a975caedface 172 int count=0, left=0, right=0; //Variable count auf 0 setzen
Enenkel 0:0b7c22955b8c 173
bulmecisco 1:f2d7bec926ce 174 MotorR_EN=MotorL_EN=1; // Beide Motoren ENABLE
bulmecisco 4:a975caedface 175 while(ticks--) { // mache solang ticks
bulmecisco 4:a975caedface 176 MotorR_FORWARD = MotorL_FORWARD = 1; // Beide Motoren vorwärts EIN
bulmecisco 4:a975caedface 177 LED_D10 = SensorL; // LED D10 blinkt
bulmecisco 4:a975caedface 178 if(SensorL == 1)
bulmecisco 4:a975caedface 179 left++;
bulmecisco 4:a975caedface 180 if(SensorR == 1)
bulmecisco 4:a975caedface 181 right++;
bulmecisco 4:a975caedface 182 LED_D13 = SensorR; // LED D13 blinkt
bulmecisco 4:a975caedface 183 count++;
bulmecisco 4:a975caedface 184 wait_ms(1); // warte 1 mSekunde
bulmecisco 4:a975caedface 185 }
bulmecisco 4:a975caedface 186 DEBUG_PRINT("SensorL: %d SensorR: %d anzahl: %d\right\n", left, right, count);
bulmecisco 4:a975caedface 187
bulmecisco 1:f2d7bec926ce 188 MotorR_FORWARD = MotorL_FORWARD = 0; // Motoren AUS
bulmecisco 4:a975caedface 189 MotorR_EN=MotorL_EN=0;
bulmecisco 4:a975caedface 190 //wait_ms(250);
bulmecisco 4:a975caedface 191 return true;
bulmecisco 1:f2d7bec926ce 192 }
bulmecisco 4:a975caedface 193
bulmecisco 4:a975caedface 194 void TurnBack()
bulmecisco 1:f2d7bec926ce 195 {
bulmecisco 4:a975caedface 196 MotorR_EN=1;
bulmecisco 4:a975caedface 197
bulmecisco 4:a975caedface 198 MotorR_REVERSE = 1;
bulmecisco 4:a975caedface 199 wait_ms(750);
bulmecisco 4:a975caedface 200 MotorR_REVERSE = 0;
bulmecisco 4:a975caedface 201 MotorR_EN=0;
bulmecisco 4:a975caedface 202
bulmecisco 4:a975caedface 203 wait_ms(250);
bulmecisco 1:f2d7bec926ce 204 }
bulmecisco 3:c7e5419fa980 205
bulmecisco 4:a975caedface 206 void TurnLeftTicks(int ticks)
bulmecisco 4:a975caedface 207 {
bulmecisco 4:a975caedface 208 MotorR_EN=1;
bulmecisco 4:a975caedface 209 MotorL_EN=1;
bulmecisco 4:a975caedface 210 int left=0, right=0;
bulmecisco 4:a975caedface 211 while(ticks--) {
bulmecisco 4:a975caedface 212 MotorR_FORWARD = MotorL_REVERSE = 1;
bulmecisco 4:a975caedface 213 LED_D10 = SensorL;
bulmecisco 4:a975caedface 214 if(SensorL == 1)
bulmecisco 4:a975caedface 215 left++;
bulmecisco 4:a975caedface 216 if(SensorR == 1)
bulmecisco 4:a975caedface 217 right++;
bulmecisco 4:a975caedface 218 LED_D13 = SensorR;
bulmecisco 4:a975caedface 219 wait_ms(1);
bulmecisco 4:a975caedface 220 }
bulmecisco 4:a975caedface 221 DEBUG_PRINT("\right\nTurnLeft: SensorL: %d SensorR: %d \right\n", left, right);
bulmecisco 3:c7e5419fa980 222
bulmecisco 4:a975caedface 223 MotorL_REVERSE = MotorR_FORWARD = 0;
bulmecisco 4:a975caedface 224 MotorL_EN=MotorR_EN=0;
bulmecisco 4:a975caedface 225
bulmecisco 4:a975caedface 226 wait_ms(250);
bulmecisco 3:c7e5419fa980 227 }
bulmecisco 1:f2d7bec926ce 228
bulmecisco 4:a975caedface 229 void TurnLeftTime(int ms)
bulmecisco 1:f2d7bec926ce 230 {
bulmecisco 4:a975caedface 231 MotorR_EN=1; // motor right ENABLE
bulmecisco 4:a975caedface 232
bulmecisco 4:a975caedface 233 MotorR_FORWARD = 1; // motor right forward ON
bulmecisco 4:a975caedface 234 wait_ms(ms); // wait for ms
bulmecisco 4:a975caedface 235 MotorR_FORWARD = 0; // motor right OFF
bulmecisco 4:a975caedface 236 MotorR_EN=0;
bulmecisco 4:a975caedface 237
bulmecisco 4:a975caedface 238 wait_ms(250); // only to step the robot
bulmecisco 4:a975caedface 239 }
bulmecisco 4:a975caedface 240
bulmecisco 4:a975caedface 241 void TurnLeft()
bulmecisco 4:a975caedface 242 {
bulmecisco 4:a975caedface 243 MotorR_EN=1; // motor right ENABLE
bulmecisco 1:f2d7bec926ce 244
bulmecisco 4:a975caedface 245 MotorR_FORWARD = 1; // motor right forward ON
bulmecisco 4:a975caedface 246 wait_ms(500); // wait 500 ms (90°)
bulmecisco 4:a975caedface 247 MotorR_FORWARD = 0; // motor right forward OFF
bulmecisco 4:a975caedface 248 MotorR_EN=0;
bulmecisco 4:a975caedface 249
bulmecisco 4:a975caedface 250 wait_ms(250); // only to step the robot
bulmecisco 4:a975caedface 251 }
bulmecisco 4:a975caedface 252
bulmecisco 5:7b091d085c70 253 bool WaitUntilButtonPressed()
bulmecisco 5:7b091d085c70 254 {
bulmecisco 5:7b091d085c70 255 char cmd[3];
bulmecisco 5:7b091d085c70 256 int16_t btns;
bulmecisco 5:7b091d085c70 257 bool wert;
bulmecisco 5:7b091d085c70 258
bulmecisco 5:7b091d085c70 259 cmd[0] = 0x06;
bulmecisco 5:7b091d085c70 260 cmd[1] = 0x00;
bulmecisco 5:7b091d085c70 261 i2c.write(addr, cmd, 2); // Define Port0 = Out
bulmecisco 5:7b091d085c70 262
bulmecisco 5:7b091d085c70 263 cmd[0]=0x01;
bulmecisco 5:7b091d085c70 264 i2c.write(addr, cmd, 1);
bulmecisco 5:7b091d085c70 265 i2c.read(addr|1, cmd, 1);
bulmecisco 5:7b091d085c70 266 btns = cmd[0];
bulmecisco 5:7b091d085c70 267 if( btns & (0xFF))
bulmecisco 5:7b091d085c70 268 wert = false;
bulmecisco 5:7b091d085c70 269 else
bulmecisco 5:7b091d085c70 270 wert = true;
bulmecisco 5:7b091d085c70 271 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
bulmecisco 5:7b091d085c70 272 return wert;
bulmecisco 5:7b091d085c70 273 }
bulmecisco 5:7b091d085c70 274
bulmecisco 4:a975caedface 275 void TurnLedOn(int16_t led)
bulmecisco 4:a975caedface 276 {
bulmecisco 4:a975caedface 277 char cmd[3];
bulmecisco 4:a975caedface 278
bulmecisco 4:a975caedface 279 cmd[0] = 0x02;
bulmecisco 4:a975caedface 280 cmd[1] = ~led;
bulmecisco 4:a975caedface 281 i2c.write(addr, cmd, 2);
bulmecisco 4:a975caedface 282 wait(0.5);
bulmecisco 4:a975caedface 283 }
bulmecisco 1:f2d7bec926ce 284
bulmecisco 4:a975caedface 285 void TurnLedOff(int16_t led)
bulmecisco 4:a975caedface 286 {
bulmecisco 4:a975caedface 287 char cmd[3];
bulmecisco 4:a975caedface 288
bulmecisco 4:a975caedface 289 cmd[0] = 0x02;
bulmecisco 4:a975caedface 290 cmd[1] = led;
bulmecisco 4:a975caedface 291 i2c.write(addr, cmd, 2);
bulmecisco 4:a975caedface 292 wait(0.5);
bulmecisco 4:a975caedface 293 }
bulmecisco 4:a975caedface 294
bulmecisco 5:7b091d085c70 295 /*
bulmecisco 4:a975caedface 296 void TurnAllLedOff()
bulmecisco 4:a975caedface 297 {
bulmecisco 4:a975caedface 298 TurnLedOff(LED_ALL);
bulmecisco 4:a975caedface 299 }
bulmecisco 4:a975caedface 300
bulmecisco 4:a975caedface 301 void TurnAllLedOn()
bulmecisco 4:a975caedface 302 {
bulmecisco 4:a975caedface 303 TurnLedOn(LED_ALL);
bulmecisco 2:43547160ab56 304 }
bulmecisco 4:a975caedface 305
bulmecisco 4:a975caedface 306 void MoveMore(int anzahl)
bulmecisco 4:a975caedface 307 {
bulmecisco 4:a975caedface 308 for(int i=0; i < anzahl; i++)
bulmecisco 4:a975caedface 309 Move();
bulmecisco 4:a975caedface 310 }
bulmecisco 4:a975caedface 311
bulmecisco 4:a975caedface 312 void Back()
bulmecisco 2:43547160ab56 313 {
bulmecisco 4:a975caedface 314 TurnLeftTicks(150);
bulmecisco 4:a975caedface 315 TurnLeftTicks(150);
bulmecisco 4:a975caedface 316 }
bulmecisco 4:a975caedface 317
bulmecisco 4:a975caedface 318 void MoveWhileFrontIsClear()
bulmecisco 4:a975caedface 319 {
bulmecisco 4:a975caedface 320 while(FrontIsClear()) {
bulmecisco 4:a975caedface 321 MoveTicks(30);
bulmecisco 4:a975caedface 322 }
bulmecisco 4:a975caedface 323 }
bulmecisco 4:a975caedface 324
bulmecisco 5:7b091d085c70 325 void LedTest()
bulmecisco 1:f2d7bec926ce 326 {
bulmecisco 4:a975caedface 327 TurnLedOn(LED_FL1 |LED_FR1);
bulmecisco 4:a975caedface 328 wait(1);
bulmecisco 4:a975caedface 329 TurnAllLedOn();
bulmecisco 4:a975caedface 330 wait(1);
bulmecisco 4:a975caedface 331 TurnLedOff(LED_FL1 |LED_FR1);
bulmecisco 1:f2d7bec926ce 332 }
bulmecisco 5:7b091d085c70 333 */