Diff: ur_Bertl.cpp

Revision:

0:ae8f51bac163

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ur_Bertl.cpp	Thu Apr 23 12:04:34 2015 +0000
@@ -0,0 +1,316 @@
+/***********************************
+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
+
+    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);
+    }
+    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;
+    while(_count < count+ANGLE) {
+#ifdef FRONTBUTTON
+        if(frontButtonPressed())            // get out if to much problems
+            error();
+#endif
+        DEBUG_PRINT("count: %d _count: %d", count, _count);
+    }
+    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];
+
+    cmd[0] = 0x02;
+    cmd[1] = ~led;
+    i2c.write(addr, cmd, 2);
+    wait(0.5);
+}
+
+void ur_Bertl::TurnLedOff(int16_t led)
+{
+    char cmd[3];
+
+    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()
+{
+    int dist = 0;
+    usensor.start();
+    wait_ms(10);
+    dist=usensor.get_dist_cm();
+    if(dist < 5)
+        return false;
+    else
+        return true;
+    DEBUG_PRINT("Distance: %d", dist);
+
+// 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;
+}
+
+
+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);
+}
+*/