position.c

利用avr c编程的综合程序

#include "position.h"

#define Pi 3.1415926536

#define abs(x) ( (x) < 0 ? -(x) : (x) )

volatile int angle_loca, x_loca , y_loca ;

volatile unsigned char uart_rec[6];

volatile unsigned char count;

void get_position()
{
	unsigned char temp;
	
	count = 0;
	while(1)
	{
		temp = uartGetc();
		if(temp & 0x80)break;
		else continue;
	}
	uart_rec[count] = (temp & 0x7F);
	count ++;
	
	while(count < 6)
	{
		temp = uartGetc();
		uart_rec[count] = (temp & 0x7F);
		count++;
	}
	
	angle_loca = (uart_rec[0] + (uart_rec[1] << 7)) >> 1;
	x_loca = uart_rec[2] + (uart_rec[3] << 7);
	y_loca = uart_rec[4] + (uart_rec[5] << 7);
	return;
}

int get_rel_angle(int angle_des)
{
	int rel_angle = angle_des - angle_loca;
	if(rel_angle >= -180 && rel_angle <= 180)
		return rel_angle;
	else if(rel_angle > 180)
		return (rel_angle - 360);
	else if(rel_angle < -180)
		return (rel_angle + 360);
	else 
		return 0;
}

short get_rotate_speed(int angle)
{
	if(angle > 120)
		return 50;
	else if(angle > 90)
		return 40;
	else if(angle > 45)
		return 30;
	else if(angle > 15)
		return 20;
	else 
		return 10;
}

void rotate(int angle_des, short move_speed)
{
	int rel_angle;
	short rotate_speed;
	
	motor(0x1a, move_speed, move_speed);
	_delay_ms(10);
	
	get_position();
	rel_angle = get_rel_angle(angle_des);
	
	while(abs(rel_angle) > 1)
	{
		get_position();
		rel_angle = get_rel_angle(angle_des);
		rotate_speed = get_rotate_speed(abs(rel_angle));
		if(rel_angle > 0)
			motor(0x1a, rotate_speed+move_speed, -1*rotate_speed+move_speed);
		else 
			motor(0x1a, -1*rotate_speed+move_speed, rotate_speed+move_speed);
    }
	
	motor(0x1a,move_speed,move_speed);
	return;
}

int get_angle_des(int x_des, int y_des)
{
	int angle_des; 
	if(x_des == x_loca)
	{
		if(y_des > y_loca)
			angle_des = 90;
		else
			angle_des = -90;
	}
	else
		angle_des = (int)((atan(((double)(y_des-y_loca))/((double)(x_des-x_loca))))*180.0/Pi) ;
	
	if(x_des < x_loca)
		angle_des += 180; 
	else
		angle_des = (angle_des+360)%360;
	
	return angle_des;
}

unsigned char walk_line(int x_des, int y_des)
{
	int angle_des_dir;
	int diff = 0, speed = 0, dist = 500;
	int rotate_integrate = 0;
	
	short rotate_speed = 0;
	
	get_position();
	angle_des_dir = get_angle_des(x_des,y_des);
	rotate(angle_des_dir,0);
	
	while(dist > 100)
	{ 
		#define max(x,y)             (x) > (y) ? (x):(y)
		#define min(x,y)             (x) < (y) ? (x):(y)
		motor(0x1a,speed + rotate_speed,speed - rotate_speed);
		
		get_position();
		angle_des_dir = get_angle_des(x_des,y_des);
		diff = angle_des_dir - angle_loca;
		if(abs(diff) > 180)
		{
			if(diff < 0)
			{
				diff += 360;
			}
			else
			{
				diff -= 360;
			}
		}
		diff *= 2;
		rotate_integrate += diff;
		
		diff = min(diff,50);
		diff = max(diff,-50);
		
		rotate_speed = rotate_integrate/20 + diff;
		rotate_integrate = max(rotate_integrate, 100);
		rotate_integrate = min(rotate_integrate, -100);
		
		rotate_speed = min(rotate_speed,100);
		rotate_speed = max(rotate_speed,-100);
		
		dist = speed = abs(x_des-x_loca) + abs(y_des-y_loca);
		speed /= 2;
		speed = min(speed,150);
	}
	
	motor(0x1a,0,0);
	return 1;
}

#if 0
unsigned char walk_line(int x_des, int y_des)
{
	volatile int angle_des_dir;

	volatile int speed, distance = 0;

	get_position();
	angle_des_dir = get_angle_des(x_des, y_des);
	rotate(angle_des_dir,0);
	
	distance = abs(x_loca - x_des) + abs(y_loca - y_des);
	
	while(distance > 200)
	{ 
		distance /= 10;
		speed = distance > 150 ? 150:distance;
		
		rotate(angle_des_dir, speed);
		
		//_delay_ms(50);
		
		get_position();
		angle_des_dir = get_angle_des(x_des, y_des);
		
		distance = abs(x_loca - x_des) + abs(y_loca - y_des);
	}
	motor(0x1a,0,0);
	return 1;
}
#endif