Thuật toán điều khiển robot tự động

[tahi]

mắt tinh: cảm biến tốt
chân khỏe: động cơ ngon
1 cái đầu thông minh: code tốt
1 cái đầu thông minh sẽ đỡ 1 phần cho cái mắt kém, và 1 cái chân yếu
Em viết dò đường hay viết theo kiểu máy trạng thái đơn giản

void read_sensor()
{
}
void direct(int mode)
{
	//set up direct for motor controller 
}
void speed(unsigned int motor1,unsigned int motor2)
{
	//justify speed of left and right motor
}
main()
{
	direct(forward);
	speed(100,100);
	while(1)
	{
		switch(partern)
		{
			case wait:
				//waiting user press any button
			case straight:
				switch(read_sensor())
					case 0000 0000:
						partern = straight;
						break;
					case xxx0 0xxx:
						//A little inclined to the left
					case 0000 xxxx:
						partern = turn_left;
						break;
					case xxxx xxxx:
						partern = out_road;
			case turn_left:
				direct(forward);
				toc_do(20,100);
				partern = straight;
				break;
			case turn_right:
				break;
			case check_crossline:
				break;
			case out_road:
				direct(back);
				speed(100,100);		
				break;
		}
	}
}