北京航空航天大学附录A主程序包含模糊算法.docx
《北京航空航天大学附录A主程序包含模糊算法.docx》由会员分享，可在线阅读，更多相关《北京航空航天大学附录A主程序包含模糊算法.docx（31页珍藏版）》请在冰豆网上搜索。
北京航空航天大学附录A主程序包含模糊算法
//=============================================================================
//File:
MainTask.c
//=============================================================================
/*
**Purpose:
**
**
**Version:
1.0.0
**
**
*/
/*Includingusedmodulesforcompillingprocedure*/
#include
#include
#include
#include"Cpu.h"
/*Includesharedmodules,whichareusedforwholeproject*/
#include"PE_Types.h"
#include"PE_Error.h"
#include"PE_Const.h"
#include"IO_Map.h"
#include"hidef.h"
#include"DataType.h"
#include"state_comm.h"
#include"init.h"
#include"command.h"
#include"globals_fun.h"
#include"sci.h"
#include"eeprom.h"
#include"IIC.h"
#include"Timer.h"
#include"PWM.h"
#include"spi.h"
#include"nRF2401.h"
#include"target.h"
#include"various.h"
#include"lcd.h"
#include"keyboard.h"
#include"lcd.h"
#include"lcdlogo.h"
#defineMAIN_TASK_GLOBALS
#include"MainTask.h"
#include"InterfaceTask.h"
#include"timer.h"
#include"Argument.h"
#include"video.h"
#include"ad.h"
#include"menu.h"
#include"ccd.h"
#include"MainControl.h"
#include"ConfigArgument.h"
voidmain（void）
{
unsignedcharucKey,ucMode;
PE_low_level_init（）;
PLL_Init（）;
GlobalsInit（）;
EEPROM_Init（）;
Argument_Init（）;
SCI0_Communication_Init（）;
TimerModule_Init（）;
Timer0_Init
（1）;
PWM23_45_67_Init（）;
Motor_Init（）;
Various_Init（）;
nRF2401_Init（）;
KeyboardInit（）;//
LCDInitCHN（）;
ucKey=test1;
Video_Init（）;
CCD_Init（）;
AD1_Init（）;
EnableInterrupts;
Delay_MS（100）;
g_ucRunMode=CheckRunMode（）;
Delay_MS（100）;
InterfaceInit（）;
INTERFACE_STANDBY;
for（;;）
{
#if（COMMAND_HANDLE_IN_INTERRUPT==0）
Command_Handle（）;
#endif
StatusMonitor（）;
MainControl（）;//发车
}
}
//=============================================================================
//File:
MainControl.c
//=============================================================================
/*
**Purpose:
**
**
**Version:
1.0.0
**
**
*/
#include
#include
#include"DataType.h"
#include"video.h"
#include"ccd.h"
#defineMAIN_CTRL_GLOBALS
#include"MainControl.h"
#include"CalInformation.h"
#include"SpecialHandle.h"
#include"pid.h"
#include"various.h"
#include"ccd.h"
#include"Argument.h"
#include"pwm.h"
#include"Cpu.h"
#include"IO_Map.h"
#include"init.h"
#include"timer.h"
#include"target.h"
#include"ResultHandle.h"
#include"Keyboard.h"
#include"globals_fun.h"
#include"MainTask.h"
#include"state_comm.h"
#include"command.h"
#include"lcd.h"
#include"BCB_Simulator.h"
#include"target.h"
#include"Uart_Command.h"
#include"nRF2401.h"
#include"ad.h"
#include"various.h"
#include"pid2.h"
externintabs（inti）;
#if（BCB_DEBUG==1）
externintDoEvent（void）;
externintWaitMS（intnTime）;
externvoidDrawVideoLine（void）;
#endif
#if（BCB_DEBUG==0）
voidDebugMode_Init（void）
{
TIMER0_STOP;
MOTOR_SPEED_AD_CONVERSION;//启动AD采集小车速度
MAINCTRL_CLEARSCREEN;
g_ucSampleFlag=SAMPLE_FLAG_CAR;//启动图像采集
g_uiControlCount=0;
g_ucSpecialHandleFlag=FALSE;
PID_Init（）;
}
voidDebugMode（void）
{
unsignedcharucMask=0xff;
MAINCTRL_CLEARSCREEN;
LCDOutTextCHN（LINE2,"调试模式"）;
NRF2401_INTERRUPT_DISABLE;
g_ucSampleFlag=SAMPLE_FLAG_CAR;//启动图像采集
DebugMode_Init（）;
while（g_uc_CCD_FrameFlag==FALSE）;//等待新的一帧开始
g_uc_CCD_FrameFlag=FALSE;
for（;;）
{
StatusMonitor（）;
while（g_uc_CCD_FrameFlag==FALSE）/*Command_Handle（）*/;
g_uc_CCD_FrameFlag=FALSE;
memcpy（g_ucVideoSendBuffer,g_ucCCD_LinePixel,g_ucCCD_SampleLines）;
NRF2401_ISR（）;
Command_Handle（）;
ucMask^=0xff;
if（ucMask）LED1_ON;
elseLED1_OFF;
}
}
#endif
voidMainControl_Init（void）
{
TIMER0_STOP;
MOTOR_SPEED_AD_CONVERSION;//启动AD采集小车速度
MAINCTRL_CLEARSCREEN;
g_ucSampleFlag=SAMPLE_FLAG_CAR;//启动图像采集
g_uiControlCount=0;
g_ucSpecialHandleFlag=FALSE;
PID_Init（）;
#if（BCB_DEBUG==0）
if（g_ucDebugOutput==0）
{
MOTOR_DUTY_LIMIT_ENABLE;
Set_SM_Duty（g_u16StartSpeed）;//设置电机转速
if（g_ucMotorRunMode==0）//设置电机的驱动模式
{
SetMotorRunMode（MOTOR_RUN_MODE_BIPOLAR）;
}
else
{
SetMotorRunMode（MOTOR_RUN_MODE_BREAK_FRONT）;
}
}
Set_DM_Duty（g_u16_DM_CenterDuty）;//舵机设为0角度
g_sc_Angle=g_u16_DM_CenterDuty;
g_ucCarSpeed=0;
Delay_MS（100）;
MOTOR_DUTY_LIMIT_DISABLE;
#endif
}
#if（BCB_DEBUG==0）
BOOLEANMainControlWaitCCD（void）
{
while（g_uc_CCD_FrameFlag==FALSE）//等待一帧采集完成
{
CalInformation（）;//在采集时间间隙中计算赛道信息
}
g_uc_CCD_FrameFlag=FALSE;
returnTRUE;
}
#else
BOOLEANMainControlWaitCCD（void）
{
unsignedcharucBuffer[CCD_SAMPLE_MAX_LINES];
U16SpeedDuty,AngleDuty;
SpeedDuty=（U16）（（long）g_ucCarSpeed*（g_u16_SM_MaxSpeed-g_u16_SM_MinSpeed）/100+g_u16_SM_MinSpeed）;
AngleDuty=AngleToDuty（g_sc_Angle）;
SpeedDuty+=g_u16_SM_DeathDuty;
Debug_Printf（"CarSpeed=%-5dAngle=%-5dAngleDuty=%-5d,SpeedDuty=%-5d",
g_ucCarSpeed,g_sc_Angle,AngleDuty,SpeedDuty）;
if（SendCarControlInformationDuty（AngleDuty,SpeedDuty,0,0,MOTOR_RUN_MODE_BIPOLAR,
ucBuffer,&g_ucCCD_SampleLines）==FALSE）
{
Debug_Printf（"Sendcontrolinformationfail!
"）;
returnFALSE;
}
memcpy（g_ucCCD_LinePixel,ucBuffer,g_ucCCD_SampleLines）;
memset（g_ucCCD_LineStatus,LINE_STATUS_OK,g_ucCCD_SampleLines）;
DrawVideoLine（）;
CalInformation（）;//计算赛道信息
DoEvent（）;
returnTRUE;
}
#endif
/*
---------------------------------------------------------------------
主模糊控制算法
---------------------------------------------------------------------
*/
voidMainControl（void）
{
unsignedcharucMask=0xff;
MainControl_Init（）;
#if（BCB_DEBUG==1）
bRunFlag=TRUE;
Debug_Printf（"RunStart"）;
#endif
MainControlWaitCCD（）;
for（;;）
{
StatusMonitor（）;
if（MainControlWaitCCD（）==FALSE）continue;
g_ucMotorSpeed=GET_MOTOR_SPEED;//获取小车当前实际速度
MOTOR_SPEED_AD_CONVERSION;//启动AD采集小车速度
if（SpecialHandle（）==TRUE）
{
continue;//特殊处理
}
SpeedFuzzyControl（）;//转速控制
AngleFuzzyControl（）;//转向控制
ResultHandle（）;//结果处理
ucMask^=0xff;
if（ucMask）LED1_ON;
elseLED1_OFF;
g_uiControlCount++;
}
}
voidSpeedFuzzyControl（void）
{
if（g_ucTrackType==TRACK_TYPE_STRAIGHT||abs（g_I16_XPosition）{
g_U16_SpeedDuty=g_U16_StraightSpeed;
}
else
{
g_U16_SpeedDuty=（200-abs（g_I16_XPosition））*g_fSpeedRatio+
g_u16_SM_MinSpeed;
if（g_U16_SpeedDuty>g_u16_SM_MaxSpeed）g_U16_SpeedDuty=g_u16_SM_MaxSpeed;
}
}
consttagAngleControlFuncitonAngleControlFunctionTable[]=
{
PID,
PID2,
};
voidAngleFuzzyControl（void）
{
AngleControlFunctionTable[AngleControlFunctionSelect（）]（）;
}
unsignedcharAngleControlFunctionSelect（void）
{
returnANGLE_CONTROL_METHOD_PID;
}
//=============================================================================
//File:
Video.c
//=============================================================================
/*
**Purpose:
**
**
**Version:
1.0.0
**
**
*/
#include"cpu.h"
#include"PE_Types.h"
#include"PE_Error.h"
#include"PE_Const.h"
#include"hidef.h"
#include"Io_Map.h"
#include"DataType.h"
#include"Timer.h"
#include"Init.h"
#defineVIDEO_GLOBALS
#include"video.h"
#include"ccd.h"
#include"various.h"
#include"maintask.h"
#include"globals_fun.h"
#include"lcd.h"
#include"keyboard.h"
#include"target.h"
#include"SearchTable.h"
U8VideoAddress@0x40AA;
voidVideo_Init（void）
{
INTCR=0xc0;//IRQ下降沿触发,打开IRQ中断
PPSH|=（1<PERH&=~（1<DDRH&=~（1<PIFH=（1<PIEH_PIEH0=1;//使能PORTH0中断
DDRH&=~（1<VIDEO_WRITE_RESET_ENABLE;
VIDEO_READ_RESET_ENABLE;
DDRH|=（（1<RDRH&=~（（1<g_uiCurrentVideoLine=0;
g_ucSampleFlag=SAMPLE_FLAG_NONE;
g_ucFilterValue=80;
g_ucSampleIndex=0;
MakeVideoSampleTable（）;
g_uiSampleLine=g_uiVideoSampleTable[0]+VIDEO_LINE_YREF;
g_ucLCDDisplayXREF=VIDEO_LINE_XREF;
g_ucSampleStatus=SAMPLE_STATUS_DISABLE;
}
#defineOVER_CHECK1
#pragmaCODE_SEG__NEAR_SEGNON_BANKED
interruptvoidIRQ_Interrupt（void）
{
unsignedcharucTemp;
#if（OVER_CHECK）
staticunsignedcharucOverFlag=FALSE;
#endif
/*如果不打开中断,并且没有使用UPD42102,就必需保证在本中断函数中处理完所有的事情*/
EnableInterrupts;
if（g_ucSampleFlag!
=SAMPLE_FLAG_NONE）
{
#if（USE_UPD42102==0）
if（g_uiCurrentVideoLine==g_uiSampleLine）{
g_ucSampleFlag=SAMPLE_FLAG_NONE;
SampleVideo2Buffer（）;
g_ucSampleStatus=SAMPLE_STATUS_COMPLETE;
}
#else
if（g_uiCurrentVideoLine==g_uiSampleLine-1）
{
VIDEO_WRITE_RESET_DISABLE;
g_ucSampleStatus=SAMPLE_STATUS_PREPARE;
}
elseif（g_uiCurrentVideoLine==g_uiSampleLine&&
SAMPLE_STATUS_PREPARE==g_ucSampleStatus）
{
VIDEO_WRITE_RESET_ENABLE;
g_ucSampleStatus=SAMPLE_STATUS_PREPARE2;
}
elseif（SAMPLE_STATUS_PREPARE2==g_ucSampleStatus&&
SAMPLE_FLAG_NONE!
=g_ucSampleFlag&&
g_uiCurrentVideoLine==g_uiSampleLine+1）
{
if（g_ucSampleFlag==SAMPLE_FLAG_CAR）
{
g_ucSampleIndex++;
if（g_ucSampleIndex>g_ucVideoSampleTableCount-1）
{
g_ucSampleIndex=0;
}
g_uiSampleLine=g_uiVideoSampleTable[g_ucSampleIndex]+VIDEO_LINE_YREF;
}else{
g_ucSampleFlag=SAMPLE_FLAG_NONE;
}
VIDEO_READ_RESET_ENABLE;
ucTemp=VideoAddress;
VIDEO_READ_RESET_DISABLE;
#if（OVER_CH
升级会员 