/****************************************************************************
* 名 称：Moto_Action_Process
* 功 能：电机根据指令执行动作
* 参 数：
* 反 回：无
* 说 明：
****************************************************************************/
void Moto_Action_Process(CAR_ACTION MotoCmd)
{
switch(MotoCmd)
{
case CAR_ACTION_FORWARD://前进
SetMotoForward();
if(CarData->RunMode == CAR_MODE_MANUAL)
Set_Moto_Speed(CarData->SetSpeed,CarData->SetSpeed);
break;
case CAR_ACTION_BACK://后退
SetMotoBack();
if(CarData->RunMode == CAR_MODE_MANUAL)
Set_Moto_Speed(CarData->SetSpeed,CarData->SetSpeed);
break;
case CAR_ACTION_TURN_LEFT://左转
if(CarData->RunMode == CAR_MODE_MANUAL)//自动模式根据磁传感器的数据自动进行转弯
{
Set_Moto_Speed(CarData->SetSpeed*15/10,CarData->SetSpeed*5/10);
Turn_Left_Count = 0;
if(Turn_Left_Count_Timer_Index != -1)
Free_Timer(&Turn_Left_Count_Timer_Index);
Malloc_Timer(10,Auto_Mode,Turn_Left_Count_Timeout,NULL,&Turn_Left_Count_Timer_Index);
}
break;
case CAR_ACTION_TURN_RIGHT://右转
if(CarData->RunMode == CAR_MODE_MANUAL)//自动模式根据磁传感器的数据自动进行转弯
{
Set_Moto_Speed(CarData->SetSpeed*5/10,CarData->SetSpeed*15/10);
Turn_Right_Count = 0;
if(Turn_Right_Count_Timer_Index != -1)
Free_Timer(&Turn_Right_Count_Timer_Index);
Malloc_Timer(10,Auto_Mode,Turn_Right_Count_Timeout,NULL,&Turn_Right_Count_Timer_Index);
}
break;
case CAR_ACTION_BREAK://刹车
BreakCar();
break;
case CAR_ACTION_STOP: //普通停车
StopCar();
break;
case CAR_ACTION_URGENT_STOP://紧急停车
BreakCar();
StopCar();
break;
default:
//BreakCar();
//StopCar();
break;
}
g_f_TurnFlag = MotoCmd;
}
/* ================= main ====================
desc: 程序入口函数
pre: 无
Post: 无
*/
void main(void) {
// Local Declarations
byte laser_history_array[LASER_HISTORY_MAX]; //激光管历史状态数组
Bool temp_usualRoad_flag = TRUE; //当前判断是否正常路段标志
Bool last_usualRoad_flag = TRUE; //上次判断是否正常路段标志
Bool stopCar_flag = FALSE; //停车标志
Bool breakCar_flag = FALSE; //刹车标志
byte startlineFlag_count = 0; //经过起始线的次数
byte laser_hitNum = 1; //照到黑线的激光管个数
byte inside_count = INSIDE_COUNT_MAX; //激光管连续在黑线范围内的次数
byte outside_count = 0; //激光管连续在黑线外的次数
byte last_error = 0; //直道加速匀速控制的上次误差
LASER_STATUS last_laserStatus = MID7; //上次激光管状态
LASER_STATUS temp_laserStatus = MID7; //当前激光管状态
int last_turnAngle = PWM1_MID; //上次舵机调整的角度
int temp_turnAngle = PWM1_MID; //当前舵机需要调整的角度
int last_speed = PWM3_FAST0; //上次速度
int temp_speed = PWM3_FAST0; //当前速度
int i;
int testcount=0; //发送激光管信息计数值定义
for(i=0;i<LASER_HISTORY_MAX;i++) {
laser_history_array[i] = MID7;
}
// Statements
DisableInterrupts;
MCUInit();
SmartcarInit();
EnableInterrupts;
for(;;) {
if(PITINTE_PINTE0 == 0) { //若PIT0采集中断为关,即道路信息采集完成
laser_hitNum = 15 - CalculateLaserHitNum(g_temp_laser_array);
temp_usualRoad_flag = IsUsualRoad (laser_hitNum); //判断是否为正常道路
if (temp_usualRoad_flag) {
temp_laserStatus = GetLaserStatus(last_laserStatus,g_temp_laser_array,laser_hitNum,&inside_count,&breakCar_flag,laser_history_array,&outside_count); //得到当前激光管状态
temp_turnAngle = CalculateAngle(temp_laserStatus); //得到舵机需要调整的转角
temp_speed = CalculateSpeed (temp_turnAngle,stopCar_flag,inside_count,outside_count); //得到需要输出的速度
}
else {
if((last_usualRoad_flag == TRUE)&&(laser_hitNum>=8&&laser_hitNum<=11)) { //一定执行
startlineFlag_count = CountStartlineFlag(startlineFlag_count,g_temp_laser_array); //计算小车经过起始线的次数
if(startlineFlag_count == 2)
stopCar_flag = TRUE; //若是第二次经过起始线，停车标志置位，即停车
StopCar(stopCar_flag);
}
} /**/
testcount++;
if(testcount%50==0){
testcount=1;
SendSmartcarInfo(g_temp_laser_array,temp_laserStatus,last_laserStatus,g_temp_pulse);//发送激光管信息
} /* */
PITINTE_PINTE0 = 1; //开PIT0采集中断
}
DerectionCtrl(temp_turnAngle); //调整舵机
if(breakCar_flag == TRUE) { //若直道入弯，反转减速刹车
BreakCar(g_temp_pulse, &breakCar_flag);
}
else
SpeedCtrl(temp_speed,g_temp_pulse,&last_error); //调整正转速度
last_speed = temp_speed; //保存当前速度
last_laserStatus = temp_laserStatus; //保存当前激光管状态
last_turnAngle = temp_turnAngle; //保存当前舵机转角
last_usualRoad_flag = temp_usualRoad_flag; //保存当前是否正常道路的标志
for(i=LASER_HISTORY_MAX-1;i>0;i--){ //保存激光管历史状态
laser_history_array[i] = laser_history_array[i-1];
}
laser_history_array[0] = temp_laserStatus;
}
} //main
