sf.c

mega128的多电机控制程序

#include "avr/io.h"
#include "avr/interrupt.h"
//#include "avr/signal.h"
//#include "avr/eeprom.h"
#define uchar unsigned char
#define uint unsigned int
#define ADdelay 100
#define FishID 5
#define CLI SREG&=0x7f
#define SEI SREG|=0x80
uchar i=0,T,num=0,v,Tv,isRemoteControl=0;
signed char direct=0;
int step;
uchar UartData[4]={0x40,0x07,0xCC,0};
uint x1[18]={822,793,760,724,692,667,652,649,657,678,707,740,776,808,833,848,851,843};
uint x2[18]={711,681,658,647,647,661,686,718,753,789,819,842,853,853,839,814,782,747};
uint x3[18]={628,684,748,812,867,908,932,936,915,872,816,752,688,633,592,568,564,585};
//x4[18]={976,974,944,886,814,737,664,600,552,524,526,556,614,686,763,836,900,948};

void delay(unsigned char x)
{	 uchar j,jj;
	 while((x--)!=0)
	   for(j=0;j<250;j++)
		for(jj=0;jj<250;jj++);
}
void decision(uchar *data)
{ uchar IrAD,temp;  
	if((num==6)&&(UartData[2]==0xBB)&&(UartData[3]==FishID)){
	num=1;
	v=UartData[1]&0x0F;	
	temp=UartData[0];	
	direct=(temp>>4)-4;	
	}
	else if((num==6)&&(UartData[2]==0xCC)&&(UartData[3]==FishID)){
	num=1;
	v=UartData[1]&0x0F;	
	}
	if((UartData[2]==0xCC))
	{	IrAD=(PINA&0x38)>>3;
		switch(IrAD){
			case 0:if(direct>=0)	direct=3;// L M R
					else direct=-3;
					break;
			case 1:direct=2;break;// L M
			case 2:direct=0;break;// L R
			case 3:direct=1;break;// L
			case 4:direct=-2;break;// M R
			case 5:if(direct>=0)	direct=2;// M
					else direct=-2;
					break;
			case 6:direct=-1;break;// R
			case 7:direct=0;break;//none
		}
	}
}
void move(void)
{if(T==2)
	{	T=0;		Tv--;
		if(Tv==0)
		{	step=60*direct;
			step=-60*direct;
			if(v>0)
			{	Tv=v;	
				OCR1AH=(x1[i]+step)/256;OCR1AL=(x1[i]+step)%256;
				OCR1BH=(x2[i]+step)/256;OCR1BL=(x2[i]+step)%256;
				OCR1CH=(x3[i]+step)/256;OCR1CL=(x3[i]+step)%256;	
				i++;	i%=18;	
			}
			else if(v==0)
			{	Tv=1;
				OCR1AH=(0x2F8+step)/256;OCR1AL=(0x2F8+step)%256;
				OCR1BH=(0x2F8+step)/256;OCR1BL=(0x2F8+step)%256;
				OCR1CH=(0x2F8+step)/256;OCR1CL=(0x2F8+step)%256;
			}
		}
	}	
	
}
SIGNAL (SIG_OVERFLOW1)
{T=0x02;}
SIGNAL (SIG_UART0_RECV)
{  	uchar recvch;
	CLI;
	recvch=UDR0;
	if(recvch==0xAA)	
	{num++;		SEI;return;	}
	if(num==2)			
	{	num++;	UartData[2]=recvch;SEI;return;	}
	if(num==3)			
	{	num++;	UartData[0]=recvch;	SEI;return;}
	if(num==4)	
	{	num++;	UartData[1]=recvch;	SEI;return;}
	if(num==5)	
	{	num++;	UartData[3]=recvch;	SEI;return;}
}
void init(void)
{//-----------SET OF PWM---------------
	DDRB|=(1<<PB5)|(1<<PB6)|(1<<PB7);//OC1A~OC1C
	TIMSK|=(1<<TOIE1);
	OCR1AH=0x2;OCR1AL=0xF8;
	OCR1BH=0x2;OCR1BL=0xF8;
	OCR1CH=0x2;OCR1CL=0xF8;//Neutral
	ICR1H=0x27;ICR1L=0x10;//T=20MS
	TCCR1A|=(1<<COM1A1)|(1<<COM1B1)|(1<<COM1C1)|(1<<WGM11);
	TCCR1B|=(1<<WGM13)|(1<<CS11);//8M 8分频 1010模式
//---------------SET OF USART-----------------
	UCSR0B=0x00;	UBRR0L=0x33;	UBRR0H=0;	
	UCSR0A&=0xFD;	UCSR0B=0x98;	//串口8M 9600 U2X=0;	
//---------------SET OF IO------------------------	
	DDRA|=0xC7;//IrAD
	//DDRA=0xFF;
//------------------------------------------------

	SEI;
	v=7;Tv=v;
	direct=0;
}
void main(void)
{init();

	while(1)
	{
		decision(UartData);
		move();
		//delay(1);

	}
}