zl.rar_单片机 调速控制

//***************************FileName:Robbot.c***********************// //***************************ICCAVR6.30编译**************************// #include <io8535v.h> #include <macros.h> //****************************宏定义********************************// #define Aa 0.5 //PID参数 #define Ba -0.5 //PID参数 #define Ca 0 //PID参数 #define Ab 0.09 //PID参数 #define Bb -0.1 //PID参数 #define Cb 0 //PID参数 #define U1 12 //PID参数 #define MaxSpeed 0x40 //最大速度 #define MidSpeed 0x18 //中速,用于转弯 #define Size 6 //任务数组大小 //*****************************全局变量定义**************************// char sflag=0x00; //记录上一次校偏状态 char crossflag = 0; //过线标志，用于判断是否过线 char forflag=0; //记录上一次机器人行进状态 char forlight; //记录上一次A口光电传感器的状态 float EkA; //本次左边电机速度误差 float EkA_1=0; //上次左边电机速度误差 float EkA_2=0; //上上次左边电机速度误差 float EkB; //本次左边电机速度误差 float EkB_1=0; //上次左边电机速度误差 float EkB_2=0; //上上次左边电机速度误差 char flage=0; char a=0; //溢出次数，控制PID窗口时间 char c=0; //控制寻线频率 int desireV=10; char b=0; char time=0; //机器人行走步数 char fob=0; //=1后退，=0前进 char Task[Size]={0x37,0x27,0x1B,0x29,0x23,0x1B};//任务数据数组 //******************************定时器1初始化*************************// void Timer1Init(int temptimsk,int temptccrA,int temptccrB) {unsigned char sreg; TIMSK = temptimsk; sreg = SREG; //保存全局中断标志 _CLI(); //屏蔽所有中断 TCCR1A = temptccrA; TCCR1B = temptccrB; SREG = sreg; //恢复全局中断标志 } //******************************写OCR1A寄存器**************************// void SetOutputComReg1A(int tempocr) {unsigned char sreg; sreg = SREG; _CLI(); OCR1A = tempocr; SREG = sreg; } //******************************写OCR1B寄存器**************************// void SetOutputComReg1B(int tempocr) {unsigned char sreg; sreg = SREG; _CLI(); OCR1B = tempocr; SREG = sreg; } //******************************读OCR1A寄存器**************************// int GetOutputComReg1A() {int temp; temp = OCR1A; return(temp); } //******************************读OCR1B寄存器**************************// int GetOutputComReg1B() {int temp; temp = OCR1B; return(temp); } //******************************长延时函数***************************// void DELAY(int delaytime) {int i,j; for(i=0;i<=delaytime;i++) {for (j=0;j<=0xFFFE;j++) ;} } //******************************短延时函数***************************// void delay(int i) {int j; for(j=0;j<=i;j++) ; } //******************************PID调节:左电机***********************// void PIDA() {char y; //本次采样速度值 float u; //电压差值 int z; //本次输出增量 int temp1; //临时记录值 y = TCNT1; EkA=y-desireV; if (EkA==(0-desireV)) //当Ek大于某一值时直接加最大 {temp1=0x0000; SetOutputComReg1A(temp1);} else {EkA=Aa*EkA; EkA_1=Ba*EkA_1; EkA_2=Ca*EkA_2; u=EkA+EkA_1+EkA_2; z=u/U1*0x03FF; temp1=GetOutputComReg1A(); temp1=temp1+z; SetOutputComReg1A(temp1); EkA_2=EkA_1; EkA_1=EkA;} TCNT1=0x00; } //******************************PID调节:右电机***********************// void PIDB() {char y; float u; //电压差值 int z; int temp1; y = TCNT2; EkB=y-desireV; if(EkB==(0-desireV)) //当Ek大于某一值时直接加最大// {temp1=0x0000; SetOutputComReg1B(temp1);} else {EkB =Ab*EkB; EkB_1=Bb*EkB_1; EkB_2=Cb*EkB_2; u=EkB+EkB_1+EkB_2; z=u/U1*0x03FF; temp1=GetOutputComReg1B(); temp1=temp1+z; SetOutputComReg1B(temp1); EkB_2=EkB_1; EkB_1=EkB;} TCNT2=0x00; } //***************************寻线处理函数1***************************// char Talone() {if ((forlight&0x09)==0) //3单独亮 return(0); else return(1); //返回为校偏状态值 } //***************************寻线处理函数2***************************// char FUalone() //4单独亮 {if ((forlight&0x05)==0) return(0); else return(2); } //***************************寻线处理函数3***************************// char FIalone() //5单独亮 {if ((forlight&0x22)==0) return(0); else return(1); } //***************************寻线处理函数4***************************// char Salone() //6单独亮 {if ((forlight&0x12)==0) return(0); else return(2); } //***************************寻线处理函数5****************************// char InLine1(void) //线上偏离判断 {char discrepancy; //行进状态 char l2; //传感器状态 l2=0xFF^PINA; l2=0x3C&l2; l2=l2>>2; switch(l2) {case 0x00: discrepancy=0; break; //0000b未偏 case 0x01: discrepancy=0; break; case 0x02: discrepancy=0; break; case 0x04: discrepancy=2; break; //0010b左偏 case 0x05: discrepancy=2; break; case 0x06: discrepancy=2; break; case 0x08: discrepancy=1; break; //0001b右偏 case 0x09: discrepancy=1; break; case 0x0A: discrepancy=1; break;} return(discrepancy); } //***************************寻线处理函数6****************************// char InLine2(void) //线上偏离判断 {char discrepancy; char l2; l2=0xFF^PINA; l2=0x3C&l2;//3,4,5,6灯 l2=l2>>2; switch(l2) {case 0x00: discrepancy=0; break; //0000b//未偏 case 0x01: discrepancy=1; break; //0001b右偏 case 0x02: discrepancy=2; break; //0010b左偏 case 0x04: discrepancy=0; break; case 0x05: discrepancy=1; break; case 0x06: discrepancy=2; break; case 0x08: discrepancy=0; break; case 0x09: discrepancy=1; break; case 0x0A: discrepancy=2; break;} return(discrepancy); } //***************************寻线处理函数7****************************// char OutLine(void) //线外偏离判断 {char discrepancy; char l2; l2=PINA^0xFF; l2=0x3C&l2; //00111100b l2=l2>>2; switch(l2) {case 0x00: discrepancy=forflag; break; //0000b case 0x01: discrepancy=Talone(); break; //0001b case 0x02: discrepancy=FUalone(); break; //0010b case 0x04: discrepancy=FIalone(); break; //0100b case 0x05: discrepancy=0; break; //0101b case 0x06: discrepancy=0; break; //0110b case 0x08: discrepancy=Salone(); break; //1000b case 0x09: discrepancy=0; break; //1001b case 0x0A: discrepancy=0; break; //1010b case 0x0F: discrepancy=0; break; //1111b } return(discrepancy); } //***************************校偏函数********************************// void Revise(char discrepancy) {if(discrepancy==0) {forflag=0; if(fob==1) {PORTB=0x00; return;} PORTB=0x0C; return;} if(discrepancy==1) {PORTB=0x08; //A为左边电机 delay(0x0100); PORTB=0x0C; forflag=1; return;} if(discrepancy==2) //B为右边电机 {forflag=2; PORTB=0x04; delay(0x0100); PORTB=0x0C; return;} if(discrepancy==3) {PORTB=0x08; //A为左边电机 forflag=1; return;} if(discrepancy==4) {forflag=2; PORTB=0x04; return;} } //***************************寻线函数********************************// void SearchLine()