BERTL_CHEL18 / Mbed 2 deprecated Func_Bertl_Ultra

ultra sonic sensor asdded

Dependencies:   HCSR mbed

Fork of func_Bertl by BULME_BERTL14

main.cpp@6:be710baf53ec, 2015-01-22 (annotated)

Committer:
bulmecisco
Date:
Thu Jan 22 13:44:59 2015 +0000
Revision:
6:be710baf53ec
Parent:
5:7b091d085c70
Child:
7:cb6947e1f1d3
MoveMeasure Bug fixed.

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