autocontrol.c

第一届华中科技大学智能车led源码

//*************************************************************************
//*																								                        *
//*			 *************************智能车控制************************		  *
//*																								                        *
//*************************************************************************
#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;
  signed char sc_SetSpeed;

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  signed int  si_SteerEx;
unsigned int  ui_MotorEx,ui_MotorExLast;

//*************************************************************************
//*																								                        *
//*			 *************************自动控制函数**********************		  *
//*																								                        *
//*************************************************************************
void vAutoControl(void)
{
    signed int siDiffMotorEx,siTemp;
  si_SteerEx=sc_CenterEx*18;
                           //+3*(sc_CenterEx-sc_CenterExLast)
  ui_Direction=STEER_DTY_CENTER-si_SteerEx;
    
  ui_MotorExLast=ui_MotorEx;
  if(sc_CenterEx>0)
    ui_MotorEx=sc_CenterEx;
  else
    ui_MotorEx=-sc_CenterEx;
  ui_MotorEx/=5;
  
  siDiffMotorEx=ui_MotorEx-ui_MotorExLast; 
  
  sc_SetSpeed=12-siDiffMotorEx;
  
  siTemp=sc_SetSpeed-ui_DetectSpeed;
  
  if(siTemp>-1)
  {
    ui_Speed=1930;
  }
  else
  {
    if(siTemp<-7)
      ui_Speed=20;
    else
      ui_Speed=120;
//    if(siTemp<-10)
  }
//  ui_Speed=1000+(sc_SetSpeed-ui_DetectSpeed)*100;
    
//*************************************************************************
//限定舵机和直流电机输出量，保证不超出范围。
  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;      //直流电机输出控制
  
}