Bertl
Fork of Bertl by
ur_Bertl.cpp
- Committer:
- bulmecisco
- Date:
- 2015-03-26
- Revision:
- 2:1cd559ff516b
- Parent:
- 1:fafbac0ba96d
- Child:
- 3:01b183fe8b41
File content as of revision 2:1cd559ff516b:
/*********************************** name: ur_Bertl.cpp Version: 1.1 author: PE HTL BULME email: pe@bulme.at description: Implementation portion of class ur_Bertl The Robot Blue LED and RGB LED test added ***********************************/ #include "mbed.h" #include "config.h" #include "ur_Bertl.h" // Constructor ur_Bertl::ur_Bertl() : _interrupt(P1_12) // left sensor P1_13 { i2c.frequency(40000); // I2C Frequenz 40kHz 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 Frequenz 40kHz 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 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+DISTANCE) { //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; 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(); } 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 } 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; } void ur_Bertl::ShutOff() { MotorR_FORWARD = MotorL_FORWARD = 0; // motor OFF MotorR_EN=MotorL_EN=0; // motor disable } // 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; } //-----------------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); } } 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; } // ISR void ur_Bertl::increment() { _count++; } int ur_Bertl::Read() { return _count; } //---------------------------------------------------------------------- bool ur_Bertl::FrontIsClear() { #ifdef HCSR int dist = 0; usensor.start(); wait_ms(10); dist=usensor.get_dist_cm(); if(dist < 5) 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::NextToABeeper() { if (BottomIsBlack()) return true; else return false; } int ur_Bertl::AnyBeeperInBag() { if(beepersInBag > 0) return beepersInBag; else return 0; } 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; } int ur_Bertl::BottomIsBlack() { int detect; detect = linesensor; return detect; } void ur_Bertl::MoveBackwards() { 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 while(_count < count+DISTANCE) { if(!backIsClear()) break; DEBUG_PRINT("count: %d _count: %d", count, _count); } MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off MotorR_EN=MotorL_EN=0; wait_ms(250); } */