BERTL_CHEL18 / Mbed 2 deprecated Func_Bertl_Ultra

ultra sonic sensor asdded

Dependencies:   HCSR mbed

Fork of func_Bertl by BULME_BERTL14

main.cpp@4:a975caedface, 2015-01-11 (annotated)

Committer:
bulmecisco
Date:
Sun Jan 11 08:42:08 2015 +0000
Revision:
4:a975caedface
Parent:
3:c7e5419fa980
Child:
5:7b091d085c70
ur_Karel v 0.2 OK

Who changed what in which revision?

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