File content as of revision 17:308802267a62:

/***********************************
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)         
{
    MotorSpg = 1;
    IncrementalgeberSpg = 1;
    LinienSensorSpg = 1;

    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;
    TurnLedOff(0xFF);
}

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;
   TurnLedOff(0xFF);
}

// 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) {        // DISTANCE maybe change to 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::TurnOff()
{
    MotorR_FORWARD = MotorL_FORWARD = 0;    // motor OFF
    MotorR_EN=MotorL_EN=0;                  // motor disable
    MotorSpg = 0;
    IncrementalgeberSpg = 0;
    LinienSensorSpg = 0;
    BlueLedsOFF();
    TurnLedOff(LED_ALL);
    NibbleLeds(0x00);
}

// 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)
{
    leds = leds | 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] = ~leds;
    i2c.write(addr, cmd, 2);
    wait(0.5);
}

void ur_Bertl::TurnLedOff(int16_t led)
{
    leds = leds & ~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] = ~leds;
    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;
}