main.c

基于MC9S12单片机的智能汽车控制程序,包括路径检测,转向控制,速度测量及电机的PID控制.

#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */
#include <math.h>
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"

#include "main_asm.h" /* interface to the assembly module */

unsigned int track;
unsigned char rudder_data;
unsigned char motor_data;
unsigned int Get_Track_Info(void);

char direction;
char track_error;
char error_buffer;
char d1_track_error;
char track_error_array[7]=0;
char Rudder_Kp;
char Rudder_Kd;
char Rudder_Uk;
unsigned char rudder_pwm;

unsigned char motor_pwm;
unsigned char current_speed;
unsigned char setting_speed;
unsigned char te=0;
unsigned char dte=0;
char speed_error;
char speed_error_array[3]=0;
char motor_dUk;
unsigned char motor_Kp;
unsigned char motor_Ki;
unsigned char motor_Kd;

void  motor_startup(void);
void  change_to_neg_direction(void);
void  change_to_pos_direction(void);

void  track_error_code(void);
void  d_track_error_calc(void);
void  rudder_PID_control(void);

void  d_speed_error_calc(void);
void  motor_speed_control(void);

void  delay(void);
void  delay_100us(void);
void  delay_5ms(void);
void  delay_50ms(void);
void  delay_500ms(void);

/*const unsigned char speed_set[5][21]=
  {
      {8,8,9,9,9,10,10,11,11,12,12,12,11,11,10,10,9,9,9,8,8},
      {9,9,9,10,10,11,11,12,12,13,13,13,12,12,11,11,10,10,9,9,9},
      {10,10,10,11,11,12,12,13,13,14,14,14,13,13,12,12,11,11,10,10,10},
      {9,9,9,10,10,11,11,12,12,13,13,13,12,12,11,11,10,10,9,9,9},
      {8,8,9,9,9,10,10,11,11,12,12,12,11,11,10,10,9,9,9,8,8}
  };*/
const unsigned char speed_set[5][21]=
  {
      {12,12,12,12,12,12,12,12,12,13,13,13,12,12,12,12,12,12,12,12,12},
      {12,12,12,12,12,12,12,12,12,13,13,13,12,12,12,12,12,12,12,12,12},
      {13,13,13,13,13,13,13,14,14,14,14,14,14,14,13,13,13,13,13,13,13},
      {12,12,12,12,12,12,12,12,12,13,13,13,12,12,12,12,12,12,12,12,12},
      {12,12,12,12,12,12,12,12,12,13,13,13,12,12,12,12,12,12,12,12,12}
  };
/*--------------------------------------------------------------*/
/*  main function                                               */
/*--------------------------------------------------------------*/
void  main(void)
  {
    EnableInterrupts;
  
    DDRA=0xF0;
    DDRB=0x00;
    DDRP=0xFF;

    PWMDTY3=152;
    Rudder_Kp=3;
    Rudder_Kd=1;
    
    motor_Kp=3;
    motor_Ki=1;
    motor_Kd=1;
    
    Clock_Initial();
    SCI0_Initial();
    PWM_Initial();
    motor_startup();
    
    PAC_Initial();
    RTICTL=0x59;    //RTI Interval: 10.24ms
    CRGINT_RTIE=1;
    
    
    while(1)
      {
        track=Get_Track_Info();
        track_error_code();
        
      }
  }

/*--------------------------------------------------------------*/
/*  motor startup function                                      */
/*  start slowly to protect the deive circuit                   */
/*--------------------------------------------------------------*/
void  motor_startup(void)
  {
    for(PWMDTY0=75;PWMDTY0<132;PWMDTY0++)
      {
        delay();
      }
  }
/*--------------------------------------------------------------*/
/*  change_to_neg_direction_function                            */
/*  change the direction slowly to protect the deive circuit    */
/*--------------------------------------------------------------*/
void  change_to_neg_direction(void)
  {
    PWMDTY3=rudder_data;
    motor_data=PWMDTY0;
    for(PWMDTY0=motor_data;PWMDTY0>20;PWMDTY0--)
      {
        delay();
        track=Get_Track_Info();
      }
  }
/*--------------------------------------------------------------*/
/*  change_to_pos_direction_function                            */
/*  change the direction slowly to protect the deive circuit    */
/*--------------------------------------------------------------*/
void  change_to_pos_direction(void)
  {
     for(PWMDTY0=20;PWMDTY0<120;PWMDTY0++)
      {
        delay();
        track=Get_Track_Info();  
      }
  }
/*--------------------------------------------------------------*/
/*  track error code function                                   */
/*  calculate the error between the car's center and track      */
/*--------------------------------------------------------------*/
void track_error_code(void)
  {
    if (track!=0)
      {
        switch (track)
          {
            case 0b0000000000000001:
              track_error=10;
              error_buffer=track_error;
              break;
            case 0b0000000000000011:
              track_error=9;
              error_buffer=track_error;
              break;
            case 0b0000000000000010:
              track_error=8;
              error_buffer=track_error;
              break;
            case 0b0000000000000110:
              track_error=7;
              error_buffer=track_error;
              break;
            case 0b0000000000000100:
              track_error=6;
              error_buffer=track_error;
              break;
            case 0b0000000000001100:
              track_error=5;
              error_buffer=track_error;
              break;
            case 0b0000000000001000:
              track_error=4;
              error_buffer=track_error;
              break;
            case 0b0000000000011000:
              track_error=3;
              error_buffer=track_error;
              break;
            case 0b0000000000010000:
              track_error=2;
              error_buffer=track_error;
              break;
            case 0b0000000000110000:
              track_error=1;
              error_buffer=track_error;
              break;
            case 0b0000000000100000:
              track_error=0;
              error_buffer=track_error;
              break;
            case 0b0000000001100000:
              track_error=-1;
              error_buffer=track_error;
              break;
            case 0b0000000001000000:
              track_error=-2;
              error_buffer=track_error;
              break;
            case 0b0000000011000000:
              track_error=-3;
              error_buffer=track_error;
              break;
            case 0b0000000010000000:
              track_error=-4;
              error_buffer=track_error;
              break;
            case 0b0000000110000000:
              track_error=-5;
              error_buffer=track_error;
              break;
            case 0b0000000100000000:
              track_error=-6;
              error_buffer=track_error;
              break;
            case 0b0000001100000000:
              track_error=-7;
              error_buffer=track_error;
              break;
            case 0b0000001000000000:
              track_error=-8;
              error_buffer=track_error;
              break;
            case 0b0000011000000000:
              track_error=-9;
              error_buffer=track_error;
              break;
            case 0b0000010000000000:
              track_error=-10;
              error_buffer=track_error;
              break;
            default:
              track_error=error_buffer;
          }
      }
    else
      {
        if (error_buffer>0)
          track_error=10;
        else if(error_buffer<0)
          track_error=-10;
        else
          track_error=0;
      }
    te=track_error+10;
  }
/*--------------------------------------------------------------*/
/*  d_track_error_calc function                                 */
/*  calculate the e(k-1) and e(k-2)                             */
/*--------------------------------------------------------------*/
void  d_track_error_calc(void)
  {
    track_error_array[0]=track_error_array[1];
    track_error_array[1]=track_error_array[2];
    track_error_array[2]=track_error_array[3];
    track_error_array[3]=track_error_array[4];
    track_error_array[4]=track_error_array[5];
    track_error_array[5]=track_error_array[6];
    track_error_array[6]=track_error;
    d1_track_error=track_error_array[6]-track_error_array[0];
      if(d1_track_error<-2)
        d1_track_error=-2;
      else if(d1_track_error>2)
        d1_track_error=2;
      else
        d1_track_error=d1_track_error;
    dte=d1_track_error+2;
  }
/*--------------------------------------------------------------*/
/*  Rudder PID Control function                                 */
/*  calculate the angle of rudder                               */
/*--------------------------------------------------------------*/
void  rudder_PID_control(void)
  {
    if(3>track_error&track_error>-3)
      {
        Rudder_Kp=2;
        Rudder_Kd=1;
      }
    else
      {
        Rudder_Kp=3;
        Rudder_Kd=2;
      }
    Rudder_Uk=Rudder_Kp*track_error+Rudder_Kd*d1_track_error;
    rudder_pwm=152+Rudder_Uk;
    if(rudder_pwm>182)
      {
        rudder_pwm=182;
        PWMDTY3=rudder_pwm;
      }
    else if(rudder_pwm<122)
      {
        rudder_pwm=122;
        PWMDTY3=rudder_pwm;
      }
    else
      PWMDTY3=rudder_pwm;
  }
/*--------------------------------------------------------------*/
/**/
/*--------------------------------------------------------------*/    
void  d_speed_error_calc(void)
  {
    setting_speed=speed_set[dte][te];
    setting_speed=setting_speed-2;
    speed_error=setting_speed-current_speed;
    speed_error_array[0]=speed_error_array[1];
    speed_error_array[1]=speed_error_array[2];
    speed_error_array[2]=speed_error;
  }
/*--------------------------------------------------------------*/
/*  Motor speed control function*/
/*--------------------------------------------------------------*/    
void motor_speed_control(void)
  {
    char a;
    char b;
    char c;
    d_speed_error_calc();
    a=speed_error_array[2]-speed_error_array[1];
    b=speed_error_array[2];
    c=speed_error_array[2]-2*speed_error_array[1]+speed_error_array[0];
    motor_dUk=motor_Kp*a+motor_Ki*b+motor_Kd*c;
    motor_pwm=PWMDTY0+motor_dUk;
      if(motor_pwm>150)
        PWMDTY0=150;
      else if(motor_pwm<76)
        PWMDTY0=76;
      else
        PWMDTY0=motor_pwm;
    /*setting_speed=speed_set[dte][te]-5;
      if(current_speed>setting_speed&PWMDTY0>6)
        PWMDTY0--;
      else if(current_speed<setting_speed&PWMDTY0<149)
        PWMDTY0++;
      else
        PWMDTY0=PWMDTY0;*/
  }
/*--------------------------------------------------------------*/
/*  delay function                                              */
/*--------------------------------------------------------------*/
void delay(void)
  {
    int i;
    for(i=0;i<300;i++)
      {
        unsigned char j;
        for(j=0;j<40;j++)
          {
            _asm "nop";
          }
      }
  }
/*--------------------------------------------------------------*/
/*  100us delay based on an 24MHz Bus clock                     */
/*--------------------------------------------------------------*/    
void delay_100us(void)
  {
    unsigned char i;
    for(i=0;i<150;i++)
      {
        _asm "nop";
      }
  }
/*--------------------------------------------------------------*/
/*  5ms delay based on an 24MHz Bus clock                     */
/*--------------------------------------------------------------*/    
void delay_5ms(void)
  {
    unsigned char i;
    for(i=0;i<50;i++)
      {
        delay_100us();
      }
  }
/*--------------------------------------------------------------*/
/*  50ms delay based on an 24MHz Bus clock                     */
/*--------------------------------------------------------------*/    
void delay_50ms(void)
  {
    unsigned char i;
    for(i=0;i<10;i++)
      {
        delay_5ms();
      }
  }
/*--------------------------------------------------------------*/
/*  500ms delay based on an 24MHz Bus clock                     */
/*--------------------------------------------------------------*/    
void delay_500ms(void)
  {
    unsigned char i;
    for(i=0;i<10;i++)
      {
        delay_50ms();
      }
  }
/*--------------------------------------------------------------*/
/**/
/*--------------------------------------------------------------*/    
interrupt void RTI_handler(void)
  {
    DisableInterrupts;
    CRGFLG_RTIF=1;
    
    current_speed=PACN0;
    PACN0=0;
    
    track_error_code();
    d_track_error_calc();
    rudder_PID_control();
    
    motor_speed_control();
    EnableInterrupts;
  }