msp430f5529标准库学习之位置速度串级pid

经过上一次pid的学习以及2021年电赛的送药小车题目，我对pid进一步学习，位置速度串级pid分为内环和外环。外环是位置环，主要控制小车的移动距离以及转向，内环是速度环，传入值是外环的传出值，传出值是占空比。具体流程图如下。

 定时器部分代码如下

void tima0_init(uint16_t arr)
{
    //gpio初始化
    GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P1,GPIO_PIN2 | GPIO_PIN3);//功能输出引脚

    //定时器输出pwm初始化
    Timer_A_outputPWMParam tima0_pwm={0};
    tima0_pwm.clockSource=TIMER_A_CLOCKSOURCE_SMCLK;//时钟源16*1048576hz
    tima0_pwm.clockSourceDivider=TIMER_A_CLOCKSOURCE_DIVIDER_16;//16分频
    tima0_pwm.compareOutputMode=TIMER_A_OUTPUTMODE_RESET_SET;//复位模式
    tima0_pwm.compareRegister=TIMER_A_CAPTURECOMPARE_REGISTER_1 ;//通道1
    tima0_pwm.dutyCycle=0;//比较值
    tima0_pwm.timerPeriod=arr;//重装载值

    Timer_A_outputPWM(TIMER_A0_BASE,&tima0_pwm);
    tima0_pwm.compareRegister=TIMER_A_CAPTURECOMPARE_REGISTER_2;//通道2
    Timer_A_outputPWM(TIMER_A0_BASE,&tima0_pwm);
}

void tima2_init(void)
{
    //配置更新中断
    Timer_A_initUpModeParam tima_initparam={0};
    tima_initparam.clockSource=TIMER_A_CLOCKSOURCE_SMCLK ;//时钟源16*1048576hz
    tima_initparam.clockSourceDivider=TIMER_A_CLOCKSOURCE_DIVIDER_64;//64分频,262,144
    tima_initparam.startTimer=false;//不立即启动
    tima_initparam.timerClear=TIMER_A_DO_CLEAR;//定时器重置
    tima_initparam.timerPeriod=13107;//0.05s
    tima_initparam.timerInterruptEnable_TAIE=TIMER_A_TAIE_INTERRUPT_ENABLE;//中断使能
    Timer_A_initUpMode(TIMER_A2_BASE,&tima_initparam);//初始化定时器，正计数模式，
    Timer_A_startCounter( TIMER_A2_BASE,TIMER_A_UP_MODE);//启动定时器，递增模式

    GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P2,GPIO_PIN3 | GPIO_PIN4 | GPIO_PIN5);//复用gpio

    //配置输入捕获
    Timer_A_initCaptureModeParam tima2_capture={0};
    tima2_capture.captureInputSelect=TIMER_A_CAPTURE_INPUTSELECT_CCIxA;//决定输入选择
    tima2_capture.captureInterruptEnable=TIMER_A_CAPTURECOMPARE_INTERRUPT_ENABLE;//输入捕获中断使能
    tima2_capture.captureMode=TIMER_A_CAPTUREMODE_RISING_EDGE;//上升沿沿触发
    tima2_capture.captureOutputMode=TIMER_A_OUTPUTMODE_OUTBITVALUE;//捕获输出模式选择
    tima2_capture.captureRegister=TIMER_A_CAPTURECOMPARE_REGISTER_1;//捕获通道选择1
    tima2_capture.synchronizeCaptureSource=TIMER_A_CAPTURE_SYNCHRONOUS;     //捕获源与计时器时钟同步
    Timer_A_initCaptureMode(TIMER_A2_BASE,&tima2_capture);//输入捕获初始化
    tima2_capture.captureRegister=TIMER_A_CAPTURECOMPARE_REGISTER_2;//捕获通道选择2
    Timer_A_initCaptureMode(TIMER_A2_BASE,&tima2_capture);//输入捕获初始化
    tima2_capture.captureRegister=TIMER_A_CAPTURECOMPARE_REGISTER_0;//捕获通道选择2
    Timer_A_initCaptureMode(TIMER_A2_BASE,&tima2_capture);//输入捕获初始化

    //初始化p2.3引脚，配置外部中断
    GPIO_setAsInputPinWithPullDownResistor(GPIO_PORT_P2,GPIO_PIN3);
    GPIO_clearInterrupt(GPIO_PORT_P2,GPIO_PIN3);
    GPIO_selectInterruptEdge(GPIO_PORT_P2,GPIO_PIN3,GPIO_LOW_TO_HIGH_TRANSITION);
    GPIO_enableInterrupt(GPIO_PORT_P2,GPIO_PIN3);
}

void timb0_init(void)
{
    Timer_B_initUpModeParam timb_initparam={0};
    timb_initparam.clockSource=TIMER_B_CLOCKSOURCE_SMCLK ;//时钟源16*1048576hz
    timb_initparam.clockSourceDivider=TIMER_B_CLOCKSOURCE_DIVIDER_64;//64分频,262,144
    timb_initparam.startTimer=false;//不立即启动
    timb_initparam.timerClear=TIMER_B_DO_CLEAR;//定时器重置
    timb_initparam.timerPeriod=13107;//0.05s
    timb_initparam.timerInterruptEnable_TBIE=TIMER_B_TBIE_INTERRUPT_ENABLE;//中断使能
    Timer_B_initUpMode(TIMER_B0_BASE,&timb_initparam);//初始化定时器，正计数模式，
    Timer_B_startCounter( TIMER_B0_BASE,TIMER_B_UP_MODE);//启动定时器，递增模式

    GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P3,GPIO_PIN5 | GPIO_PIN6);//复用gpio

    //配置输入捕获
    Timer_B_initCaptureModeParam timb0_capture={0};
    timb0_capture.captureInputSelect=TIMER_B_CAPTURE_INPUTSELECT_CCIxA;//决定输入选择
    timb0_capture.captureInterruptEnable=TIMER_B_CAPTURECOMPARE_INTERRUPT_ENABLE;//输入捕获中断使能
    timb0_capture.captureMode=TIMER_B_CAPTUREMODE_RISING_EDGE;//上升沿沿触发
    timb0_capture.captureOutputMode=TIMER_B_OUTPUTMODE_OUTBITVALUE;//捕获输出模式选择
    timb0_capture.captureRegister=TIMER_B_CAPTURECOMPARE_REGISTER_5;//捕获通道选择5
    timb0_capture.synchronizeCaptureSource=TIMER_B_CAPTURE_SYNCHRONOUS;     //捕获源与计时器时钟同步
    Timer_B_initCaptureMode(TIMER_B0_BASE,&timb0_capture);//输入捕获初始化
    timb0_capture.captureRegister=TIMER_B_CAPTURECOMPARE_REGISTER_6;//捕获通道选择6
    Timer_B_initCaptureMode(TIMER_B0_BASE,&timb0_capture);//输入捕获初始化
}

int right_cap_count=0;
int left_cap_count=0;
int right_tal_count=0;
int left_tal_count=0;

#pragma vector=PORT2_VECTOR
__interrupt void P2_3ISR(void)
{
    GPIO_clearInterrupt(GPIO_PORT_P2,GPIO_PIN3);
    if (P3IN & BIT6)
       left_cap_count--;//捕获上升沿次数;
    else
       left_cap_count++;//捕获上升沿次数
}


//定时器A1溢出中断
#pragma vector=TIMER2_A1_VECTOR
__interrupt void TIMER2_A1_ISR (void)
{

    float left_speed=0;//左轮转轴速度
    float right_speed=0;//右轮转轴速度
    float left_pwm=0;
    long left_temp=0;//pid调试用，表示当前速度
    float right_pwm=0;
    long right_temp=0;//pid调试用，表示当前速度
   switch(__even_in_range(TA2IV, 14))//中断事件
   {
       case 0:
           if (P3IN & BIT6)
               right_cap_count--;//捕获上升沿次数;
           else
               right_cap_count++;//捕获上升沿次数
           Timer_A_clearCaptureCompareInterrupt(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_0);//清除捕获中断
           break;//无中断TA1IV_NONE
       case 2: //ccr1中断TA1IV_TACCR1
           if (P2IN & BIT5)
              right_cap_count--;//捕获上升沿次数;
          else
              right_cap_count++;//捕获上升沿次数
           Timer_A_clearCaptureCompareInterrupt(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1);//清除捕获中断
           break;
       case 4:
           if (P2IN & BIT4)
               right_cap_count++;//捕获上升沿次数;
           else
               right_cap_count--;//捕获上升沿次数
           Timer_A_clearCaptureCompareInterrupt(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_2);//清除捕获中断
           break;//ccr2中断TA1IV_TACCR2
       case 6: break;//ccr3中断TA1IV_3
       case 8: break;//ccr4中断TA1IV_4
       case 10: break;//ccr5中断TA1IV_5
       case 12: break;//ccr6中断TA1IV_6
       case 14: //溢出中断TA1IV_TAIFG
       {
           //数据处理
           left_speed=((float)left_cap_count/PULSEE_NUMBER)*20;//速度=计数值/转一圈的计数值*时间系数，单位：圈/s
           right_speed=((float)right_cap_count/PULSEE_NUMBER)*20;//速度=计数值/转一圈的计数值*时间系数，单位：圈/s
           //UART_printf(USCI_A0_BASE,"left_speed=%f,right_speed=%f\r\n",left_speed,right_speed);
           //pid算法实现
           if(mator.open)
           {
               //(位置离散pid)
               left_pwm = PID_realize(&left_pid,left_speed*SPEED_AMPLIF);//逐步调整pwm值到set_point(目标值)(位置离散pid)      速度放大100倍，便于调试
               if(left_pwm<=0)
                   left_pwm=0;
               mator.left_compara=(long)left_pwm;
               right_pwm = PID_realize(&right_pid,right_speed*SPEED_AMPLIF);//逐步调整pwm值到set_point(目标值)(位置离散pid)      速度放大100倍，便于调试
               //UART_printf(USCI_A0_BASE,"left_pwm=%.2f,right_pwm=%.2f\r\n",left_pwm,right_pwm);
               if(right_pwm<=0)
                     right_pwm=0;
               mator.right_compara=(long)right_pwm;

               if(mator.right_compara>MAX_PWM_COMPARE)//限制
                   mator.right_compara=MAX_PWM_COMPARE;
               if(mator.left_compara>MAX_PWM_COMPARE)//限制
                   mator.left_compara=MAX_PWM_COMPARE;
           }
           //UART_printf(USCI_A0_BASE,"right_tal=%d\r\n",right_tal_count);
           left_temp=left_speed*SPEED_AMPLIF;
           right_temp=right_speed*SPEED_AMPLIF;
           set_computer_value(SEND_FACT_CMD, CURVES_CH1, &left_temp, 1);//发送实际值,通道选择ch1以让软件显示图像
           set_computer_value(SEND_FACT_CMD, CURVES_CH2, &right_temp, 1);//发送实际值,通道选择ch1以让软件显示图像

           right_cap_count=0;//计数清零
           left_cap_count=0;//计数清零
           Timer_A_clearTimerInterrupt(TIMER_A2_BASE);//清除更新中断
           break;
       }
   }
}

//定时器B0中断
#pragma vector=TIMER0_B1_VECTOR
__interrupt void TIMER0_B0_ISR (void)
{
    switch(__even_in_range(TB0IV, 14))//中断事件
       {
           case 0: break;//无中断TA1IV_NONE
           case 2: break;//ccr1中断TA1IV_TACCR1
           case 4:break;//ccr2中断TB0IV_TBCCR2
           case 6: break;//ccr3中断TB0IV_TBCCR3
           case 8: break;//ccr4中断TB0IV_TBCCR4
           case 10:
               if (P3IN & BIT6)
                   left_cap_count--;//捕获上升沿次数;
               else
                   left_cap_count++;//捕获上升沿次数
               Timer_B_clearCaptureCompareInterrupt(TIMER_B0_BASE,TIMER_B_CAPTURECOMPARE_REGISTER_5);//清除捕获中断
               break;//ccr5中断TB0IV_TBCCR5
           case 12:
               if (P3IN & BIT5)
               //if(P2IN & BIT3)
                   left_cap_count++;//捕获上升沿次数;
               else
                   left_cap_count--;//捕获上升沿次数
               Timer_B_clearCaptureCompareInterrupt(TIMER_B0_BASE,TIMER_B_CAPTURECOMPARE_REGISTER_6);//清除捕获中断
               break;//ccr6中断TB0IV_TBCCR6
           case 14: //溢出中断TB1IV_TBIFG
           {
               Timer_B_clearTimerInterrupt(TIMER_B0_BASE);//清除更新中断
               break;
           }
       }
}

接下来就是pid部分了，位置环的pid算法本质和速度环的算法一样，只是串联起来了，外环决定内环的输入，内环决定系统的输出。

主要代码如下

extern struct _pid_speed{
    float target_val;           //定义设定值
    float err;                  //定义偏差值
    float integral;             //定义积分值
    float err_last;             //定义上一个偏差值
    float Kp,Ki,Kd;             //定义比例、积分、微分系数
}left_pid,right_pid;

extern struct _pid_position{
    float target_val;           //定义设定值
    float err;                  //定义偏差值
    float integral;             //定义积分值
    float err_last;             //定义上一个偏差值
    float Kp,Ki,Kd;             //定义比例、积分、微分系数
}left_position_pid,right_position_pid;

struct _pid_speed left_pid;//left_pid结构体存储数据(位置离散pid)
struct _pid_speed right_pid;//pid结构体存储数据(位置离散pid)
struct _pid_position left_position_pid;//left_pid结构体存储数据(位置离散pid)
struct _pid_position right_position_pid;//pid结构体存储数据(位置离散pid)
void PID_init(void)
{
    left_pid.target_val=0.0;
    left_pid.err=0.0;
    left_pid.err_last=0.0;
    left_pid.integral=0.0;
    left_pid.Kp=3.5;
    left_pid.Ki=1;
    left_pid.Kd=0.8;
    
    right_pid.target_val=0.0;
    right_pid.err=0.0;
    right_pid.err_last=0.0;
    right_pid.integral=0.0;
    right_pid.Kp=3.5;
    right_pid.Ki=1;
    right_pid.Kd=0.8;

    left_position_pid.target_val=0.0;
    left_position_pid.err=0.0;
    left_position_pid.err_last=0.0;
    left_position_pid.integral=0.0;
    left_position_pid.Kp=0.2;
    left_position_pid.Ki=0.001;
    left_position_pid.Kd=0.01;

    right_position_pid.target_val=0.0;
    right_position_pid.err=0.0;
    right_position_pid.err_last=0.0;
    right_position_pid.integral=0.0;
    right_position_pid.Kp=0.2;
    right_position_pid.Ki=0.001;
    right_position_pid.Kd=0.01;
}

void limit_size(float dat1,float dat2)
{
    if(dat1>dat2)
        dat1=dat2;
}

//pid算法(速度pid)
float Speed_PID(struct _pid_speed *pid,float speed)
{
    float Out_Pwm=0;
    pid->err=pid->target_val-speed;//计算目标值与实际值的误差
    pid->integral+=pid->err;//误差累计
    Out_Pwm=pid->Kp*pid->err+pid->Ki*pid->integral+pid->Kd*(pid->err-pid->err_last);//pid算法（位置离散pid）
    pid->err_last=pid->err;//误差传递
    return Out_Pwm;//返回占空比增益
}

//位置pid
float Position_PID(struct _pid_position *pid,float position)
{
    float Out_Pwm=0;
    pid->err=pid->target_val-position;//计算目标值与实际值的误差
    pid->integral+=pid->err;//误差累计
    limit_size(pid->integral,Position_Integral_Max);
    Out_Pwm=pid->Kp*pid->err+pid->Ki*pid->integral+pid->Kd*(pid->err-pid->err_last);//pid算法（位置离散pid）
    pid->err_last=pid->err;//误差传递
    return Out_Pwm;
}

演示视频

VID_20230729_201254