autocontrol.c

主要使用codewarrier开发

//*************************************************************************
//*																								                        *
//*			 *************************智能车控制************************		  *
//*																								                        *
//*************************************************************************
#include "includes.h"

#define CENTER_NUMBER 20
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//智能车当前状态。
unsigned char uc_CarState=STATE_STOP;

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//模型车当方向和前速度
unsigned int ui_Direction=STEER_DTY_CENTER;
unsigned int ui_Speed=MOTOR_DTY_CENTER;

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//传感器检测到的模型车速度。
unsigned int ui_DetectSpeed;

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//存储由vGetDomain()计算的黑线中心坐标。
unsigned char uc_CurPointer=0;

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  signed int  si_SteerEx;
  signed int  si_MotorEx;

unsigned char uc_SteerKp=30;

//*************************************************************************
//*																								                        *
//*			 *************************自动控制函数**********************		  *
//*																								                        *
//*************************************************************************
void vAutoControl(void)
{

/*  if(sc_CenterEx<3&&sc_CenterEx>-3)
    uc_SteerKp=5;
  else
  {
    uc_SteerKp=23;
  }   */
  
  si_SteerEx=sc_CenterEx*23;
    
  ui_Direction=STEER_DTY_CENTER-si_SteerEx;
    
  if(sc_CenterEx>0)
    si_MotorEx=sc_CenterEx;
  else
    si_MotorEx=-sc_CenterEx;
    
  ui_Speed=1000+(16-ui_DetectSpeed)*65;
    
  if(sc_FarEx>10||sc_FarEx<-10)
  {
    ui_Speed-=150; 
  }
  else
  {
    if(sc_FarEx>5||sc_FarEx<-5)
    {
      ui_Speed-=50;
    }
  }

//*************************************************************************
//限定舵机和直流电机输出量，保证不超出范围。
  if(ui_Direction>STEER_DTY_MAX)
    ui_Direction=STEER_DTY_MAX;
  if(ui_Direction<STEER_DTY_MIN)
    ui_Direction=STEER_DTY_MIN;
  
  if(ui_Speed>MOTOR_DTY_MAX)
    ui_Speed=MOTOR_DTY_MAX;
  if(ui_Speed<MOTOR_DTY_MIN)
    ui_Speed=MOTOR_DTY_MAX;
  
//*************************************************************************
//此处才是真正的输出控制
  STEER_DTY=ui_Direction;   //舵机电机输出控制      
  MOTOR_DTY1=ui_Speed;      //直流电机输出控制
  MOTOR_DTY2=ui_Speed;      //直流电机输出控制
  
}