ur_Bertl.cpp

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