ctrl.c

freesacle的实时操作系统UCOS的开发程序

  /*
 ***************************************************************************************
 * 文件名:  ctrl.C
 *
 * 功能 :	 小车控制台
 *
 * 参数说明 : 
 *
 * 状态  ：
 ****************************************************************************************
 */ 
#include <mc9s12db128.h>   
#include "PWM.h"
#include "display.h"
#include "Uart.h"
#include "ATD.h"
#include "Timers.h"


extern  void time_delay(unsigned int i);

extern unsigned int duty_test ;
extern  int f_a;

extern unsigned char key;

float d_old,srl,sr,sl;
int ssrl,ssr,ssl,ssr_old,ssl_old;
char R_L_old,js,zj,qzh;
unsigned char gate_v[14];
unsigned char  spd_lock,max_d,open_lock; 

unsigned int speed_max,speed_min,speed_turn;

unsigned char base_duty;
unsigned  int sp_d;
/*---------------------------------------------------------------------------------
*舵机的摆角控制 (无修正)
*
*/ 
void turn_R_L(float d){
		 /*
     uart_putchar('N');
		 if(d>=0)
		 display2((unsigned long)d);
		 else
		 display2((unsigned long)((-1)*d));
		 uart_putchar('Y');
		 if(d>=0)
		 display2((unsigned long)d_old);
		 else
		 display2((unsigned long)((-1)*d_old));
		 uart_putchar(' ');
		 										 */
		 //if ((d<(d_old-0.1))||(d>(d_old+0.1)))
		 //{
		 d_old=d;
     d=d/18.0;        //得到输入驼机的占空比
     d=7.5+d;
     PWM2outPut(50,d);
		// }
 }
/*---------------------------------------------------------------------------------
*舵机的摆角控制 (带修正)
*
*/ 
void SP_RL(char temp) 

{ char temp2;
  float temp1,temp3;

  
  /////路径变化强度以及转弯的角度修正    +
  
        /////总的变化强度																	 +
        
        /////计算srl得到了检测到的路径的抖动强度
        /////衰减数为1，倍数关系为4也就是说如果							 
        /////路径变化了1，则需要用4cm的距离来平伏
        
		 if(temp>=11) 
  		 {										 //脱离黑线，
  		    temp=R_L_old;
  		     
  		 }
   temp2=temp;
   
   		 
     
     if(temp<R_L_old)					 
      {												  
        ssrl=ssrl+(temp-R_L_old)*4;	 //路径的抖动强度//4倍关系
 
     
        ssl=ssl+(R_L_old-temp)*3;	   //左偏转强度//3倍关系

        ssr-=2; 
      } 
      
     else if(temp>R_L_old)		 
      {
        ssrl=ssrl+(temp-R_L_old)*4;	 //路径的抖动强度//4倍关系
        
        ssr=ssr+(temp-R_L_old)*3;	   //右偏转强度//3倍关系

        ssl-=2;   
      }
     
     else
      {
         if(ssrl>0)  //总平滑
            ssrl--;
         else
            ssrl++;
         
         
         ssr--;
         if(ssr<=0)   //右平滑
            ssr=0;
         
         ssl--;
         if(ssl<=0)		//左平滑
            ssl=0;
        
      }
 if(ssr>20)	 //最高值限制
   ssr=20;
 if(ssl>20)
   ssl=20;
 if(ssrl>20)
   ssrl=20;
 


  /////路径变化强度以及转弯的角度修正    -

   
  temp1=(float)ssr;
  temp3=(float)ssl;
  temp1=temp1*0.2;
  temp3=temp3*0.2;
  temp3=temp1-temp3;
  
  temp1=(float)temp2;
  
  if((temp>9)||(temp<(-9))) 
    {
      temp1=temp1*2.4;  
    }
    
  else if((temp>7)||(temp<(-7))) 
    {
      temp1=temp1*2.3;  
    }   
  else if((temp>5)||(temp<(-5))) 
    {
      temp1=temp1*2.2;  
    }  
  else if((temp>3)||(temp<(-3))) 
    {
      temp1=temp1*2.1;  
    }
  
  else if((temp>1)||(temp<(-1)))
    {
      temp1=temp1*2.0;  
    } 
  else 
    {
      temp1=temp1*1.8; 
    }
    

 temp1+=temp3; 

    R_L_old=temp; 
    	 //角度限制     +
    	 if(temp1<-26)
        temp1=-26;
       else if (temp1>26)
        temp1=26;
       //角度限制     -
         
       temp1=temp1/18.0; //*0.045;       
       temp1=7.5-temp1;  //得到输入驼机的占空比
    	 PWM2outPut(50,temp1);
}
/*---*电机的速度控制
*
* 
 void Speed_ctrl(float v){
     v=v/18.0;        //得到输入电机的占空比
     v=v+7.5 ;
     PWM3outPut(1000,v);
 }*------------------------------------------------------------------------------

*/
 /*-------------------------------------------------------------------------
 *
 * 测试程序。test_
 */
 /*-------------------------------------------------------------------------
 *
 * 接收管门限电压检测  led_re_test
 * 函 数 名:  void led_re_test(void);
 * 输入参数:  无
 * 输出参数:  无
 * 全局变量:  gate_v
 */
void led_re_test(void) 
{


}


void speed_ctrl(void)

{	

if(sp_d>80) {
    ssr_old=0;
    ssl_old=0;
  
  }  

if((ssr>6)||(ssl>6))
    {
      if(ssr>ssr_old)
        {
         if(speed>140)
            sp_d=0; 
         ssr_old=ssr;
        }
      if(ssl>ssl_old)
        {
         if(speed>140)
            sp_d=0; 
         ssl_old=ssl;
        }
    } 

  
   
   if(speed>speed_max)
    {
     if(sp_d<80)
      {
        duty_test=0;
        qzh=1;
        
      } else 
      {
       sp_d=81; 
      }
    }
     
   else if(speed>(speed_max-10))
    {
     if(sp_d<80)
      {
        duty_test=0;
        qzh=1;
        
      }else 
      {
       sp_d=91; 
      }
    }

    else if(speed>200)
    {
     if(sp_d<80)
      {
        duty_test=0;
      } 
    }
   else if(speed>190)
    {
     if(sp_d<80)
      {
        duty_test=0;
      }
    }

   else if(speed>180)
    {
     if(sp_d<80)
      {
        duty_test=0;
      }
    }
   else if(speed>170)
    {
     if(sp_d<80)
      {
        duty_test=0;
      }
    }
  
   else if(speed>speed_min)
    {
     if(sp_d<80)
      {
        duty_test=0;
      }		
    }

    
   else
    { 
     if(sp_d<80)
      { 
        if(!qzh) 
          {
              sp_d=81;
              ssr=0;
              ssl=0;
           }
      }
      
     else
     {
       if(speed<130)
          sp_d=87;
     }
    }
   		 
  if(qzh) 
    {
      if(speed>9)
         qzh=0;
    }
   	
  if(sp_d>80) 
    {
      if((R_L_old>5)||(R_L_old<(-5)))
       {
          {
            if(speed>speed_turn)
                 sp_d=78 ;
            else
                 sp_d=87 ;
          }
       }
      duty_test=(sp_d-80)*2+base_duty;
    }
    
if(sp_d<80)
    PTM=0xff;
else
    PTM=0x0;

/*    
if((ssrl>10)||(ssrl<(-10)))
    PORTB&=0x0f;
else
    PORTB|=0xf0;

*/
//if(duty_test>95)
//   sp_d=96;

PWM3outPut(1000,duty_test);  
     
}


//////////////////////////////////    next _ 1
void speed_ctrl_next_1(void)

{	
		/////////////////////////减速触发   +
if(sp_d>80) {
    ssr_old=0;
    ssl_old=0;
  
  }  
if((ssr>6)||(ssl>6))
    {
      if(ssr>ssr_old)
        {
         if(speed>140)
            sp_d=0; 
         ssr_old=ssr;
        }
      if(ssl>ssl_old)
        {
         if(speed>140)
            sp_d=0; 
         ssl_old=ssl;
        }
    } 

  
   		/////////////////////////减速触发   -
   		
 if(speed<(speed_min+10))
  {
   if(sp_d<80)
    {
      sp_d=81;///条件不满足，刹车无效
    }
  }
 
  		
	 ////不在刹车状态
  if(sp_d>80) 
    {
      if((R_L_old>5)||(R_L_old<(-5))) //处于弯道
       {
          if(speed>speed_turn-10)
            {
               sp_d=78;			//就要超过转弯的安全速度，要减速
            }
          else
            {
               duty_test=(sp_d-80)*5/4+base_duty;		//驱动占空比调节 
            }
       }
      else 
       {
          if(speed>speed_max-10)
            {
               sp_d=78;			//就要超过转弯的安全速度，要减速
            }
          else
            {
               duty_test=(sp_d-80)*2+base_duty;		//驱动占空比调节 
            }
       }
    }


/////////刹车    +    
if(sp_d<80)
    PTM=0xff;
else
    PTM=0x00;
/////////刹车    - 
if(duty_test>95)
  {
    duty_test=90;
  }

PWM3outPut(1000,duty_test);  
     
}

//////////////////////////////////    next _ 2
void speed_ctrl_next_2(void)
{	
		/////////////////////////减速触发   +
if(sp_d>80) {
    ssr_old=0;
    ssl_old=0;
  
  }  
if((ssr>6)||(ssl>6))
    {
      if(ssr>ssr_old)
        {
         if(speed>140)
            sp_d=0; 
         ssr_old=ssr;
        }
      if(ssl>ssl_old)
        {
         if(speed>140)
            sp_d=0; 
         ssl_old=ssl;
        }
    } 

   		/////////////////////////减速触发   -
   		
 if(speed<(speed_min+10))
  {
   if(sp_d<80)
    {
      sp_d=81;///条件不满足，刹车无效
    }
  }
 
  		
	 ////不在刹车状态
  if(sp_d>80) 
    {
      if((R_L_old>5)||(R_L_old<(-5))) //处于弯道
       {
          if(speed>speed_turn-10)
            {
               sp_d=78;			//就要超过转弯的安全速度，要减速
            }
          else
            {
               duty_test=(sp_d-80)*5/4+base_duty;		//驱动占空比调节 
            }
       }
      else 
       {
          if(speed>speed_max-10)
            {
               sp_d=78;			//就要超过转弯的安全速度，要减速
            }
          else
            {
               duty_test=(sp_d-80)*2+base_duty;		//驱动占空比调节 
            }
       }
    }


/////////刹车    +    
if(sp_d<80)
    PTM=0xff;
else
    PTM=0x0;
/////////刹车    - 
if(duty_test>95)
  {
    duty_test=90;
  }

PWM3outPut(1000,duty_test);  
     
}