ur_Bertl.cpp

00001 /***********************************
00002 name:   ur_Bertl.cpp    Version: 3.0
00003         class Bertl included
00004 author: PE HTL BULME
00005 email:  pe@bulme.at
00006 WIKI:   https://developer.mbed.org/teams/BERTL_CHEL_18/code/ur_Bertl/
00007 description:
00008         Implementation portion of class ur_Bertl The Robot 
00009         boolean commands added for if/else, while, ...   
00010         int ReturnButtonPressed() added which returns the int value of button pressed    
00011 ***********************************/
00012 #include "mbed.h"
00013 #include "config.h"
00014 #include "ur_Bertl.h"
00015 
00016 // Constructor
00017 ur_Bertl::ur_Bertl() : _interrupt(MOTORENC)         
00018 {
00019     MotorSpg = 1;
00020     IncrementalgeberSpg = 1;
00021     LinienSensorSpg = 1;
00022 
00023     i2c.frequency(40000);                           // I2C init
00024     char init1[2] = {0x6, 0x00};
00025     char init2[2] = {0x7, 0xff};
00026     i2c.write(0x40, init1, 2);
00027     i2c.write(0x40, init2, 2);
00028     mg1 = mg2 = SPEED;
00029     _interrupt.rise(this, &ur_Bertl::increment);    // attach increment function of this counter instance ie. motor sensor
00030     _count = 0;
00031     beepersInBag = 0;
00032     TurnLedOff(0xFF);
00033 }
00034 
00035 ur_Bertl::ur_Bertl(PinName pin) :  _interrupt(pin)  // create the InterruptIn on the pin specified to Counter
00036 {
00037     i2c.frequency(40000);                           // I2C init
00038     char init1[2] = {0x6, 0x00};
00039     char init2[2] = {0x7, 0xff};
00040     i2c.write(0x40, init1, 2);
00041     i2c.write(0x40, init2, 2);
00042     mg1 = SPEED;
00043     mg2 = SPEED * OFFSET;
00044     _interrupt.rise(this, &ur_Bertl::increment);    // attach increment function of this counter instance ie. motor sensor
00045     _count = 0;
00046     beepersInBag = 0;
00047    TurnLedOff(0xFF);
00048 }
00049 
00050 // Pulblic methodes
00051 void ur_Bertl::Move()
00052 {
00053     int count = _count;
00054     MotorR_EN=MotorL_EN=1;                  // both motor ENABLE
00055     MotorR_FORWARD = MotorL_FORWARD = 1;    // both motor forward ON
00056 #ifdef TIME
00057     wait_ms(MOVE);
00058 #else
00059 
00060     while(_count < count+DISTANCE) {        // DISTANCE maybe change to move
00061         //if(!FrontIsClear())       // more convenient because there are no accidents :-)
00062           //  break;
00063 #ifdef FRONTBUTTON
00064         if(frontButtonPressed())
00065             error();
00066 #endif
00067         DEBUG_PRINT("count: %d _count: %d", count, _count);
00068     }
00069 #endif
00070     MotorR_FORWARD = MotorL_FORWARD = 0;    // both motor off
00071     MotorR_EN=MotorL_EN=0;
00072     //if(move == MOVE)
00073         wait_ms(250);
00074 }
00075 
00076 void ur_Bertl::PutBeeper()
00077 {
00078 //    wait_ms(500);
00079     if(beepersInBag > 0)
00080         beepersInBag--;
00081     else
00082         error();
00083 }
00084 
00085 void ur_Bertl::PickBeeper()
00086 {
00087 //    wait_ms(500);
00088     if (linesensor)
00089         beepersInBag++;
00090     else 
00091         error();   
00092     if(beepersInBag > 16)
00093         error();
00094 }
00095 
00096 void ur_Bertl::TurnLeft()
00097 {
00098     int count = _count;
00099     MotorR_EN=MotorL_EN=1;                  // motor left and right ENABLE
00100 
00101     MotorR_FORWARD = MotorL_REVERSE = 1;
00102 #ifdef TIME
00103     wait_ms(TURN);
00104 #else
00105 
00106     while(_count < count+ANGLE) {
00107 #ifdef FRONTBUTTON
00108         if(frontButtonPressed())            // get out if to much problems
00109             error();
00110 #endif
00111         DEBUG_PRINT("count: %d _count: %d", count, _count);
00112     }
00113 #endif
00114     MotorR_FORWARD = MotorL_REVERSE = 0;
00115     MotorR_EN=MotorL_EN=0;
00116     wait_ms(250);           // only to step the robot
00117 }
00118 
00119 void ur_Bertl::TurnOff()
00120 {
00121     MotorR_FORWARD = MotorL_FORWARD = 0;    // motor OFF
00122     MotorR_EN=MotorL_EN=0;                  // motor disable
00123     MotorSpg = 0;
00124     IncrementalgeberSpg = 0;
00125     LinienSensorSpg = 0;
00126     BlueLedsOFF();
00127     TurnLedOff(LED_ALL);
00128     NibbleLeds(0x00);
00129 }
00130 
00131 // Public LEDs methodes
00132 
00133 void ur_Bertl::BlueLedsON()
00134 {
00135       LED_blue=0;
00136 }
00137 
00138 void ur_Bertl::BlueLedsOFF()
00139 {
00140       LED_blue=1;
00141 }
00142 
00143 void ur_Bertl::RGBLed(bool red, bool green, bool blue)
00144 {
00145       RGB_blue=!blue;
00146       RGB_red=!red;
00147       RGB_green=!green;
00148 }
00149 
00150 void ur_Bertl::TurnLedOn(int16_t led)
00151 {
00152     leds = leds | led;
00153     char cmd[3];
00154 
00155     if(led == 0xBB) {
00156         LED_blue=0;
00157         return;
00158     } else if (led == 0xFF) {
00159         RGBLed(1,1,1);
00160         LED_blue=0;
00161     }
00162     cmd[0] = 0x02;
00163     cmd[1] = ~leds;
00164     i2c.write(addr, cmd, 2);
00165     wait(0.5);
00166 }
00167 
00168 void ur_Bertl::TurnLedOff(int16_t led)
00169 {
00170     leds = leds & ~led;
00171     char cmd[3];
00172     if(led == 0xBB) {
00173         LED_blue=1;
00174         return;
00175     } else if (led == 0xFF) {
00176         RGBLed(0,0,0);      //don't work?
00177         LED_blue=1;
00178     }
00179     cmd[0] = 0x02;
00180     cmd[1] = ~leds;
00181     i2c.write(addr, cmd, 2);
00182     wait(0.5);
00183 }
00184 
00185 void ur_Bertl::NibbleLeds(int value)
00186 {
00187     NibbleLEDs = value%16;
00188 }
00189 
00190 //----------------------------------------------------------------------
00191 
00192 bool ur_Bertl::FrontIsClear()
00193 {
00194 #ifdef HCSR
00195     int dist = 0;
00196     usensor.start();
00197     wait_ms(10);
00198     dist=usensor.get_dist_cm();
00199     if(dist < ULTRASONIC_DISTANCE)
00200         return false;
00201     else
00202         return true;
00203 #else
00204 // if there is no ultra sonic sensor use this - with front buttons
00205     char cmd[3];            // array for I2C
00206     int16_t btns;
00207     bool wert;
00208 
00209     cmd[0] = 0x06;
00210     cmd[1] = 0x00;
00211     i2c.write(addr, cmd, 2); 
00212 
00213     cmd[0]=0x01;
00214     i2c.write(addr, cmd, 1);
00215     i2c.read(addr|1, cmd, 1);
00216     btns = cmd[0];
00217     if( btns & (BTN_FL|BTN_FM|BTN_FR))
00218         wert = false;
00219     else
00220         wert = true;
00221     DEBUG_PRINT("WERT: %d", wert);
00222     return wert;
00223 #endif
00224 }
00225 
00226 bool ur_Bertl::WaitUntilButtonPressed()
00227 {
00228     char cmd[3];
00229     int16_t btns;
00230     bool wert;
00231 
00232     RGB_blue=RGB_red=RGB_green=0;
00233     cmd[0] = 0x06;
00234     cmd[1] = 0x00;
00235     i2c.write(addr, cmd, 2); 
00236 
00237     cmd[0]=0x01;
00238     i2c.write(addr, cmd, 1);
00239     i2c.read(addr|1, cmd, 1);
00240     btns = cmd[0];
00241     if( btns & (0xFF))
00242         wert = false;
00243     else
00244         wert = true;
00245     DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
00246     return wert;
00247 }
00248 
00249 bool ur_Bertl::NextToABeeper()
00250 {
00251     if (bottomIsBlack())
00252         return true;
00253     else 
00254         return false;
00255 }
00256 
00257 int ur_Bertl::AnyBeeperInBag()
00258 {
00259     if(beepersInBag > 0)
00260         return beepersInBag;
00261     else
00262         return 0;
00263 }
00264 
00265 bool ur_Bertl::IsButtonPressed(const int btn)
00266 {
00267     char cmd[3];            // array for I2C
00268     int16_t btns;
00269     bool wert;
00270 
00271     cmd[0] = 0x06;
00272     cmd[1] = 0x00;
00273     i2c.write(addr, cmd, 2); 
00274 
00275     cmd[0]=0x01;
00276     i2c.write(addr, cmd, 1);
00277     i2c.read(addr|1, cmd, 1);
00278     btns = cmd[0];
00279     if( btns & btn)
00280         wert = true;
00281     else
00282         wert = false;
00283     DEBUG_PRINT("WERT: %d", wert);
00284     return wert;
00285 }
00286 
00287 int ur_Bertl::ReturnButtonPressed()
00288 {
00289     char cmd[3];            // array for I2C
00290     int16_t btns;
00291 
00292     cmd[0] = 0x06;
00293     cmd[1] = 0x00;
00294     i2c.write(addr, cmd, 2); 
00295 
00296     cmd[0]=0x01;
00297     i2c.write(addr, cmd, 1);
00298     i2c.read(addr|1, cmd, 1);
00299     btns = cmd[0];
00300     DEBUG_PRINT("Button: %d", btns);
00301     return btns;
00302 }
00303 // Protected methodes
00304 
00305 int ur_Bertl::bottomIsBlack()
00306 {
00307     int detect;
00308     
00309     detect = linesensor;
00310     return detect;
00311 }
00312 
00313 bool ur_Bertl::backIsClear()
00314 {
00315     char cmd[3];            // array for I2C
00316     int16_t btns;
00317     bool wert;
00318 
00319     cmd[0] = 0x06;
00320     cmd[1] = 0x00;
00321     i2c.write(addr, cmd, 2); 
00322 
00323     cmd[0]=0x01;
00324     i2c.write(addr, cmd, 1);
00325     i2c.read(addr|1, cmd, 1);
00326     btns = cmd[0];
00327     if( btns & (BTN_BL|BTN_BM|BTN_BR))
00328         wert = false;
00329     else
00330         wert = true;
00331     DEBUG_PRINT("WERT: %d", wert);
00332     return wert;
00333 }
00334 
00335 bool ur_Bertl::frontButtonPressed()
00336 {
00337     char cmd[3];            // array for I2C
00338     int16_t btns;
00339     bool wert;
00340 
00341     cmd[0] = 0x06;
00342     cmd[1] = 0x00;
00343     i2c.write(addr, cmd, 2); 
00344 
00345     cmd[0]=0x01;
00346     i2c.write(addr, cmd, 1);
00347     i2c.read(addr|1, cmd, 1);
00348     btns = cmd[0];
00349     if( btns & (BTN_FL|BTN_FM|BTN_FR|BTN_FRR|BTN_FLL))
00350         wert = true;
00351     else
00352         wert = false;
00353     DEBUG_PRINT("WERT: %d", wert);
00354     return wert;
00355 }
00356 
00357 
00358 
00359 //-----------------INTERNAL USE ONLY ----------------------------
00360 void ur_Bertl::error()
00361 {
00362     int wait = 500;
00363     MotorR_FORWARD = MotorL_FORWARD = 0;    // both motor off
00364     MotorR_REVERSE = MotorL_REVERSE = 0;    // both motor off
00365     MotorR_EN=MotorL_EN=0;
00366     while(1) {
00367         TurnLedOff(0xFF);
00368         LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0;
00369         LED_blue=1;
00370         RGB_blue=RGB_green=RGB_red=1;
00371         wait_ms(wait);
00372         TurnLedOn(0xFF);
00373         LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
00374         LED_blue=0;
00375         RGB_blue=RGB_green=1;RGB_red=0;
00376         wait_ms(wait);
00377     }
00378 }
00379 
00380 // ISR
00381 
00382 void ur_Bertl::increment()
00383 {
00384     _count++;
00385 }
00386 
00387 int ur_Bertl::Read()
00388 {
00389     return _count;
00390 }
00391 
00392 // -------------------- BERTL CLASS -------------------------------------
00393 int Bertl::MoveBackwards(int move)
00394 {
00395     int count = _count;
00396     //wait_ms(250);                         // waite until Bertl stops
00397     MotorR_EN=MotorL_EN=1;                  // both motor ENABLE
00398     MotorR_REVERSE = MotorL_REVERSE = 1;    // both motor backwards ON
00399 #ifndef TIME
00400     while(_count < count+move) {
00401         if(!backIsClear())
00402             break;
00403         DEBUG_PRINT("count: %d _count: %d", count, _count);
00404     }
00405 #else
00406     wait_ms(move);
00407 #endif
00408     MotorR_REVERSE = MotorL_REVERSE = 0;    // both motor off
00409     MotorR_EN=MotorL_EN=0;
00410     if(move == MOVE)
00411         wait_ms(250);
00412     return _count - count;
00413 }
00414 // ------------------------- BERT CLASS --------------------------------------
00415 int Bertl::Move(int move)
00416 {
00417     int count = _count;
00418     MotorR_EN=MotorL_EN=1;                  // both motor ENABLE
00419     MotorR_FORWARD = MotorL_FORWARD = 1;    // both motor forward ON
00420 #ifdef TIME
00421     wait_ms(move);
00422 #else
00423 
00424     while(_count < count+move) {
00425         //if(!FrontIsClear())       // more convenient because there are no accidents :-)
00426           //  break;
00427 #ifdef FRONTBUTTON
00428         if(frontButtonPressed())
00429             error();
00430 #endif
00431         DEBUG_PRINT("count: %d _count: %d", count, _count);
00432     }
00433 #endif
00434     MotorR_FORWARD = MotorL_FORWARD = 0;    // both motor off
00435     MotorR_EN=MotorL_EN=0;
00436     if(move == MOVE)
00437         wait_ms(250);
00438     return _count - count;
00439 }
00440 
00441 void Bertl::TurnRigth()
00442 {
00443     int count = _count;
00444     MotorR_EN=MotorL_EN=1;                  // motor left and right ENABLE
00445 
00446     MotorR_FORWARD = MotorL_REVERSE = 0;
00447     MotorL_FORWARD = MotorR_REVERSE = 1;
00448 #ifdef TIME
00449     wait_ms(TURN);
00450 #else
00451 
00452     while(_count < count+ANGLE) {
00453 #ifdef FRONTBUTTON
00454         if(frontButtonPressed())            // get out if to much problems
00455             error();
00456 #endif
00457         DEBUG_PRINT("count: %d _count: %d", count, _count);
00458     }
00459 #endif
00460     MotorL_FORWARD = MotorR_REVERSE = 0;
00461     MotorR_FORWARD = MotorL_REVERSE = 0;
00462     MotorR_EN=MotorL_EN=0;
00463     wait_ms(250);           // only to step the robot
00464 }
00465 // ------------------------- to adjust turns ---------------------------------
00466 void Bertl::TurnRigthStep(int step)
00467 {
00468     int count = _count;
00469     MotorR_EN=MotorL_EN=1;                  // motor left and right ENABLE
00470 
00471     MotorR_FORWARD = MotorL_REVERSE = 0;
00472     MotorL_FORWARD = MotorR_REVERSE = 1;
00473     wait_ms(step);
00474 #ifdef TIME
00475 #else
00476 
00477     while(_count < count+1) {
00478 #ifdef FRONTBUTTON
00479         if(frontButtonPressed())            // get out if to much problems
00480             error();
00481 #endif
00482         DEBUG_PRINT("count: %d _count: %d", count, _count);
00483     }
00484 #endif
00485     MotorL_FORWARD = MotorR_REVERSE = 0;
00486     MotorR_FORWARD = MotorL_REVERSE = 0;
00487     MotorR_EN=MotorL_EN=0;
00488 //    wait_ms(250);           // only to step the robot
00489 }
00490 
00491 void Bertl::TurnLeftStep(int step)
00492 {
00493     int count = _count;
00494     MotorR_EN=MotorL_EN=1;                  // motor left and right ENABLE
00495 
00496     MotorR_FORWARD = MotorL_REVERSE = 1;
00497     wait_ms(step);
00498 
00499 #ifdef TIME
00500 #else
00501 
00502     while(_count < count+1) {
00503 #ifdef FRONTBUTTON
00504         if(frontButtonPressed())            // get out if to much problems
00505             error();
00506 #endif
00507         DEBUG_PRINT("count: %d _count: %d", count, _count);
00508     }
00509 #endif
00510     MotorR_FORWARD = MotorL_REVERSE = 0;
00511     MotorR_EN=MotorL_EN=0;
00512     //wait_ms(250);           // only to step the robot
00513 }
00514 
00515 uint8_t Bertl::GetLineValues()
00516 {
00517     uint8_t detect;
00518     
00519     detect = linesensor;
00520     return detect;
00521 }
00522 
00523 void Bertl::RGBLed(bool red, bool green, bool blue)
00524 {
00525       RGB_blue=!blue;
00526       RGB_red=!red;
00527       RGB_green=!green;
00528 }