Immanuel Rechar / Bertl

Bau

Dependencies:   HCSR

Fork of Bertl by Franz Pucher

ur_Bertl.cpp@0:66e9a0afcbd6, 2015-02-26 (annotated)

Committer:
bulmecisco
Date:
Thu Feb 26 10:12:06 2015 +0000
Revision:
0:66e9a0afcbd6
Child:
1:fafbac0ba96d
ur_Bertl based on simple class design with documentaton

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bulmecisco 0:66e9a0afcbd6 1 /***********************************
bulmecisco 0:66e9a0afcbd6 2 name: ur_Bertl.cpp Version: 1.1
bulmecisco 0:66e9a0afcbd6 3 author: PE HTL BULME
bulmecisco 0:66e9a0afcbd6 4 email: pe@bulme.at
bulmecisco 0:66e9a0afcbd6 5 description:
bulmecisco 0:66e9a0afcbd6 6 Implementation portion of class ur_Bertl The Robot
bulmecisco 0:66e9a0afcbd6 7 Blue LED and RGB LED test added
bulmecisco 0:66e9a0afcbd6 8
bulmecisco 0:66e9a0afcbd6 9 ***********************************/
bulmecisco 0:66e9a0afcbd6 10 #include "mbed.h"
bulmecisco 0:66e9a0afcbd6 11 #include "config.h"
bulmecisco 0:66e9a0afcbd6 12 #include "ur_Bertl.h"
bulmecisco 0:66e9a0afcbd6 13
bulmecisco 0:66e9a0afcbd6 14 // Constructor
bulmecisco 0:66e9a0afcbd6 15 ur_Bertl::ur_Bertl() : _interrupt(P1_12) // left sensor P1_13
bulmecisco 0:66e9a0afcbd6 16 {
bulmecisco 0:66e9a0afcbd6 17 i2c.frequency(40000); // I2C Frequenz 40kHz
bulmecisco 0:66e9a0afcbd6 18 mg1 = mg2 = SPEED;
bulmecisco 0:66e9a0afcbd6 19 _interrupt.rise(this, &ur_Bertl::increment); // attach increment function of this counter instance ie. motor sensor
bulmecisco 0:66e9a0afcbd6 20 _count = 0;
bulmecisco 0:66e9a0afcbd6 21 beepersInBag = 0;
bulmecisco 0:66e9a0afcbd6 22 }
bulmecisco 0:66e9a0afcbd6 23
bulmecisco 0:66e9a0afcbd6 24 ur_Bertl::ur_Bertl(PinName pin) : _interrupt(pin) // create the InterruptIn on the pin specified to Counter
bulmecisco 0:66e9a0afcbd6 25 {
bulmecisco 0:66e9a0afcbd6 26 i2c.frequency(40000); // I2C Frequenz 40kHz
bulmecisco 0:66e9a0afcbd6 27 mg1 = mg2 = SPEED;
bulmecisco 0:66e9a0afcbd6 28 _interrupt.rise(this, &ur_Bertl::increment); // attach increment function of this counter instance ie. motor sensor
bulmecisco 0:66e9a0afcbd6 29 _count = 0;
bulmecisco 0:66e9a0afcbd6 30 beepersInBag = 0;
bulmecisco 0:66e9a0afcbd6 31 }
bulmecisco 0:66e9a0afcbd6 32
bulmecisco 0:66e9a0afcbd6 33
bulmecisco 0:66e9a0afcbd6 34 // Pulblic methodes
bulmecisco 0:66e9a0afcbd6 35 void ur_Bertl::Move()
bulmecisco 0:66e9a0afcbd6 36 {
bulmecisco 0:66e9a0afcbd6 37 int count = _count;
bulmecisco 0:66e9a0afcbd6 38 MotorR_EN=MotorL_EN=1; // both motor ENABLE
bulmecisco 0:66e9a0afcbd6 39 MotorR_FORWARD = MotorL_FORWARD = 1; // both motor forward ON
bulmecisco 0:66e9a0afcbd6 40
bulmecisco 0:66e9a0afcbd6 41 while(_count < count+DISTANCE) {
bulmecisco 0:66e9a0afcbd6 42 //if(!FrontIsClear()) // more convenient because there are no accidents :-)
bulmecisco 0:66e9a0afcbd6 43 // break;
bulmecisco 0:66e9a0afcbd6 44 #ifdef FRONTBUTTON
bulmecisco 0:66e9a0afcbd6 45 if(frontButtonPressed())
bulmecisco 0:66e9a0afcbd6 46 error();
bulmecisco 0:66e9a0afcbd6 47 #endif
bulmecisco 0:66e9a0afcbd6 48 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 0:66e9a0afcbd6 49 }
bulmecisco 0:66e9a0afcbd6 50 MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off
bulmecisco 0:66e9a0afcbd6 51 MotorR_EN=MotorL_EN=0;
bulmecisco 0:66e9a0afcbd6 52 wait_ms(250);
bulmecisco 0:66e9a0afcbd6 53 }
bulmecisco 0:66e9a0afcbd6 54
bulmecisco 0:66e9a0afcbd6 55 void ur_Bertl::PutBeeper()
bulmecisco 0:66e9a0afcbd6 56 {
bulmecisco 0:66e9a0afcbd6 57 // wait_ms(500);
bulmecisco 0:66e9a0afcbd6 58 if(beepersInBag > 0)
bulmecisco 0:66e9a0afcbd6 59 beepersInBag--;
bulmecisco 0:66e9a0afcbd6 60 else
bulmecisco 0:66e9a0afcbd6 61 error();
bulmecisco 0:66e9a0afcbd6 62 }
bulmecisco 0:66e9a0afcbd6 63
bulmecisco 0:66e9a0afcbd6 64 void ur_Bertl::PickBeeper()
bulmecisco 0:66e9a0afcbd6 65 {
bulmecisco 0:66e9a0afcbd6 66 // wait_ms(500);
bulmecisco 0:66e9a0afcbd6 67 if (linesensor)
bulmecisco 0:66e9a0afcbd6 68 beepersInBag++;
bulmecisco 0:66e9a0afcbd6 69 else
bulmecisco 0:66e9a0afcbd6 70 error();
bulmecisco 0:66e9a0afcbd6 71 }
bulmecisco 0:66e9a0afcbd6 72
bulmecisco 0:66e9a0afcbd6 73 void ur_Bertl::TurnLeft()
bulmecisco 0:66e9a0afcbd6 74 {
bulmecisco 0:66e9a0afcbd6 75 int count = _count;
bulmecisco 0:66e9a0afcbd6 76 MotorR_EN=MotorL_EN=1; // motor left and right ENABLE
bulmecisco 0:66e9a0afcbd6 77
bulmecisco 0:66e9a0afcbd6 78 MotorR_FORWARD = MotorL_REVERSE = 1;
bulmecisco 0:66e9a0afcbd6 79 while(_count < count+ANGLE) {
bulmecisco 0:66e9a0afcbd6 80 #ifdef FRONTBUTTON
bulmecisco 0:66e9a0afcbd6 81 if(frontButtonPressed()) // get out if to much problems
bulmecisco 0:66e9a0afcbd6 82 error();
bulmecisco 0:66e9a0afcbd6 83 #endif
bulmecisco 0:66e9a0afcbd6 84 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 0:66e9a0afcbd6 85 }
bulmecisco 0:66e9a0afcbd6 86 MotorR_FORWARD = MotorL_REVERSE = 0;
bulmecisco 0:66e9a0afcbd6 87 MotorR_EN=MotorL_EN=0;
bulmecisco 0:66e9a0afcbd6 88 wait_ms(250); // only to step the robot
bulmecisco 0:66e9a0afcbd6 89 }
bulmecisco 0:66e9a0afcbd6 90
bulmecisco 0:66e9a0afcbd6 91 bool ur_Bertl::WaitUntilButtonPressed()
bulmecisco 0:66e9a0afcbd6 92 {
bulmecisco 0:66e9a0afcbd6 93 char cmd[3];
bulmecisco 0:66e9a0afcbd6 94 int16_t btns;
bulmecisco 0:66e9a0afcbd6 95 bool wert;
bulmecisco 0:66e9a0afcbd6 96
bulmecisco 0:66e9a0afcbd6 97 RGB_blue=RGB_red=RGB_green=0;
bulmecisco 0:66e9a0afcbd6 98 cmd[0] = 0x06;
bulmecisco 0:66e9a0afcbd6 99 cmd[1] = 0x00;
bulmecisco 0:66e9a0afcbd6 100 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 101
bulmecisco 0:66e9a0afcbd6 102 cmd[0]=0x01;
bulmecisco 0:66e9a0afcbd6 103 i2c.write(addr, cmd, 1);
bulmecisco 0:66e9a0afcbd6 104 i2c.read(addr|1, cmd, 1);
bulmecisco 0:66e9a0afcbd6 105 btns = cmd[0];
bulmecisco 0:66e9a0afcbd6 106 if( btns & (0xFF))
bulmecisco 0:66e9a0afcbd6 107 wert = false;
bulmecisco 0:66e9a0afcbd6 108 else
bulmecisco 0:66e9a0afcbd6 109 wert = true;
bulmecisco 0:66e9a0afcbd6 110 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
bulmecisco 0:66e9a0afcbd6 111 return wert;
bulmecisco 0:66e9a0afcbd6 112 }
bulmecisco 0:66e9a0afcbd6 113
bulmecisco 0:66e9a0afcbd6 114 void ur_Bertl::ShutOff()
bulmecisco 0:66e9a0afcbd6 115 {
bulmecisco 0:66e9a0afcbd6 116 MotorR_FORWARD = MotorL_FORWARD = 0; // motor OFF
bulmecisco 0:66e9a0afcbd6 117 MotorR_EN=MotorL_EN=0; // motor disable
bulmecisco 0:66e9a0afcbd6 118 }
bulmecisco 0:66e9a0afcbd6 119
bulmecisco 0:66e9a0afcbd6 120 // LEDs methodes
bulmecisco 0:66e9a0afcbd6 121
bulmecisco 0:66e9a0afcbd6 122 void ur_Bertl::BlueLedsON()
bulmecisco 0:66e9a0afcbd6 123 {
bulmecisco 0:66e9a0afcbd6 124 LED_blue=0;
bulmecisco 0:66e9a0afcbd6 125 }
bulmecisco 0:66e9a0afcbd6 126
bulmecisco 0:66e9a0afcbd6 127 void ur_Bertl::BlueLedsOFF()
bulmecisco 0:66e9a0afcbd6 128 {
bulmecisco 0:66e9a0afcbd6 129 LED_blue=1;
bulmecisco 0:66e9a0afcbd6 130 }
bulmecisco 0:66e9a0afcbd6 131
bulmecisco 0:66e9a0afcbd6 132 void ur_Bertl::RGBLed(bool red, bool green, bool blue)
bulmecisco 0:66e9a0afcbd6 133 {
bulmecisco 0:66e9a0afcbd6 134 RGB_blue=!blue;
bulmecisco 0:66e9a0afcbd6 135 RGB_red=!red;
bulmecisco 0:66e9a0afcbd6 136 RGB_green=!green;
bulmecisco 0:66e9a0afcbd6 137 }
bulmecisco 0:66e9a0afcbd6 138
bulmecisco 0:66e9a0afcbd6 139 void ur_Bertl::TurnLedOn(int16_t led)
bulmecisco 0:66e9a0afcbd6 140 {
bulmecisco 0:66e9a0afcbd6 141 char cmd[3];
bulmecisco 0:66e9a0afcbd6 142
bulmecisco 0:66e9a0afcbd6 143 cmd[0] = 0x02;
bulmecisco 0:66e9a0afcbd6 144 cmd[1] = ~led;
bulmecisco 0:66e9a0afcbd6 145 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 146 wait(0.5);
bulmecisco 0:66e9a0afcbd6 147 }
bulmecisco 0:66e9a0afcbd6 148
bulmecisco 0:66e9a0afcbd6 149 void ur_Bertl::TurnLedOff(int16_t led)
bulmecisco 0:66e9a0afcbd6 150 {
bulmecisco 0:66e9a0afcbd6 151 char cmd[3];
bulmecisco 0:66e9a0afcbd6 152
bulmecisco 0:66e9a0afcbd6 153 cmd[0] = 0x02;
bulmecisco 0:66e9a0afcbd6 154 cmd[1] = led;
bulmecisco 0:66e9a0afcbd6 155 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 156 wait(0.5);
bulmecisco 0:66e9a0afcbd6 157 }
bulmecisco 0:66e9a0afcbd6 158
bulmecisco 0:66e9a0afcbd6 159 void ur_Bertl::NibbleLeds(int value)
bulmecisco 0:66e9a0afcbd6 160 {
bulmecisco 0:66e9a0afcbd6 161 NibbleLEDs = value%16;
bulmecisco 0:66e9a0afcbd6 162 }
bulmecisco 0:66e9a0afcbd6 163
bulmecisco 0:66e9a0afcbd6 164 //-----------------INTERNAL USE ONLY ----------------------------
bulmecisco 0:66e9a0afcbd6 165 void ur_Bertl::error()
bulmecisco 0:66e9a0afcbd6 166 {
bulmecisco 0:66e9a0afcbd6 167 int wait = 500;
bulmecisco 0:66e9a0afcbd6 168 MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off
bulmecisco 0:66e9a0afcbd6 169 MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off
bulmecisco 0:66e9a0afcbd6 170 MotorR_EN=MotorL_EN=0;
bulmecisco 0:66e9a0afcbd6 171 while(1) {
bulmecisco 0:66e9a0afcbd6 172 TurnLedOff(0xFF);
bulmecisco 0:66e9a0afcbd6 173 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0;
bulmecisco 0:66e9a0afcbd6 174 LED_blue=1;
bulmecisco 0:66e9a0afcbd6 175 RGB_blue=RGB_green=RGB_red=1;
bulmecisco 0:66e9a0afcbd6 176 wait_ms(wait);
bulmecisco 0:66e9a0afcbd6 177 TurnLedOn(0xFF);
bulmecisco 0:66e9a0afcbd6 178 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
bulmecisco 0:66e9a0afcbd6 179 LED_blue=0;
bulmecisco 0:66e9a0afcbd6 180 RGB_blue=RGB_green=1;RGB_red=0;
bulmecisco 0:66e9a0afcbd6 181 wait_ms(wait);
bulmecisco 0:66e9a0afcbd6 182 }
bulmecisco 0:66e9a0afcbd6 183 }
bulmecisco 0:66e9a0afcbd6 184
bulmecisco 0:66e9a0afcbd6 185 bool ur_Bertl::frontButtonPressed()
bulmecisco 0:66e9a0afcbd6 186 {
bulmecisco 0:66e9a0afcbd6 187 char cmd[3]; // array for I2C
bulmecisco 0:66e9a0afcbd6 188 int16_t btns;
bulmecisco 0:66e9a0afcbd6 189 bool wert;
bulmecisco 0:66e9a0afcbd6 190
bulmecisco 0:66e9a0afcbd6 191 cmd[0] = 0x06;
bulmecisco 0:66e9a0afcbd6 192 cmd[1] = 0x00;
bulmecisco 0:66e9a0afcbd6 193 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 194
bulmecisco 0:66e9a0afcbd6 195 cmd[0]=0x01;
bulmecisco 0:66e9a0afcbd6 196 i2c.write(addr, cmd, 1);
bulmecisco 0:66e9a0afcbd6 197 i2c.read(addr|1, cmd, 1);
bulmecisco 0:66e9a0afcbd6 198 btns = cmd[0];
bulmecisco 0:66e9a0afcbd6 199 if( btns & (BTN_FL|BTN_FM|BTN_FR|BTN_FRR|BTN_FLL))
bulmecisco 0:66e9a0afcbd6 200 wert = true;
bulmecisco 0:66e9a0afcbd6 201 else
bulmecisco 0:66e9a0afcbd6 202 wert = false;
bulmecisco 0:66e9a0afcbd6 203 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 0:66e9a0afcbd6 204 return wert;
bulmecisco 0:66e9a0afcbd6 205 }
bulmecisco 0:66e9a0afcbd6 206
bulmecisco 0:66e9a0afcbd6 207 // ISR
bulmecisco 0:66e9a0afcbd6 208
bulmecisco 0:66e9a0afcbd6 209 void ur_Bertl::increment()
bulmecisco 0:66e9a0afcbd6 210 {
bulmecisco 0:66e9a0afcbd6 211 _count++;
bulmecisco 0:66e9a0afcbd6 212 }
bulmecisco 0:66e9a0afcbd6 213
bulmecisco 0:66e9a0afcbd6 214 int ur_Bertl::Read()
bulmecisco 0:66e9a0afcbd6 215 {
bulmecisco 0:66e9a0afcbd6 216 return _count;
bulmecisco 0:66e9a0afcbd6 217 }
bulmecisco 0:66e9a0afcbd6 218
bulmecisco 0:66e9a0afcbd6 219 //----------------------------------------------------------------------
bulmecisco 0:66e9a0afcbd6 220 /*
bulmecisco 0:66e9a0afcbd6 221 bool ur_Bertl::FrontIsClear()
bulmecisco 0:66e9a0afcbd6 222 {
bulmecisco 0:66e9a0afcbd6 223 int dist = 0;
bulmecisco 0:66e9a0afcbd6 224 usensor.start();
bulmecisco 0:66e9a0afcbd6 225 wait_ms(10);
bulmecisco 0:66e9a0afcbd6 226 dist=usensor.get_dist_cm();
bulmecisco 0:66e9a0afcbd6 227 if(dist < 5)
bulmecisco 0:66e9a0afcbd6 228 return false;
bulmecisco 0:66e9a0afcbd6 229 else
bulmecisco 0:66e9a0afcbd6 230 return true;
bulmecisco 0:66e9a0afcbd6 231 DEBUG_PRINT("Distance: %d", dist);
bulmecisco 0:66e9a0afcbd6 232
bulmecisco 0:66e9a0afcbd6 233 // if there is no ultra sonic sensor use this - with front buttons
bulmecisco 0:66e9a0afcbd6 234 char cmd[3]; // array for I2C
bulmecisco 0:66e9a0afcbd6 235 int16_t btns;
bulmecisco 0:66e9a0afcbd6 236 bool wert;
bulmecisco 0:66e9a0afcbd6 237
bulmecisco 0:66e9a0afcbd6 238 cmd[0] = 0x06;
bulmecisco 0:66e9a0afcbd6 239 cmd[1] = 0x00;
bulmecisco 0:66e9a0afcbd6 240 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 241
bulmecisco 0:66e9a0afcbd6 242 cmd[0]=0x01;
bulmecisco 0:66e9a0afcbd6 243 i2c.write(addr, cmd, 1);
bulmecisco 0:66e9a0afcbd6 244 i2c.read(addr|1, cmd, 1);
bulmecisco 0:66e9a0afcbd6 245 btns = cmd[0];
bulmecisco 0:66e9a0afcbd6 246 if( btns & (BTN_FL|BTN_FM|BTN_FR))
bulmecisco 0:66e9a0afcbd6 247 wert = false;
bulmecisco 0:66e9a0afcbd6 248 else
bulmecisco 0:66e9a0afcbd6 249 wert = true;
bulmecisco 0:66e9a0afcbd6 250 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 0:66e9a0afcbd6 251 return wert;
bulmecisco 0:66e9a0afcbd6 252 }
bulmecisco 0:66e9a0afcbd6 253
bulmecisco 0:66e9a0afcbd6 254
bulmecisco 0:66e9a0afcbd6 255 bool ur_Bertl::NextToABeeper()
bulmecisco 0:66e9a0afcbd6 256 {
bulmecisco 0:66e9a0afcbd6 257 if (BottomIsBlack())
bulmecisco 0:66e9a0afcbd6 258 return true;
bulmecisco 0:66e9a0afcbd6 259 else
bulmecisco 0:66e9a0afcbd6 260 return false;
bulmecisco 0:66e9a0afcbd6 261 }
bulmecisco 0:66e9a0afcbd6 262
bulmecisco 0:66e9a0afcbd6 263 int ur_Bertl::AnyBeeperInBag()
bulmecisco 0:66e9a0afcbd6 264 {
bulmecisco 0:66e9a0afcbd6 265 if(beepersInBag > 0)
bulmecisco 0:66e9a0afcbd6 266 return beepersInBag;
bulmecisco 0:66e9a0afcbd6 267 else
bulmecisco 0:66e9a0afcbd6 268 return 0;
bulmecisco 0:66e9a0afcbd6 269 }
bulmecisco 0:66e9a0afcbd6 270
bulmecisco 0:66e9a0afcbd6 271 bool ur_Bertl::backIsClear()
bulmecisco 0:66e9a0afcbd6 272 {
bulmecisco 0:66e9a0afcbd6 273 char cmd[3]; // array for I2C
bulmecisco 0:66e9a0afcbd6 274 int16_t btns;
bulmecisco 0:66e9a0afcbd6 275 bool wert;
bulmecisco 0:66e9a0afcbd6 276
bulmecisco 0:66e9a0afcbd6 277 cmd[0] = 0x06;
bulmecisco 0:66e9a0afcbd6 278 cmd[1] = 0x00;
bulmecisco 0:66e9a0afcbd6 279 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 280
bulmecisco 0:66e9a0afcbd6 281 cmd[0]=0x01;
bulmecisco 0:66e9a0afcbd6 282 i2c.write(addr, cmd, 1);
bulmecisco 0:66e9a0afcbd6 283 i2c.read(addr|1, cmd, 1);
bulmecisco 0:66e9a0afcbd6 284 btns = cmd[0];
bulmecisco 0:66e9a0afcbd6 285 if( btns & (BTN_BL|BTN_BM|BTN_BR))
bulmecisco 0:66e9a0afcbd6 286 wert = false;
bulmecisco 0:66e9a0afcbd6 287 else
bulmecisco 0:66e9a0afcbd6 288 wert = true;
bulmecisco 0:66e9a0afcbd6 289 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 0:66e9a0afcbd6 290 return wert;
bulmecisco 0:66e9a0afcbd6 291 }
bulmecisco 0:66e9a0afcbd6 292
bulmecisco 0:66e9a0afcbd6 293 int ur_Bertl::BottomIsBlack()
bulmecisco 0:66e9a0afcbd6 294 {
bulmecisco 0:66e9a0afcbd6 295 int detect;
bulmecisco 0:66e9a0afcbd6 296
bulmecisco 0:66e9a0afcbd6 297 detect = linesensor;
bulmecisco 0:66e9a0afcbd6 298 return detect;
bulmecisco 0:66e9a0afcbd6 299 }
bulmecisco 0:66e9a0afcbd6 300
bulmecisco 0:66e9a0afcbd6 301 void ur_Bertl::MoveBackwards()
bulmecisco 0:66e9a0afcbd6 302 {
bulmecisco 0:66e9a0afcbd6 303 int count = _count;
bulmecisco 0:66e9a0afcbd6 304 //wait_ms(250); // waite until Bertl stops
bulmecisco 0:66e9a0afcbd6 305 MotorR_EN=MotorL_EN=1; // both motor ENABLE
bulmecisco 0:66e9a0afcbd6 306 MotorR_REVERSE = MotorL_REVERSE = 1; // both motor backwards ON
bulmecisco 0:66e9a0afcbd6 307 while(_count < count+DISTANCE) {
bulmecisco 0:66e9a0afcbd6 308 if(!backIsClear())
bulmecisco 0:66e9a0afcbd6 309 break;
bulmecisco 0:66e9a0afcbd6 310 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 0:66e9a0afcbd6 311 }
bulmecisco 0:66e9a0afcbd6 312 MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off
bulmecisco 0:66e9a0afcbd6 313 MotorR_EN=MotorL_EN=0;
bulmecisco 0:66e9a0afcbd6 314 wait_ms(250);
bulmecisco 0:66e9a0afcbd6 315 }
bulmecisco 0:66e9a0afcbd6 316 */