pid_adjust_speed.c

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

                                    Motor3_Enable(Stop);
                                    break;
                               }
                              palced_magor[motor].stop_sem = 1;
                              palced_magor[motor].old_usr_require = 0;
                              palced_magor[motor].EncoCnt=0;
                              palced_magor[motor].motor_exe_step=1;
                          }
                          
                          if(palced_magor[motor].EncoCnt>=palced_magor[motor].g_up_code)
                          {
                              palced_magor[motor].motor_exe_step = 2;
                              palced_magor[motor].EncoCnt=0;                         
                          }
                        break;
                    case 2:
                          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)
                          {
                              if(temp_currentduty < 2000) temp_currentduty = 2000;
                              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].usr_require==0x02)//指定距离
                          { 
                        
                              if((palced_magor[motor].EncoCnt>=palced_magor[motor].g_stab_codenumber)
                                 && (palced_magor[motor].const_speed<palced_magor[motor].start_speed))
                              {
                                  palced_magor[motor].motor_exe_step = 3;
                                  palced_magor[motor].EncoCnt=0;
                              }
                              
                              if((palced_magor[motor].EncoCnt + palced_magor[motor].g_up_code) >= palced_magor[motor].total_codenumber)
                              {
                                  palced_magor[motor].sem = 1;
                                  switch(motor)
                                  {
                                      case 0:
                                        Motor1_Enable(Stop);
                                      break;
                                      case 1:
                                        Motor2_Enable(Stop);
                                        break;
                                      case 2:
                                        Motor3_Enable(Stop);
                                        break;
                                   }
                                  palced_magor[motor].stop_sem = 1;
                                  palced_magor[motor].old_usr_require = 0;
                                  palced_magor[motor].EncoCnt=0;
                                  palced_magor[motor].motor_exe_step=1;
                              }
                          }
                          if((palced_magor[motor].old_usr_require==0x02)&&(palced_magor[motor].usr_require==0x01))
                          {
                              palced_magor[motor].usr_require=0x02;                             
                          }
                        break;
                    case 3:
                          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;
                          }
                          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;
                          }
                          switch(motor)
                          {
                              case 0:
                                temp_currentduty = palced_magor[motor].motor_current_duty;
                              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].g_down_code)
                          {
                              palced_magor[motor].sem = 1;
                              switch(motor)
                              {
                                  case 0:
                                    Motor1_Enable(Stop);
                                  break;
                                  case 1:
                                    Motor2_Enable(Stop);
                                    break;
                                  case 2:
                                    Motor3_Enable(Stop);
                                    break;
                          }
                          palced_magor[motor].stop_sem = 1;
                          palced_magor[motor].old_usr_require = 0;
                          palced_magor[motor].EncoCnt=0;
                          palced_magor[motor].motor_exe_step=1;
                          }
                          break;
               }
            }
        }
    }   
}