pid_adjust_speed.c

基于Cortex-M3的全自动焊接机

#include "globaltype.h"
#include "motor.h"
#include "define.h"
#include  "pwm_init.h"
unsigned short total_EncoCnt[MOTOR_NUMBER];
extern struct placed_relate_parm palced_magor[];
extern unsigned long ulPeriod;
//unsigned int temp_currentduty;




void PID_adjust_speed(unsigned long ulStatus, unsigned char motor)
{
    unsigned long temp;
    unsigned long MOTOR_PWM;
    unsigned long MOTOR_PWM_BIT;
    unsigned char par;
    unsigned int temp_currentduty;
    //stop_sem = 1;电机并未真的停止
    //stop_sem = 0;电机真的停止
    if(!palced_magor[motor].stop_sem)
    {
        switch(motor)
        {
            case 0:
                temp = Motor1_Enco_A;
                switch(palced_magor[0].dir)
                {
                    case Forward:
                      MOTOR_PWM = motor1_pwm_forward_out;
                      MOTOR_PWM_BIT = motor1_pwm_forward_bit_out;
                      break;
                    case Reverse:
                      MOTOR_PWM = motor1_pwm_reverse_out;
                      MOTOR_PWM_BIT = motor1_pwm_reverse_bit_out;
                      break;
                }
                break;
            case 1:
                temp = Motor2_Enco_A;
                switch(palced_magor[1].dir)
                {
                    case Forward:
                      MOTOR_PWM = motor2_pwm_forward_out;
                      MOTOR_PWM_BIT = motor2_pwm_forward_bit_out;
                      break;
                    case Reverse:
                      MOTOR_PWM = motor2_pwm_reverse_out;
                      MOTOR_PWM_BIT = motor2_pwm_reverse_bit_out;
                      break;
                }
                break;
            case 2:
                temp = Motor3_Enco_B;
                switch(palced_magor[2].dir)
                {
                    case Forward:
                      MOTOR_PWM = motor3_pwm_forward_out;
                      MOTOR_PWM_BIT = motor3_pwm_forward_bit_out;
                      break;
                    case Reverse:
                      MOTOR_PWM = motor3_pwm_reverse_out;
                      MOTOR_PWM_BIT = motor3_pwm_reverse_bit_out;
                      break;
                }
                break;
        }
        if ( ulStatus & temp )                                //  如果PC6的中断状态有效
        {
            total_EncoCnt[motor]++; //这个变量暂时用不到
            palced_magor[motor].EncoCnt ++;   
            if(palced_magor[motor].EncoCnt>=PREDEAL_CODE )//上电之后的预处理,其实最好每次电机停止到重新运动都最好有个预处理，因此要有个清零动作
            {
               palced_magor[motor].real_timerunit = palced_magor[motor].real_timerunit/NORMOLLATION_SPEED;
               switch(palced_magor[motor].motor_exe_step)
               {
                   case 1:
                        palced_magor[motor].theory_timerunit = palced_magor[motor].start_speed - (palced_magor[motor].EncoCnt*palced_magor[motor].up_normalization_step)/NORMALIZE;
                        palced_magor[motor].theory_timerunit = palced_magor[motor].theory_timerunit/NORMOLLATION_SPEED; 
                        par = 100;
                        break;
                   case 2:
                        palced_magor[motor].theory_timerunit = palced_magor[motor].const_speed;
                        palced_magor[motor].theory_timerunit = palced_magor[motor].theory_timerunit/NORMOLLATION_SPEED;
                        par = 100;
                        break;
                   case 3:
                        if(palced_magor[motor].usr_require==0x02)//以固定速速转动指定距离
                        {
                            palced_magor[motor].theory_timerunit = palced_magor[motor].const_speed + (palced_magor[motor].EncoCnt*palced_magor[motor].down_normalization_step)/NORMALIZE;
                            palced_magor[motor].theory_timerunit = palced_magor[motor].theory_timerunit/NORMOLLATION_SPEED;
                            par = 100;
                        }
                        break;
               }
               switch(palced_magor[motor].motor_exe_step)//调整占空比
               {
                    
                  
                    case 1:
                         
                          if(palced_magor[motor].theory_timerunit >=palced_magor[motor].real_timerunit)
                          {
                              palced_magor[motor].motor_current_duty = palced_magor[motor].motor_current_duty*(par-
                              (par*(palced_magor[motor].theory_timerunit - palced_magor[motor].real_timerunit)/palced_magor[motor].real_timerunit)
                                                                        )/par
                               // -(palced_magor[motor].theory_timerunit - palced_magor[motor].real_timerunit)/C_RATE
                                ;       
                          }
                          else
                          {
                              palced_magor[motor].motor_current_duty = palced_magor[motor].motor_current_duty*(par+
                              (par*(palced_magor[motor].real_timerunit - palced_magor[motor].theory_timerunit)/palced_magor[motor].real_timerunit)
                                                                        )/par
                               // +(palced_magor[motor].real_timerunit - palced_magor[motor].theory_timerunit)/C_RATE
                                ;
                          }
                       
                          switch(motor)
                          {
                              case 0:
                                temp_currentduty = palced_magor[motor].motor_current_duty;
                                //temp_currentduty = ulPeriod -100;
                              break;
                              case 1:
                                temp_currentduty = palced_magor[motor].motor_current_duty;
                              break;
                              case 2:
                                temp_currentduty = palced_magor[motor].motor_current_duty;
                              break;
                          }
                          if(palced_magor[motor].motor_current_duty < ulPeriod)                //下面两个函数参数可是不一致的哦
                          {                            
                              PWMPulseWidthSet(PWM_BASE, MOTOR_PWM, temp_currentduty);
                              PWMOutputState(PWM_BASE, MOTOR_PWM_BIT , true);      
                          }
                          else
                          {
                              PWMPulseWidthSet(PWM_BASE, MOTOR_PWM, (ulPeriod - 100));
                              PWMOutputState(PWM_BASE, MOTOR_PWM_BIT , true);
                          }
                          if((palced_magor[motor].EncoCnt>=palced_magor[motor].total_codenumber) && (palced_magor[motor].usr_require==0x02))
                          {
                              palced_magor[motor].sem = 1;
                              switch(motor)
                              {
                                  case 0:
                                    Motor1_Enable(Stop);
                                  break;
                                  case 1:
                                    Motor2_Enable(Stop);
                                    break;
                                  case 2: