Class Bertl added
Dependencies: HCSR
Fork of ur_Bertl by
ur_Bertl.cpp
- Committer:
- bulmecisco
- Date:
- 2015-04-27
- Revision:
- 13:3ce84646fd74
- Parent:
- 12:cedc088eaf05
File content as of revision 13:3ce84646fd74:
/*********************************** name: ur_Bertl.cpp Version: 3.0 class Bertl included author: PE HTL BULME email: pe@bulme.at WIKI: https://developer.mbed.org/teams/BERTL_CHEL_18/code/ur_Bertl/ description: Implementation portion of class ur_Bertl The Robot boolean commands added for if/else, while, ... int ReturnButtonPressed() added which returns the int value of button pressed ***********************************/ #include "mbed.h" #include "config.h" #include "ur_Bertl.h" // Constructor ur_Bertl::ur_Bertl() : _interrupt(MOTORENC) { i2c.frequency(40000); // I2C init char init1[2] = {0x6, 0x00}; char init2[2] = {0x7, 0xff}; i2c.write(0x40, init1, 2); i2c.write(0x40, init2, 2); mg1 = mg2 = SPEED; _interrupt.rise(this, &ur_Bertl::increment); // attach increment function of this counter instance ie. motor sensor _count = 0; beepersInBag = 0; } ur_Bertl::ur_Bertl(PinName pin) : _interrupt(pin) // create the InterruptIn on the pin specified to Counter { i2c.frequency(40000); // I2C init char init1[2] = {0x6, 0x00}; char init2[2] = {0x7, 0xff}; i2c.write(0x40, init1, 2); i2c.write(0x40, init2, 2); mg1 = mg2 = SPEED; _interrupt.rise(this, &ur_Bertl::increment); // attach increment function of this counter instance ie. motor sensor _count = 0; beepersInBag = 0; } // Pulblic methodes void ur_Bertl::Move(int move) { int count = _count; MotorR_EN=MotorL_EN=1; // both motor ENABLE MotorR_FORWARD = MotorL_FORWARD = 1; // both motor forward ON #ifdef TIME wait_ms(move); #else while(_count < count+move) { //if(!FrontIsClear()) // more convenient because there are no accidents :-) // break; #ifdef FRONTBUTTON if(frontButtonPressed()) error(); #endif DEBUG_PRINT("count: %d _count: %d", count, _count); } #endif MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off MotorR_EN=MotorL_EN=0; if(move == MOVE) wait_ms(250); } void ur_Bertl::PutBeeper() { // wait_ms(500); if(beepersInBag > 0) beepersInBag--; else error(); } void ur_Bertl::PickBeeper() { // wait_ms(500); if (linesensor) beepersInBag++; else error(); if(beepersInBag > 16) error(); } void ur_Bertl::TurnLeft() { int count = _count; MotorR_EN=MotorL_EN=1; // motor left and right ENABLE MotorR_FORWARD = MotorL_REVERSE = 1; #ifdef TIME wait_ms(TURN); #else while(_count < count+ANGLE) { #ifdef FRONTBUTTON if(frontButtonPressed()) // get out if to much problems error(); #endif DEBUG_PRINT("count: %d _count: %d", count, _count); } #endif MotorR_FORWARD = MotorL_REVERSE = 0; MotorR_EN=MotorL_EN=0; wait_ms(250); // only to step the robot } void ur_Bertl::ShutOff() { MotorR_FORWARD = MotorL_FORWARD = 0; // motor OFF MotorR_EN=MotorL_EN=0; // motor disable } // Public LEDs methodes void ur_Bertl::BlueLedsON() { LED_blue=0; } void ur_Bertl::BlueLedsOFF() { LED_blue=1; } void ur_Bertl::RGBLed(bool red, bool green, bool blue) { RGB_blue=!blue; RGB_red=!red; RGB_green=!green; } void ur_Bertl::TurnLedOn(int16_t led) { char cmd[3]; if(led == 0xBB) { LED_blue=0; return; } else if (led == 0xFF) { RGBLed(1,1,1); LED_blue=0; } cmd[0] = 0x02; cmd[1] = ~led; i2c.write(addr, cmd, 2); wait(0.5); } void ur_Bertl::TurnLedOff(int16_t led) { char cmd[3]; if(led == 0xBB) { LED_blue=1; return; } else if (led == 0xFF) { RGBLed(0,0,0); //don't work? LED_blue=1; } cmd[0] = 0x02; cmd[1] = led; i2c.write(addr, cmd, 2); wait(0.5); } void ur_Bertl::NibbleLeds(int value) { NibbleLEDs = value%16; } //---------------------------------------------------------------------- bool ur_Bertl::FrontIsClear() { #ifdef HCSR int dist = 0; usensor.start(); wait_ms(10); dist=usensor.get_dist_cm(); if(dist < ULTRASONIC_DISTANCE) return false; else return true; #else // if there is no ultra sonic sensor use this - with front buttons char cmd[3]; // array for I2C int16_t btns; bool wert; cmd[0] = 0x06; cmd[1] = 0x00; i2c.write(addr, cmd, 2); cmd[0]=0x01; i2c.write(addr, cmd, 1); i2c.read(addr|1, cmd, 1); btns = cmd[0]; if( btns & (BTN_FL|BTN_FM|BTN_FR)) wert = false; else wert = true; DEBUG_PRINT("WERT: %d", wert); return wert; #endif } bool ur_Bertl::WaitUntilButtonPressed() { char cmd[3]; int16_t btns; bool wert; RGB_blue=RGB_red=RGB_green=0; cmd[0] = 0x06; cmd[1] = 0x00; i2c.write(addr, cmd, 2); cmd[0]=0x01; i2c.write(addr, cmd, 1); i2c.read(addr|1, cmd, 1); btns = cmd[0]; if( btns & (0xFF)) wert = false; else wert = true; DEBUG_PRINT("\right\nWERT: %d \right\n", wert); return wert; } bool ur_Bertl::NextToABeeper() { if (bottomIsBlack()) return true; else return false; } int ur_Bertl::AnyBeeperInBag() { if(beepersInBag > 0) return beepersInBag; else return 0; } bool ur_Bertl::IsButtonPressed(const int btn) { char cmd[3]; // array for I2C int16_t btns; bool wert; cmd[0] = 0x06; cmd[1] = 0x00; i2c.write(addr, cmd, 2); cmd[0]=0x01; i2c.write(addr, cmd, 1); i2c.read(addr|1, cmd, 1); btns = cmd[0]; if( btns & btn) wert = true; else wert = false; DEBUG_PRINT("WERT: %d", wert); return wert; } int ur_Bertl::ReturnButtonPressed() { char cmd[3]; // array for I2C int16_t btns; cmd[0] = 0x06; cmd[1] = 0x00; i2c.write(addr, cmd, 2); cmd[0]=0x01; i2c.write(addr, cmd, 1); i2c.read(addr|1, cmd, 1); btns = cmd[0]; DEBUG_PRINT("Button: %d", btns); return btns; } // Protected methodes int ur_Bertl::bottomIsBlack() { int detect; detect = linesensor; return detect; } bool ur_Bertl::backIsClear() { char cmd[3]; // array for I2C int16_t btns; bool wert; cmd[0] = 0x06; cmd[1] = 0x00; i2c.write(addr, cmd, 2); cmd[0]=0x01; i2c.write(addr, cmd, 1); i2c.read(addr|1, cmd, 1); btns = cmd[0]; if( btns & (BTN_BL|BTN_BM|BTN_BR)) wert = false; else wert = true; DEBUG_PRINT("WERT: %d", wert); return wert; } bool ur_Bertl::frontButtonPressed() { char cmd[3]; // array for I2C int16_t btns; bool wert; cmd[0] = 0x06; cmd[1] = 0x00; i2c.write(addr, cmd, 2); cmd[0]=0x01; i2c.write(addr, cmd, 1); i2c.read(addr|1, cmd, 1); btns = cmd[0]; if( btns & (BTN_FL|BTN_FM|BTN_FR|BTN_FRR|BTN_FLL)) wert = true; else wert = false; DEBUG_PRINT("WERT: %d", wert); return wert; } //-----------------INTERNAL USE ONLY ---------------------------- void ur_Bertl::error() { int wait = 500; MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off MotorR_EN=MotorL_EN=0; while(1) { TurnLedOff(0xFF); LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0; LED_blue=1; RGB_blue=RGB_green=RGB_red=1; wait_ms(wait); TurnLedOn(0xFF); LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1; LED_blue=0; RGB_blue=RGB_green=1;RGB_red=0; wait_ms(wait); } } // ISR void ur_Bertl::increment() { _count++; } int ur_Bertl::Read() { return _count; } // -------------------- BERTL CLASS ------------------------------------- int Bertl::MoveBackwards(int move) { int count = _count; //wait_ms(250); // waite until Bertl stops MotorR_EN=MotorL_EN=1; // both motor ENABLE MotorR_REVERSE = MotorL_REVERSE = 1; // both motor backwards ON #ifndef TIME while(_count < count+move) { if(!backIsClear()) break; DEBUG_PRINT("count: %d _count: %d", count, _count); } #else wait_ms(move); #endif MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off MotorR_EN=MotorL_EN=0; if(move == MOVE) wait_ms(250); return _count - count; } // ------------------------- BERT CLASS -------------------------------------- int Bertl::Move(int move) { int count = _count; MotorR_EN=MotorL_EN=1; // both motor ENABLE MotorR_FORWARD = MotorL_FORWARD = 1; // both motor forward ON #ifdef TIME wait_ms(move); #else while(_count < count+move) { //if(!FrontIsClear()) // more convenient because there are no accidents :-) // break; #ifdef FRONTBUTTON if(frontButtonPressed()) error(); #endif DEBUG_PRINT("count: %d _count: %d", count, _count); } #endif MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off MotorR_EN=MotorL_EN=0; if(move == MOVE) wait_ms(250); return _count - count; } void Bertl::TurnRigth() { int count = _count; MotorR_EN=MotorL_EN=1; // motor left and right ENABLE MotorR_FORWARD = MotorL_REVERSE = 0; MotorL_FORWARD = MotorR_REVERSE = 1; #ifdef TIME wait_ms(TURN); #else while(_count < count+ANGLE) { #ifdef FRONTBUTTON if(frontButtonPressed()) // get out if to much problems error(); #endif DEBUG_PRINT("count: %d _count: %d", count, _count); } #endif MotorL_FORWARD = MotorR_REVERSE = 0; MotorR_FORWARD = MotorL_REVERSE = 0; MotorR_EN=MotorL_EN=0; wait_ms(250); // only to step the robot } // ------------------------- to adjust turns --------------------------------- void Bertl::TurnRigthStep(int step) { int count = _count; MotorR_EN=MotorL_EN=1; // motor left and right ENABLE MotorR_FORWARD = MotorL_REVERSE = 0; MotorL_FORWARD = MotorR_REVERSE = 1; wait_ms(step); #ifdef TIME #else while(_count < count+1) { #ifdef FRONTBUTTON if(frontButtonPressed()) // get out if to much problems error(); #endif DEBUG_PRINT("count: %d _count: %d", count, _count); } #endif MotorL_FORWARD = MotorR_REVERSE = 0; MotorR_FORWARD = MotorL_REVERSE = 0; MotorR_EN=MotorL_EN=0; // wait_ms(250); // only to step the robot } void Bertl::TurnLeftStep(int step) { int count = _count; MotorR_EN=MotorL_EN=1; // motor left and right ENABLE MotorR_FORWARD = MotorL_REVERSE = 1; wait_ms(step); #ifdef TIME #else while(_count < count+1) { #ifdef FRONTBUTTON if(frontButtonPressed()) // get out if to much problems error(); #endif DEBUG_PRINT("count: %d _count: %d", count, _count); } #endif MotorR_FORWARD = MotorL_REVERSE = 0; MotorR_EN=MotorL_EN=0; //wait_ms(250); // only to step the robot } uint8_t Bertl::GetLineValues() { uint8_t detect; detect = linesensor; return detect; } void Bertl::RGBLed(bool red, bool green, bool blue) { RGB_blue=!blue; RGB_red=!red; RGB_green=!green; }