hier so siehts jetzt aus.
jetzt hängt er immer nach der 3ten LED.
kann ja nur noch was mit dem drive sein, oder?

Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_I2CMasterLib.h"

void I2C_requestedDataReady(uint8_t dataRequestID)
{
	checkRP6Status(dataRequestID);
}

void I2C_transmissionError(uint8_t errorState)
{
	writeString_P("\nI2C ERROR - TWI STATE: 0x");
	writeInteger(errorState, HEX);
	writeChar('\n');
}


uint8_t transmit[10];

#define CMD_MOVE_AT_SPEED 5

void drive(uint8_t spdl, uint8_t spdr)
{
	transmit[0] = 0;
	transmit[1] = CMD_MOVE_AT_SPEED;
	transmit[2] = spdl;
	transmit[3] = spdr;
	I2CTWI_transmitBytes(I2C_RP6_BASE_ADR, transmit, 4);
        mSleep(10);
}

void batpower(void)
{
	uint8_t bat[2];
	I2CTWI_transmitByte(I2C_RP6_BASE_ADR, 21);
	I2CTWI_readBytes(I2C_RP6_BASE_ADR,bat,2);
	setCursorPosLCD(1,13);
	writeIntegerLengthLCD(bat[0] + (bat[1]<<8),DEC,3);
}

int main(void)
{
	initRP6Control();  
	initLCD();
	uint8_t lightsens[4];
	int x,y,z;
	I2CTWI_initMaster(150);  
	I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
	mSleep(500);
	I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
	mSleep(500);
	while(true) 
	{
		task_I2CTWI();
		I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, 3, 0b100000);
		setLEDs(0b1000);
		mSleep(50);
		I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, 3, 0b010000);
		mSleep(50);
		I2CTWI_transmitByte(I2C_RP6_BASE_ADR, 13);
		I2CTWI_readBytes(I2C_RP6_BASE_ADR,lightsens,4);
		setLEDs(0b0100);
		I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, 3, 0b001000);
		mSleep(50);
		x=lightsens[0] + (lightsens[1]<<8);
		y=lightsens[2] + (lightsens[3]<<8);
		setCursorPosLCD(0,2);
		writeIntegerLengthLCD(x,DEC,3);
		I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, 3, 0b000100);
		mSleep(50);
		setCursorPosLCD(1,2);
		writeIntegerLengthLCD(y,DEC,3);
		if(y>x)
		{
			z=y;
		}
		else
		{
			z=x;
		}
		setLEDs(0b0010);
		I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, 3, 0b000010);
		mSleep(50);
		showScreenLCD("X:","Y:       Bat:");
		batpower();
		if(z>400)
		{
			drive(75,75);
		}
		else
		{
			drive(0,0);
		}
		setLEDs(0b0001);
		I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, 3, 0b000001);
		mSleep(50);
	}
	return 0;
}