File content as of revision 0:50d2b4671eac:

#include "mbed.h"
#include "config.h"

void Move()
{
	MotorL_EN = MotorR_EN = 1;
	MotorL_FORWARD = MotorR_FORWARD = 1;
	wait_ms(300);
	MotorL_FORWARD = MotorR_FORWARD = 0;
	MotorL_EN = MotorR_EN = 0;
	
}

void Back()
{
	MotorL_EN = MotorR_EN = 1;
	MotorL_REVERSE = MotorR_REVERSE = 1;
	wait_ms(300);
	MotorL_REVERSE = MotorR_REVERSE = 0;
	MotorL_EN = MotorR_EN = 0;
	
}

void TurnRight()
{
	MotorL_EN = 1;
	MotorL_FORWARD = 1;
	wait_ms(100);
	MotorL_FORWARD = 0;
	MotorL_EN = 0;
	
}

void TurnLeft()
{
	MotorR_EN = 1;
	MotorR_FORWARD = 1;
	wait_ms(100);
	MotorR_FORWARD = 0;
	MotorR_EN = 0;
	
}

bool WaitUntilButtonPressed()
{
	char cmd[3];
    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 & (0xFF))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
	
}

bool Start()
{
    DEBUG_PRINT("Debug level: %d", (int) DEBUG);
    i2c.frequency(40000);       

    while(WaitUntilButtonPressed()) {    }
    return true;
	
}

bool FrontIsClear()
{
    char cmd[3];    
    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_FLL|BTN_FL|BTN_FM|BTN_FR|BTN_FRR))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
	
}

void RotateRight()
{
	MotorL_EN = MotorR_EN = 1;
	MotorL_FORWARD = MotorR_REVERSE = 1;
	wait_ms(2000);
	MotorL_FORWARD = MotorR_REVERSE = 0;
	MotorL_EN = MotorR_EN = 0;
	
}

void RotateLeft()
{
	MotorL_EN = MotorR_EN = 1;
	MotorR_FORWARD = MotorL_REVERSE = 1;
	wait_ms(2000);
	MotorR_FORWARD = MotorL_REVERSE = 0;
	MotorL_EN = MotorR_EN = 0;
	
}

void MoveTime(int c)
{
	MotorL_EN = MotorR_EN = 1;
	MotorL_FORWARD = MotorR_FORWARD = 1;
	wait_ms(c);
	MotorL_FORWARD = MotorR_FORWARD = 0;
	MotorL_EN = MotorR_EN = 1;
	
}

void BackTime(int c)
{
	MotorL_EN = MotorR_EN = 1;
	MotorL_REVERSE = MotorR_REVERSE = 1;
	wait_ms(c);
	MotorL_EN = MotorR_EN = 0;
	MotorL_REVERSE = MotorR_REVERSE = 0;
	
}

void TurnRightTime(int c)
{
	MotorL_EN = 1;
	MotorL_FORWARD = 1;
	wait_ms(c);
	MotorL_FORWARD = 0;
	MotorL_EN = 0;
	
}

void TrnLeftTime(int c)
{
	MotorR_EN = 1;
	MotorR_FORWARD = 1; 
	wait_ms(c);
	MotorR_FORWARD = 0;
	MotorR_EN = 0;
	
}

void RotateRightTime(int c)
{
	MotorL_EN = MotorR_EN = 1;
	MotorL_FORWARD = MotorR_REVERSE = 1;
	wait_ms(c);
	MotorL_FORWARD = MotorR_REVERSE = 0;
	MotorL_EN = MotorR_EN = 0;
	
}

void RotateLeftTime(int c)
{
	MotorR_EN = MotorL_EN = 1;
	MotorR_FORWARD = MotorL_REVERSE = 1;
	wait_ms(c);
	MotorR_FORWARD = MotorL_REVERSE = 0; 
	MotorL_EN = MotorR_EN = 0;
	
}

bool BackIsClear()
{
    char cmd[3];    
    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("\right\nWERT: %d \right\n", wert);
    return wert;
	
}

bool ButtonBackLeft()
{
    char cmd[3];    
    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))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
	
}

bool ButtonBackMid()
{
	char cmd[3];    
    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_BM))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
}

bool ButtonBackRight()
{
	char cmd[3];    
    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_BR))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
}

bool ButtonFrontLeftLeft()
{
	char cmd[3];    
    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_FLL))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
}

bool ButtonFrontLeft()
{
	char cmd[3];    
    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))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
}

bool ButtonFrontMid()
{
	char cmd[3];    
    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_FM))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
}

bool ButtonFrontRight()
{
	char cmd[3];    
    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_FR))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
}

bool ButtonFrontRightRight()
{
	char cmd[3];    
    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_FRR))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
}

void TurnLedOn(int16_t led)
{
    char cmd[3];

    cmd[0] = 0x02;
    cmd[1] = ~led;
    i2c.write(addr, cmd, 2);
    wait(0.5);
}

void TurnLedOff(int16_t led)
{
    char cmd[3];

    cmd[0] = 0x02;
    cmd[1] = ~led;
    i2c.write(addr, cmd, 2);
    wait(0.5);
}

bool MoveTicks(int c)
{
	int count = 0, left = 0, right = 0;
	
	MotorL_EN = MotorR_EN = 1;
	
	while(c--)
	{
		MotorL_FORWARD = MotorR_FORWARD = 1;
		
		if(SensorL == 1)
			left++;
		if(SensorR == 1)
			right++;
		
		count++;
		wait_ms(10);
	}
	
	MotorL_FORWARD = MotorR_FORWARD = 0;
	MotorL_EN = MotorR_EN = 0;
	
	return true;
	
}

bool BackTicks(int c)
{
	int count = 0, left = 0, right = 0;
	
	MotorL_EN = MotorR_EN = 1;
	
	while(c--)
	{
		MotorL_REVERSE = MotorR_REVERSE = 1;
		
		if(SensorL == 1)
			left++;
		if(SensorR == 1)
			right++;
		
		count++;
		wait_ms(10);
	}
	
	MotorL_REVERSE = MotorR_REVERSE = 0;
	MotorL_EN = MotorR_EN = 0;
	
	return true;
	
}

bool MotorRightMoveTicks(int c)
{
	int count = 0, right = 0;
	
	MotorR_EN = 1;
	
	while(c--)
	{
		MotorR_FORWARD = 1;
		
		if(SensorR == 1)
			right++;
		
		count++;
		wait_ms(10);
	}
	
	MotorR_FORWARD = 0;
	MotorR_EN = 0;
	
	return true;
	
}

bool MotorRightBackTicks(int c)
{
	int count = 0, right = 0;
	
	MotorR_EN = 1;
	
	while(c--)
	{
		MotorR_REVERSE = 1;
		
		if(SensorR == 1)
			right++;
		
		count++;
		wait_ms(10);
	}
	
	MotorR_REVERSE = 0;
	MotorR_EN = 0;
	
	return true;
	
}

bool MotorLeftMoveTicks(int c)
{
	int count = 0, right = 0;
	
	MotorL_EN = 1;
	
	while(c--)
	{
		MotorL_FORWARD = 1;
		
		if(SensorR == 1)
			right++;
		
		count++;
		wait_ms(10);
	}
	
	MotorL_FORWARD = 0;
	MotorL_EN = 0;
	
	return true;
	
}

bool MotorLeftBackTicks(int c)
{
	int count = 0, right = 0;
	
	MotorL_EN = 1;
	
	while(c--)
	{
		MotorL_REVERSE = 1;
		
		if(SensorR == 1)
			right++;
		
		count++;
		wait_ms(10);
	}
	
	MotorL_REVERSE = 0;
	MotorL_EN = 0;
	
	return true;
	
}