/**************************************************************************** * 名 称: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