BERTL_CHEL18 / Mbed 2 deprecated Func_Bertl_Ultra

ultra sonic sensor asdded

Dependencies:   HCSR mbed

Fork of func_Bertl by BULME_BERTL14

main.cpp@5:7b091d085c70, 2015-01-15 (annotated)

Committer:
bulmecisco
Date:
Thu Jan 15 10:18:16 2015 +0000
Revision:
5:7b091d085c70
Parent:
4:a975caedface
Child:
6:be710baf53ec
Function Bertl OK

Who changed what in which revision?

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