Dr. Pucher / ur_Bertl

Class Bertl added

Dependencies:   HCSR

Dependents:   BertlDrive_V2 BertlDrive_V2

ur_Bertl.cpp@3:01b183fe8b41, 2015-03-26 (annotated)

Committer:
bulmecisco
Date:
Thu Mar 26 13:06:00 2015 +0000
Revision:
3:01b183fe8b41
Parent:
2:1cd559ff516b
Child:
4:76acfddc26fb
I2C Updated for LED problems

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