PID.rar_PID校正_pid control two dof_控制平台PID_角度 pid

/*************************** 此处设计晶振的选择，故要配置熔丝位 步骤： 1 外接晶振为8M，故配置CKPOT=1，CKSEL[3:1]=1111 2 CKSEL和SUT[1：0]均设为111，复位唤醒延时65ms 3 点击下方的写入按钮，才能将熔丝位烧入程序中 接口配置： PB0 ZLG-DIO PB1 ZLG-CLK PB2 ZLG-INT PB3 ZLG-CS PB6 第9位数据 PB7 第10位数据 PD3 BRAKE 电机刹车 高电平停止 PD4 DIR PD5 pwm PD6 LD 计数器LD(重装)低电平有效 初值0x7F PD7 LE 锁存器使能高电平有效 PA 编码器计数值 ***************************/ #include <iom16v.h> #include <macros.h> #include <math.h> #include "ZLG7289.h" #define uint unsigned int #define uchar unsigned char #define DRI_set PORTD|=BIT(4); #define DRI_clr PORTD&=~BIT(4); #define BRAKE_set PORTD|=BIT(3); #define BRAKE_clr PORTD&=~BIT(3); #define LD_set PORTD|=BIT(6); #define LD_clr PORTD&=~BIT(6); #define LE_set PORTD|=BIT(7); #define LE_clr PORTD&=~BIT(7); uchar Dflag,PIDflag,shuru_flag,fangxiang_flag; //Dflag 方向标志 float count,stemp ; //角度脉冲数 float PWM,a; float angle_sj,angle_sd,Angle_temp; //角度实际与设定值 float Angle_Error;//角度，速度偏差 uchar Key=0; uchar wei=3; uchar temp[8]; float angle_set,angle_settemp; uchar string[16] = {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B,0x0C,0x0D,0x0E, 0x0F}; typedef struct PID { float setpoint; //设定值 float proportion; //比例系数 float integral; //积分系数 float derivative; //微分系数 float lasterror; //前一拍误差 float preerror; //前两拍误差 float lastp; //前两拍误差 }PID; // 定义struct PID结构体，并且新定义一个PID变量类型代替struct PID结构体 PID APID;//定义速度PID和角度PID float value; /****************************************************** 函数功能：延时ms函数 ******************************************************/ void delay(uint ms)//8M { uint i,j; for(i=0;i<ms;i++) for(j=0;j<1141;j++); } /**********us延时函数**********/ void delay_us(uint us) { if(us==0) return; while(--us); } /****************************************************** 函数功能：端口初始化 ******************************************************/ Port_init() { DDRD=0XF8; //1111 1000 PORTD&=0X17;//0001 0111 DDRA=0X00; //定义为输入口 PORTA=0XFF;//带上拉输入 DDRB&=~BIT(6); //定义为输入口 DDRB&=~BIT(7); //定义为输入口 PORTB|=BIT(6);//带上拉输入 PORTB|=BIT(7);//带上拉输入 } /****************************************************** 函数功能：功能初始化 ******************************************************/ Fun_init() { BRAKE_set;//开刹车 LD_clr; //计数器打入初值 delay(5); LD_set; //关闭计数器打入 } /****************************************************** 函数功能：快速PWM初始化 ******************************************************/ void PWM_init (void) { TCCR1A=0XC2;//1100 0010 比较匹配时OC1A置位 工作在快速PWM模式 TCCR1B=0X1A;//0001 1001 8分频 ICR1=124;//8kHZ } /****************************************************** 函数功能：占空比设置函数 ******************************************************/ void PWM_set(uchar PWM_temp) { // 100 90 80 70 60 50 40 30 20 10 0 OCR1A=PWM_temp;//匹配值 1 12 25 36 48 61 73 85 98 110 124 } /****************************************************** 函数功能：定时10ms 测量在此期间速度 ******************************************************/ void time0_init() { TIMSK=(1<<TOIE0);//使能T0溢出中断 TCCR0=(0<<CS02)|(1<<CS01)|(1<<CS00);//0000 0010 8M外部晶振 64分频 TCNT0=218; //定时1ms } /****************************************************** 函数功能：读取编码器值 ******************************************************/ /****************************************************** 函数功能：外部中断初始化 ******************************************************/ void INT2_init() { SREG=0X80; MCUCSR=(0<<ISC2); GICR=(1<<INT2); } /****************************************************** 函数功能：键盘控制平台函数 ******************************************************/ void KEY_contral() { temp[0]=ZLG7289_Key(); if((temp[0]==0x09)||(temp[0]==0x0D)) switch(temp[0]) { case 0x09:ZLG7289_Download(1,1, 0, string[10]); fangxiang_flag=1; break; case 0x0D:ZLG7289_Download(1,1, 0, string[11]); fangxiang_flag=2; break; } else if((temp[0]!=0x00)&&(temp[0]!=0x04)&&(temp[0]!=0x04)&&(temp[0]!=0x04)) { if(wei>=5) wei=3; switch(temp[0]) { case 0x03:ZLG7289_Download(1,wei, 0, string[1]); if(wei==3) angle_settemp=string[1]*10; if(wei==4) { angle_set=angle_settemp+string[1]; shuru_flag=1; } break; case 0x07:ZLG7289_Download(1,wei, 0, string[2]); if(wei==3) angle_settemp=string[2]*10; if(wei==4) { angle_set=angle_settemp+string[2]; shuru_flag=1; } break; case 0x0B:ZLG7289_Download(1,wei, 0, string[3]); if(wei==3) angle_settemp=string[3]*10; if(wei==4) { angle_set=angle_settemp+string[3]; shuru_flag=1; } break; case 0x0F:ZLG7289_Download(1,wei, 0, string[4]); if(wei==3) angle_settemp=string[4]*10; if(wei==4) { angle_set=angle_settemp+string[4]; shuru_flag=1; } break; case 0x02:ZLG7289_Download(1,wei, 0, string[5]); if(wei==3) angle_settemp=string[5]*10; if(wei==4) { angle_set=angle_settemp+string[5]; shuru_flag=1; } break; case 0x06:ZLG7289_Download(1,wei, 0, string[6]); if(wei==3) angle_settemp=string[6]*10; if(wei==4) { angle_set=angle_settemp+string[6]; shuru_flag=1; } break; case 0x0A:ZLG7289_Download(1,wei, 0, string[7]); if(wei==3) angle_settemp=string[7]*10; if(wei==4) { angle_set=angle_settemp+string[7]; shuru_flag=1; } break; case 0x0E:ZLG7289_Download(1,wei, 0, string[8]); if(wei==3) angle_settemp=string[8]*10; if(wei==4) { angle_set=angle_settemp+string[8]; shuru_flag=1; } break; case 0x01:ZLG7289_Download(1,wei, 0, string[9]); if(wei==3) angle_settemp=string[9]*10; if(wei==4) { angle_set=angle_settemp+string[9]; shuru_flag=1; } break; case 0x05:ZLG7289_Download(1,wei, 0, string[0]); if(wei==3) angle_settemp=string[0]*10; if(wei==4) { angle_set=angle_settemp+string[0];