motor_control.c

利用avr c编程的综合程序

#include "motor_control.h"

volatile unsigned char sys_time = 0;

#define hand_open() motor(0x3a,500,800)
#define hand_close() motor(0x3a,500,1600)

#define Y_DASH_MAX 7000

#define abs(x) ((x) >= 0 ? (x) : -(x))

void n_second(unsigned char n);
void get_cheese(void);

int main(void)
{
	DDRB |= 0xE0;
	uartInit();
	sensorInit();
	timeInit();
	sei();
	
	unsigned char dash_start_time;
	
	get_position();
	
	get_cheese();
	
	rotate(90, 0);
	
	dash_start_time = getTime();
	
	motor(0x1a, 150, 150);
	
	get_position();
	while((getTime() - dash_start_time) < 6 && (y_loca < Y_DASH_MAX))
		get_position();
		
	motor(0x1a, 0, 0);
	
	rotate(180, 0);
	
	motor(0x1a, -100, -100);
	
	while(!(Left_backward_straight == 0 || Right_backward_straight == 0))get_sensor();
	
	// Get to the bar
	if(Left_backward_straight == 0 && Right_forward_straight != 0)
	{
		motor(0x1a, 0, -40);
		while(Right_backward_straight != 0)	
			get_sensor();
		motor(0x1a, 0, 0);
	}
	else if(Right_backward_straight == 0 && Left_backward_straight != 0)
	{
		motor(0x1a, -40, 0);
		while(Left_backward_straight != 0)
			get_sensor();
		motor(0x1a, 0, 0);
	}
	else
		motor(0x1a, 0, 0);
	// The mission ends.
	_delay_ms(500);
	
	
	motor(0x1a, 100, 100);
	
	get_position();
	while(x_loca > 2700)
		get_position();
	
	motor(0x1a, 0, 0);
	
	rotate(270, 0);
	
	motor(0x1a, -100, -100);
	
	get_sensor();
	
	while(!(Left_backward_straight == 0 && Right_backward_straight == 0))
		get_sensor();
	
	// Get to the bar
	if(Left_backward_straight == 0 && Right_forward_straight != 0)
	{
		motor(0x1a, 0, -40);
		while(Right_backward_straight != 0)	
			get_sensor();
		motor(0x1a, 0, 0);
	}
	else if(Right_backward_straight == 0 && Left_backward_straight != 0)
	{
		motor(0x1a, -40, 0);
		while(Left_backward_straight != 0)
			get_sensor();
		motor(0x1a, 0, 0);
	}
	else
		motor(0x1a, 0, 0);
	
	motor(0x1a, 0, 0);
	
	ledDispInit();
	while(1)
	{
		;
	}
}

void n_second(unsigned char n)
{
	volatile unsigned char i = 0;
	for(; i < n; i++)
		_delay_ms(1000);
}

void get_cheese(void)
{
	uartClose();
	
	unsigned char temp_time = getTime();
	
	motor(0x1a, 150, 150);
	_delay_ms(100);
	
	motor(0x2a, 7400, -1000);
	
	_delay_ms(1000);
	hand_open();
	
	while(1)
	{
		get_sensor();
		if(Hold_pole == 0 || (getTime() - temp_time) > 3)
		break;
	}
	motor(0x1a, 0, 0);
		
	hand_close();
	n_second(6);

	motor(0x2a, 8900, -1000);
	
	motor(0x1a, -100, -100);
	_delay_ms(1000);
	
	uartOpen();
	get_position();
	rotate(0, 0);
	
	motor(0x1a, -100, -100);
	_delay_ms(1000);
	motor(0x1a, 0, 0);
}