Hier noch das Programm..

Code:
#include <nibobee/iodefs.h>
#include <nibobee/motpwm.h>
#include <nibobee/delay.h>
#include <nibobee/sens.h>
#include <nibobee/odometry.h>
#include <nibobee/led.h>
#include <nibobee/line.h>



enum
{	//state - Zustand

	MODE_STOP,// Anhalten
	MODE_DRIVE,//Linie folgen
};

int16_t speed_l;
int16_t speed_r;
uint16_t counter_o;
int16_t *korrektur_l;
int16_t *korrektur_r;


void korrektur(int16_t links, int16_t rechts, int mitte);

uint8_t do_stop();
uint8_t do_drive(int16_t links, int16_t rechts, int mitte);

uint8_t mode_check(uint8_t mode);

int main()
{
	
	line_init();
	led_init();
	sens_init();
	motpwm_init();
	//odometry_init();
	uint8_t mode;

	
	while (1==1)
	{
		enable_interrupts();
		delay(1);
		
		
				
		
		
		
		int16_t links = line_get(LINE_L);
		int16_t rechts = line_get(LINE_R);
		int16_t mitte = line_get(LINE_C);
		
		mode = mode_check(mode);
		
		//Code for state transitions.  This code only figures out what
		//state transitions to make, and then makes them.
		switch(mode)
		{
			case MODE_STOP:		    mode = do_stop();                        break;
			case MODE_DRIVE:		mode = do_drive(links, rechts, mitte);   break;
		}

		
		//state outputs
		switch(mode)
		{
			case MODE_STOP:	                                     speed_l = 0;	           speed_r = 0;            break;
			case MODE_DRIVE:  korrektur(links, rechts, mitte);	 speed_l = *korrektur_l;   speed_r = *korrektur_r; break;
		}
		
		led_set(LED_L_YE, (links  + mitte) > 750);
		led_set(LED_L_RD, (links  + mitte) > 900);
		led_set(LED_R_RD, (rechts + mitte) > 900);
		led_set(LED_R_YE, (rechts + mitte) > 750);
		
		motpwm_setLeft(speed_l);
		motpwm_setRight(speed_r);
		
		
	}
	return 0;
}


uint8_t mode_check(uint8_t mode)
{
	if (sens_getLeft() && sens_getRight())
	{
		if((sens_getLeft()==-1) && sens_getRight()==-1)
		{
			mode = MODE_STOP;
		}
		else
		{
			mode = MODE_DRIVE;
		}
	}
	return mode;
}

uint8_t do_drive(int16_t links, int16_t rechts, int mitte)
{
	uint16_t static counter_stop = 3000;
	if ((links + mitte > 900) && (mitte + rechts >900))
	{
		counter_stop--;
		if(!counter_stop)
		{
			return MODE_STOP;
		}
	}
	else
	{
		counter_stop = 3000;
		return MODE_DRIVE;
	}
	return MODE_DRIVE;
}

uint8_t do_stop()
{
	return MODE_STOP;
}




void korrektur(int16_t links, int16_t rechts, int mitte)
{
	int16_t static ir = 510;
	int16_t static il = 500;
	
	if  (((links + mitte) >750) && ((rechts + mitte) < 750))
	{
		ir = ir - 10;
		il = il + 3;
	}
	else if (((links + mitte) < 750) && ((rechts + mitte) > 750))
	{
		il = il - 10;
		ir = ir + 5;
	}
	else
	{
		il = 500;
		ir = 510;		
	}
	
	if (((links + mitte) >750) && ((rechts + mitte) >750))
	{
		il = il--;
		ir = ir--;
	}	
	
	//Min 200
	if ( il < 200)
	{
		il = 200;
	}
	if ( ir < 200)
	{
		ir = 200;
	}
	
	//Max 700
	if ( il > 700)
	{
		il = 700;
	}
	if ( ir > 700)
	{
		ir = 700;
	}
	
	korrektur_l =& il;
	korrektur_r =& ir;
}