www.pudn.com > fpga_pid.zip > hello_world.c, change:2013-10-23,size:12407b


/* 头文件------------------------------------------------------------------*/ 
#include "altera_avalon_pio_regs.h" 
#include "altera_avalon_pwm_regs.h" 
#include "altera_avalon_pwm_routines.h" 
#include "altera_avalon_timer_regs.h" 
#include "altera_avalon_uart_regs.h" 
#include "alt_types.h" 
#include "sys/alt_irq.h" 
#include "system.h" 
#include <stdlib.h>  //使用NULL时需要包含该头文件 
#include <stdio.h> 
/* 宏定义 -------------------------------------------------------------*/ 
#define PRO_CYCLE 5000000    //定时器预分频系数 
 
/* 数据类型声明 -----------------------------------------------*/ 
typedef struct 
{ 
    int Error[3];  //Error[0]:本次误差;Error[1]:上次误差;Error[2]:上上次误差 
    alt_32 ErrorSum;//误差累积和 
    float Kp;    //比例参数 
    float Ki;    //积分参数 
    float Kd;    //微分参数 
    int SetPoint;   //期望值 
}PID; 
 
/* 全局变量 ---------------------------------------------------------*/ 
alt_u8 bufSTR_RF[10];  //UART_RF接收缓存 
alt_u8 bufSTR_IMU[64]; //UArt_IMU接收缓存 
unsigned char fraRF; //UART_RF数据帧标志 
unsigned int couRF; //UART_RF接收字节计数 
unsigned char fraIMU; //UART_IMU数据帧标志 
unsigned int couIMU; //UART_IMU接收字节计数 
 
int count_lf;  //存储COUNTER_LF的计数值 
int count_rf;  //存储COUNTER_RF的计数值 
int count_lb;  //存储COUNTER_LB的计数值 
int count_rb;  //存储COUNTER_RB的计数值 
 
int pwm_lf;  //PWM_LF当前值 
int pwm_rf;  //PWM_RF当前值 
int pwm_lb;  //PWM_LB当前值 
int pwm_rb;  //PWM_RB当前值 
 
PID pid_lf;//左前轮PID参数 
PID pid_rf;//右前轮PID参数 
PID pid_lb;//左后轮PID参数 
PID pid_rb;//右后轮PID参数 
 
 
/* 函数原型声明 -----------------------------------------------*/ 
void PWM_init(void); 
void UART_RF_init(void); 
void UART_IMU_init(void); 
void Uart_RF_send(unsigned char n); 
void Uart_IMU_send(unsigned char n); 
void Uart_RF_send_n(unsigned char *ptr,unsigned char n); 
void Uart_IMU_send_n(unsigned char *ptr,unsigned char n); 
void UART_RF_ISR(void* context, alt_u32 id); 
void UART_IMU_ISR(void* context, alt_u32 id); 
void IMU_init(void); 
void TIMER_init(void); 
void TIMER_ISR(void *context,alt_u32 id); 
char *itoa(int num,char *str,int radix); 
int PID_calc(PID *pid,int CurrentPoint); 
void PID_init(PID *pid,float Kp,float Ki,float Kd,int SetPoint); 
//======================================================== 
//  函数名称:   main 
//======================================================== 
int main(void) 
{ 
//    printf("Hello World!\n"); 
    //======初始化全局变量====== 
    fraRF = 0; 
    couRF = 0; 
    fraIMU = 0; 
    couIMU = 0; 
    count_lf = 0;  //四路计数器赋初值 
    count_rf = 0; 
    count_lb = 0; 
    count_rb = 0; 
    PID_init(&pid_lf,0.024,0.018,0,0);  //四路PID参数赋初值 
    PID_init(&pid_rf,0.021,0.017,0,0); 
    PID_init(&pid_lb,0.025,.019,0,0); 
    PID_init(&pid_rb,0.025,0.018,0,0);           
    //======END初始化全局变量====== 
    IOWR_ALTERA_AVALON_PIO_DATA(COUNTER_CLR_BASE,0);  //计数器COUNTER_LF和COUNTER_RF清零 
    UART_RF_init(); 
    UART_IMU_init(); 
    IMU_init(); 
    PWM_init();   
    IOWR_ALTERA_AVALON_PIO_DATA(MOTOR_CONTROL_BASE,0x05);  //左路和右路同时前转 
    IOWR_ALTERA_AVALON_PIO_DATA(COUNTER_CLR_BASE,1);   //计数器COUNTER_LF和COUNTER_RF清零取消    
    TIMER_init(); 
    while(1); 
    return 0; 
} 
/* 函数定义 ---------------------------------------------------------*/ 
void UART_RF_init() 
{ 
    IOWR_ALTERA_AVALON_UART_CONTROL(UART_RF_BASE, 0x80);//使能接受准备好中断 
    alt_irq_register(UART_RF_IRQ,NULL,UART_RF_ISR);//注册中断函数  
} 
 
void UART_IMU_init() 
{ 
    IOWR_ALTERA_AVALON_UART_CONTROL(UART_IMU_BASE, 0x80);//使能接受准备好中断 
    alt_irq_register(UART_IMU_IRQ,NULL,UART_IMU_ISR);//注册中断函数      
} 
 
void Uart_RF_send(unsigned char data) 
{ 
    alt_u16 status; 
    status=IORD_ALTERA_AVALON_UART_STATUS(UART_RF_BASE); 
    while(!(status&0x0040))//等待发送完成       
        status=IORD_ALTERA_AVALON_UART_STATUS(UART_RF_BASE); 
    IOWR_ALTERA_AVALON_UART_TXDATA(UART_RF_BASE,data); 
} 
 
void Uart_IMU_send(unsigned char data) 
{ 
    alt_u16 status; 
    status=IORD_ALTERA_AVALON_UART_STATUS(UART_IMU_BASE); 
    while(!(status&0x0040))//等待发送完成       
        status=IORD_ALTERA_AVALON_UART_STATUS(UART_IMU_BASE); 
    IOWR_ALTERA_AVALON_UART_TXDATA(UART_IMU_BASE,data); 
} 
 
void Uart_IMU_send_n(unsigned char *ptr,unsigned char n) 
{ 
    for(;n>0;n--) 
    { 
        Uart_IMU_send(*ptr); 
        ptr++; 
    } 
} 
 
void Uart_RF_send_n(unsigned char *ptr,unsigned char n) 
{ 
    for(;n>0;n--) 
    { 
        Uart_RF_send(*ptr); 
        ptr++; 
    } 
} 
 
void UART_IMU_ISR(void* context,alt_u32 id) 
{ 
    alt_u8 buffer_temp; 
    buffer_temp =IORD_ALTERA_AVALON_UART_RXDATA(UART_IMU_BASE); 
    if(fraIMU == 1 && couIMU != 0) 
    { 
        bufSTR_IMU[couIMU] = buffer_temp; 
        couIMU++; 
    } 
    if(buffer_temp == 0xAA && couIMU == 0) 
    { 
        fraIMU = 1;     
        bufSTR_IMU[couIMU] = buffer_temp; 
        couIMU++; 
    } 
    if(couIMU == 64) 
    { 
        fraIMU = 0; 
        couIMU = 0; 
//        Uart_RF_send_n(bufSTR_IMU,64); 
    } 
     
        
} 
 
void UART_RF_ISR(void* context, alt_u32 id) 
{ 
    alt_u8 buffer_temp; 
    buffer_temp =IORD_ALTERA_AVALON_UART_RXDATA(UART_RF_BASE); 
    if(fraRF == 1) 
    { 
        bufSTR_RF[couRF] = buffer_temp; 
        couRF++; 
    } 
    if(buffer_temp == 's'||buffer_temp == 'S') 
    { 
        IOWR_ALTERA_AVALON_PWM_DUTY_CYCLE(PWM_LF_BASE, 999);//设置左前低电平比例 
        IOWR_ALTERA_AVALON_PWM_DUTY_CYCLE(PWM_RF_BASE, 999);//设置右前低电平比例 
        IOWR_ALTERA_AVALON_PWM_DUTY_CYCLE(PWM_LB_BASE, 999);//设置左后低电平比例 
        IOWR_ALTERA_AVALON_PWM_DUTY_CYCLE(PWM_RB_BASE, 999);//设置右后低电平比例 
    } 
    if(buffer_temp == '$') 
        fraRF = 1; 
    if(buffer_temp == '*') 
    { 
        fraRF = 0; 
        pid_lf.SetPoint = bufSTR_RF[0] * 100;    //设置左前低电平比例 
//        printf("%d\n",pid_lf.SetPoint); 
        pid_rf.SetPoint = bufSTR_RF[1] * 100;    //设置右前低电平比例 
        pid_lb.SetPoint = bufSTR_RF[2] * 100;    //设置左后低电平比例 
        pid_rb.SetPoint = bufSTR_RF[3] * 100;    //设置右后低电平比例     
        couRF = 0; 
    } 
} 
 
void IMU_init() 
{ 
    char s1[]="$CMD,OUTPUT,COM1,FPDRAW,0.1*FF"; 
//    char s2[]="$CMD,SAVE,CONFIG*FF"; 
    Uart_IMU_send_n(s1,29); 
//    Uart_IMU_send_n(s2,19);    
} 
 
void TIMER_init(void) 
{ 
    alt_irq_register(TIMER_IRQ,NULL,TIMER_ISR);//注册中断函数 
    IOWR_ALTERA_AVALON_TIMER_PERIODL(TIMER_BASE,PRO_CYCLE);//修改定时时间 
    IOWR_ALTERA_AVALON_TIMER_PERIODH(TIMER_BASE,PRO_CYCLE >> 16); 
    IOWR_ALTERA_AVALON_TIMER_CONTROL(TIMER_BASE,0x07);//启动定时器允许中断,连续计数 
} 
 
void TIMER_ISR(void *context,alt_u32 id) 
{ 
//    char buffer[5] = {'\0','\0','\0','\0','\0'}; 
    alt_u32 tmp_lf = 0;  //左前路计数脉冲临时存放变量 
    alt_u32 tmp_rf = 0;  //右前路计数脉冲临时存放变量 
    alt_u32 tmp_lb = 0;  //左后路计数脉冲临时存放变量 
    alt_u32 tmp_rb = 0;  //右后路计数脉冲临时存放变量 
    alt_u32 duty_lf = 0;  //存储左前路低电平比例 
    alt_u32 duty_rf = 0;  //存储右前路低电平比例 
    alt_u32 duty_lb = 0;  //存储左后路低电平比例 
    alt_u32 duty_rb = 0;  //存储右后路低电平比例      
    tmp_lf=IORD_ALTERA_AVALON_PIO_DATA(COUNTER_LF_BASE);  //读取左前路控制周期内的计数脉冲 
    tmp_rf=IORD_ALTERA_AVALON_PIO_DATA(COUNTER_RF_BASE);  //读取右前路控制周期内的计数脉冲 
    tmp_lb=IORD_ALTERA_AVALON_PIO_DATA(COUNTER_LB_BASE);  //读取左后路控制周期内的计数脉冲 
    tmp_rb=IORD_ALTERA_AVALON_PIO_DATA(COUNTER_RB_BASE);  //读取右后路控制周期内的计数脉冲    
    count_lf = tmp_lf-count_lf; 
    count_rf = tmp_rf-count_rf; 
    count_lb = tmp_lb-count_lb; 
    count_rb = tmp_rb-count_rb; 
    //======计算左前PID控制器输出值====== 
//    pwm_lf += PID_calc(&pid_lf,count_lf);   //增量式PID叠加 
    pwm_lf = PID_calc(&pid_lf,count_lf);   //位置式PID不叠加 
//    pwm_lf *= (910.0/970); //输出死区调整  
//    pwm_lf += 150; 
    if(pwm_lf>970)//输出限幅 
        pwm_lf=970; //占空比不能大于97% 
    else if(pwm_lf<100) 
        pwm_lf=100;    
    duty_lf = 1000-pwm_lf; 
     
    //=======END计算左前PID控制器输出值======== 
    //======计算右前PID控制器输出值====== 
//    pwm_rf += PID_calc(&pid_rf,count_rf); 
    pwm_rf = PID_calc(&pid_rf,count_rf); 
//    pwm_rf *= (910.0/970); //输出死区调整  
//    pwm_rf += 150; 
    if(pwm_rf>970)//输出限幅 
        pwm_rf=970;//占空比不能大于97% 
    else if(pwm_rf<100) 
        pwm_rf=100;   //输出死区   
    duty_rf = 1000-pwm_rf; 
    //=======END计算右前PID控制器输出值======= 
    //======计算左后PID控制器输出值====== 
//    pwm_lb += PID_calc(&pid_lb,count_lb); 
    pwm_lb = PID_calc(&pid_lb,count_lb); 
//    pwm_lb *= (910.0/970); //输出死区调整  
//    pwm_lb += 150; 
    if(pwm_lb>970)//输出限幅 
        pwm_lb=970;//占空比不能大于97% 
    else if(pwm_lb<100) 
        pwm_lb=100;    //输出死区  
    duty_lb = 1000-pwm_lb; 
    //=======END计算左后PID控制器输出值======= 
    //======计算右后PID控制器输出值====== 
//    pwm_rb += PID_calc(&pid_rb,count_rb); 
    pwm_rb = PID_calc(&pid_rb,count_rb); 
//    pwm_rb *= (910.0/970); //输出死区调整  
//    pwm_rb += 150; 
    if(pwm_rb>970)//输出限幅 
        pwm_rb=970;//占空比不能大于97% 
    else if(pwm_rb<100) 
        pwm_rb=100;    //输出死区  
    duty_rb = 1000-pwm_rb; 
    //=======END计算右后PID控制器输出值======= 
    IOWR_ALTERA_AVALON_PWM_DUTY_CYCLE(PWM_LF_BASE, duty_lf);//设置左前低电平比例 
    IOWR_ALTERA_AVALON_PWM_DUTY_CYCLE(PWM_RF_BASE, duty_rf);//设置右前低电平比例 
    IOWR_ALTERA_AVALON_PWM_DUTY_CYCLE(PWM_LB_BASE, duty_lb);//设置左后低电平比例 
    IOWR_ALTERA_AVALON_PWM_DUTY_CYCLE(PWM_RB_BASE, duty_rb);//设置右后低电平比例 
//    printf("%d  %d  %d  %d\n",count_lf,count_rf,count_lb,count_rb);  
//    itoa(count_lf,buffer,10); 
//    Uart_RF_send_n(buffer,5); 
//    itoa(count_rf,buffer,10); 
//    Uart_RF_send_n(buffer,5); 
//    itoa(count_lb,buffer,10); 
//    Uart_RF_send_n(buffer,5); 
//    itoa(count_rb,buffer,10); 
//    Uart_RF_send_n(buffer,5); 
     
    count_lf = tmp_lf; 
    count_rf = tmp_rf;     
    count_lb = tmp_lb; 
    count_rb = tmp_rb;  
    IOWR_ALTERA_AVALON_TIMER_STATUS(TIMER_BASE,0x00);//清状态寄存器 
} 
 
 
void PWM_init(void) 
{ 
    altera_avalon_pwm_init(PWM_LF_BASE, 1000, 999);  //50KHz,占空比为0 
    altera_avalon_pwm_init(PWM_RF_BASE, 1000, 999);     
    altera_avalon_pwm_init(PWM_LB_BASE, 1000, 999);   
    altera_avalon_pwm_init(PWM_RB_BASE, 1000, 999);    
    altera_avalon_pwm_enable(PWM_LF_BASE);  //使能输出 
    altera_avalon_pwm_enable(PWM_RF_BASE);       
    altera_avalon_pwm_enable(PWM_LB_BASE);   
    altera_avalon_pwm_enable(PWM_RB_BASE);        
} 
 
char *itoa(int num,char *str,int radix) 
{     /* 索引表*/ 
    char index[]="0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"; 
    unsigned unum; /* 中间变量 */ 
    int i=0,j,k; 
    /* 确定unum的值 */ 
    if(radix==10&&num<0) /* 十进制负数 */ 
    { 
    unum=(unsigned)-num; 
    str[i++]='-'; 
    } 
    else unum=(unsigned)num; /* 其他情况 */ 
    /* 逆序 */ 
    do{ 
        str[i++]=index[unum%(unsigned)radix]; 
        unum/=radix; 
    }while(unum); 
    str[i]='\0'; 
    /* 转换 */ 
    if(str[0]=='-') k=1; /* 十进制负数 */ 
    else k=0; 
    char temp; 
    for(j=k;j<=(i-1)/2;j++) 
    { 
        temp=str[j]; 
        str[j] = str[i-1+k-j]; 
        str[i-1+k-j] = temp; 
    } 
    return str; 
} 
 
void PID_init(PID *pid,float Kp,float Ki,float Kd,int SetPoint) 
{ 
    pid->Kp = Kp; 
    pid->Ki = Ki; 
    pid->Kd = Kd;       
    pid->SetPoint = SetPoint; 
    pid->Error[0]=0; 
    pid->Error[1]=0; 
    pid->Error[2]=0; 
    pid->ErrorSum = 0; 
} 
 
int PID_calc(PID *pid,int CurrentPoint)   
{ 
    int result; 
    pid->Error[0] = pid->SetPoint - CurrentPoint; 
    //=======增量式PID算法======= 
//    result = pid->Kp * (pid->Error[0] - pid->Error[1]) 
//             + pid->Ki * (pid->Error[0]) 
//             + pid->Kd * (pid->Error[0] - 2 * pid->Error[1] + pid->Error[2]);    
    //=======END增量式PID算法======= 
    //=======位置式PID算法======= 
    pid->ErrorSum += pid->Error[0]; 
    result = pid->Kp * pid->Error[0] 
             + pid->Ki * pid->ErrorSum 
             + pid->Kd * (pid->Error[0] - pid->Error[1]);     
    //=======END位置式PID算法======= 
    pid->Error[2] = pid->Error[1]; 
    pid->Error[1] = pid->Error[0]; 
    return result; 
}