speed.c

第三届飞思卡尔智能车竞赛华东赛一等奖的程序

#include "utile.h"
#include"speed.h"
#include <hidef.h>

#define k_first  60
#define k_second 50
#define k_third  40
#define k_forth  30
         
#define buffer_size 500

#define k_warp 1 //计算I_speed时所用的参数
#define k_length 3
#define warp_max 40

extern int RT_angle,speed_ref,warp,length,true_length,k2,speed_plus;
extern char state;
extern int cross_start_cnt;

int flag,I_flag,START_FLAG;
unsigned int last_time,delta_time;
int pct,delta_pct,pct_base,pct_rec;               //PWM的作用量（单位%）
int RT_speed,spd_rec,start_cnt;
int I_speed,L_speed,O_speed; 
int dest_speed,I_dest_speed;

//记忆数据
int spd_count;

void Spd_Init(void)
{

	TIOS&=~0x02;    //PT1 输入，下降沿捕捉
	TCTL4&=~0x04;   //EDG1A=0
	TCTL4|=0x08;    //EDG1B=1;
	TFLG1|=0x02;    //清除中断标志
	TIE|=0x02;      //允许中断
  
  pct=0;
  flag=0;
  I_flag=0;
  spd_count=0;
  START_FLAG=0;
 
 
 
}

interrupt void Timer_Overflow(void)
{ 
   TFLG2|=0x80;
   flag++;
   if(cross_start_cnt>=start_cnt) flag=0;    
   if(flag>=2) 
   {
      pct=80;
   }
   
   PWMDTY23=pct*(PWMPER23/100);
} 
 
int cal_pct_basevalue(int dest_speed) 
{
   int ret_pct;
   ret_pct=2*(dest_speed+120)/15;
   return ret_pct;
}
 

interrupt void PT1_ISR(void) 
{
  
   unsigned int tc;
 
   char sta;
  
    
  TFLG1|= 0x02;     // 写1，清中断标志
  flag=0;           //TCNT溢出标志归0
  
                          
  tc=TC1;
  
  if(tc>last_time)  delta_time=(tc-last_time)>>3;
  else  delta_time=8192-(last_time-tc>>3);
    
  last_time=tc;
  
	RT_speed=37500/delta_time;   //单位：cm/s
  spd_rec=RT_speed;
                                    
//************************调速模块                  
 sta=state; 
 
 switch(sta)
      {
            case 'I':
              if(I_flag==0) 
              { 
              
                        if(true_length>30)        
                        I_speed=L_speed;
                        
                           else if(true_length>25)                                      //确定弯道速度
                            {
                                if(warp>0)
                                {
                                  
                                      I_speed=RT_speed-6/7*(RT_speed-I_dest_speed)*warp/true_length; 
                                      PTM=0b10000000;
                                }
                                else if(warp<0)
                                {
                                      I_speed=RT_speed+6/7*(RT_speed-I_dest_speed)*warp/true_length; 
                                      PTM=0b10000000;
                                } 
                            }
                            else if(true_length>20)                                      //确定弯道速度
                            {
                                if(warp>0)
                                {
                                      I_speed=RT_speed-5/4*(RT_speed-I_dest_speed)*warp/true_length; 
                                      PTM=0b11000000;  
                                }
                                else if(warp<0)
                                {
                                      I_speed=RT_speed+5/4*(RT_speed-I_dest_speed)*warp/true_length; 
                                      PTM=0b11000000;
                                }
                            } 
                            else if(true_length>10)                                      //确定弯道速度
                            {
                                if(warp>0)
                                {
                                      I_speed=RT_speed-3/2*(RT_speed-I_dest_speed)*warp/true_length; 
                                      PTM=0b11100000;  
                                }
                                else if(warp<0) 
                                {
                                  
                                      I_speed=RT_speed+3/2*(RT_speed-I_dest_speed)*warp/true_length; 
                                      PTM=0b11100000;
                                }
                            } 
                            else  
                            {  
                                if(warp>0)
                                {
                                      I_speed=RT_speed-2*(RT_speed-I_dest_speed)*warp/true_length; 
                                      PTM=0b11110000;
                                }
                                else if(warp<0) 
                                {  
                                      I_speed=RT_speed+2*(RT_speed-I_dest_speed)*warp/true_length; 
                                      PTM=0b11110000;
                                }
                            }
                        // }
              }
           
           
              dest_speed=I_speed;
              break;
              
        
              
          case 'L':
              
              I_flag=0;
              
             dest_speed=L_speed;        
              
              break;
                 
                
          case 'O':

          I_flag=0;
          dest_speed=O_speed;
          break;
          
          default:
          sta='F';//pct=0;
          break;    
      }
          
      pct_base=cal_pct_basevalue(dest_speed);
      
     
      if((RT_speed<=dest_speed+5)&&(RT_speed>=dest_speed-5)) 
      {
              delta_pct=k_first*(dest_speed-RT_speed)/100; 
             
             
               pct=pct_base+delta_pct;
               if(pct<0) 
               {
                  pct=0; 
               }
               else if(pct>95) 
                  pct=100;

              PWMDTY23=pct*(PWMPER23/100);  
     
      }
      
      
      
      else if(RT_speed>dest_speed+5) 
      {
            /*  if(RT_speed<=dest_speed+40) 
                delta_pct=k_second*(dest_speed-RT_speed)/100-50;
              else if(RT_speed<=dest_speed+80) 
                delta_pct=k_third*(dest_speed-RT_speed)/100-52;
              else if(RT_speed<=dest_speed+120) 
                delta_pct=k_forth*(dest_speed-RT_speed)/100-62;
              else 
                delta_pct=-90;
           
           
               pct=pct_base+delta_pct;
                  
               if(pct<0) 
               {
                  pct=0; 
               }
               else if(pct>65) 
                  pct=65;  */
              
                
               PWMDTY23=0;
      }
     
     
     
      else if(RT_speed<dest_speed-5) 
      {
             if(RT_speed>=dest_speed-40) 
                  PWMDTY23=94*(PWMPER23/100);  //delta_pct=k_second*(dest_speed-RT_speed)/100+10;
                else if(RT_speed>=dest_speed-80) 
                  PWMDTY23=96*(PWMPER23/100); // delta_pct=k_third*(dest_speed-RT_speed)/100+12;
                else if(RT_speed>=dest_speed-120) 
                  PWMDTY23=98*(PWMPER23/100); // delta_pct=k_forth*(dest_speed-RT_speed)/100+22;
                else 
                 PWMDTY23=100*(PWMPER23/100);  // delta_pct=60;
                
               /* pct=pct_base+delta_pct;
                      if(pct<0) 
                 {
                    pct=0; 
                 }
                 else if(pct>95) 
                    pct=95; */ 
                
                  
                 //PWMDTY23=100*(PWMPER23/100);  
                               
      }
if(cross_start_cnt>=start_cnt-1&&START_FLAG==0) 
 { 
      L_speed=L_speed+speed_plus;I_dest_speed=I_dest_speed-speed_plus/2; O_speed=O_speed;
      START_FLAG=1;
 } /**/
 if(cross_start_cnt>=start_cnt) 
 { //cross_start_cnt=0;
     PTM=cross_start_cnt;
    spd_count++;
   
   if(spd_count>=65) 
   {
      PWMDTY23=0;
      pct=0;
   }
 }
  
        
 

       
}