task_followline.c

本人s12的一部分机器人巡线程序

#include "includes.h"
#define EXT extern
#include "main.h"
#include "position.c"
#include "PID.C"


#pragma CODE_SEG Task_FollowLineCode		//save the TaskStart code in banked flash
void Task_FollowLine(TASKDATA* pdata);

uchar FrontPositionT,e_f,e_fL,e_b,e_bL,LeftSpeed,LeftSpeedL,RightSpeed,RightSpeedL,equ_count;

uchar Comm_time; //通讯时间间隔计算
BOOLEAN key_Comm;//通讯命令
BOOLEAN STUTS;

BOOLEAN start1;

void Task_FollowLine(TASKDATA* pdata)   //循线任务
{
	#if OS_CRITICAL_METHOD == 3
	OS_CPU_SR cpu_sr;
	#endif
	INT8U err;
	uchar i=0;
	uint j=0;	
  Follow_Stuts=FALSE;
  Follow_Start=FALSE;        
  equ_count=0;
  Line_a=0;
  key_Comm=Comm_time=FrontPositionT=e_f=e_fL=e_b=e_bL=
  LeftSpeed=LeftSpeedL=RightSpeed=RightSpeedL=equ_count=Line_a=0;
  LeftSpeed=RightSpeed=30;
  start1=TRUE;
  Midspeed=50; 
	for(;;)
	{
        OSTimeDly(1);
	    if(Follow_Start)
    	  {
    	    if(start1==TRUE)
    	      {
              start1=FALSE;
              Motor_Op_En(0xFF);
              OSTimeDlyHMSM(0,0,0,600);//保持0.3s手臂闭合动作
              Motor_OpCo_Stop(0x00);//保持手臂状态
              Motor_Up_En(0xFF);//手臂上升
              OSTimeDlyHMSM(0,0,3,0);//手臂上升时间1s
              Motor_UpDw_Stop(0x00);//保持手臂状态
              OS_ENTER_CRITICAL();//发送主从数据的通信数据时要进入临界段保护发送
              LeftSpeedL=30;
              RightSpeedL=20;
              
              LeftSpeed=52;
              RightSpeed=42;
              Speed_Comm();//发送速度任务放在最后的原因有2:1,数线完成要发的0速度可以及时发送;2,离开临界段有任务调度
              OS_EXIT_CRITICAL();//这里会产生任务调度
              OSTimeDlyHMSM(0,0,4,100);
    	      }
    /********************************************************************************/
          ATD_Position();//AD采集并计算结果
    /********************************************************************************/      
          Calculat_E();//用对称平分误差输出方式计算给定速度
          //Calculate_Speed();//这三个只要选择一个即可
          //PID_Speed(e_f);
          //PID_Speed(e_b);
    /********************************************************************************/  
          //Lines_Counter();//数线,判断是否达到数线要求
    /*******************************************************************************/		              	        
          //#define Cpu_Comm
          #ifdef Cpu_Comm          
          Cpu_Commu();//与电脑通讯显示数据
          #endif
    /*******************************************************************************/            
        j++;
        if(j>=260) //follow line 2s stop
          {
              j=260;
              LeftSpeed=0;
              RightSpeed=0;
          }
           OS_ENTER_CRITICAL();//发送主从数据的通信数据时要进入临界段保护发送
          Speed_Comm();//发送速度任务放在最后的原因有2:1,数线完成要发的0速度可以及时发送;2,离开临界段有任务调度
          OS_EXIT_CRITICAL();//这里会产生任务调度

 /*
          OS_ENTER_CRITICAL();//发送主从数据的通信数据时要进入临界段保护发送
          LeftSpeedL=30;
          RightSpeedL=20;
          
          LeftSpeed=50;
          RightSpeed=40;
          Speed_Comm();//发送速度任务放在最后的原因有2:1,数线完成要发的0速度可以及时发送;2,离开临界段有任务调度
          OS_EXIT_CRITICAL();//这里会产生任务调度
          OSTimeDlyHMSM(0,0,4,900);
          OS_ENTER_CRITICAL();//发送主从数据的通信数据时要进入临界段保护发送
          LeftSpeedL=60;
          RightSpeedL=70;
          
          LeftSpeed=50;
          RightSpeed=50;
          Speed_Comm();//发送速度任务放在最后的原因有2:1,数线完成要发的0速度可以及时发送;2,离开临界段有任务调度
          OS_EXIT_CRITICAL();//这里会产生任务调度

          OS_ENTER_CRITICAL();//发送主从数据的通信数据时要进入临界段保护发送
          LeftSpeedL=60;
          RightSpeedL=70;
          
          LeftSpeed=50;
          RightSpeed=50;
          Speed_Comm();//发送速度任务放在最后的原因有2:1,数线完成要发的0速度可以及时发送;2,离开临界段有任务调度
          OS_EXIT_CRITICAL();//这里会产生任务调度
          OSTimeDlyHMSM(0,0,4,200);
*/          
    /*********************************************************************************/
    	 }
	}
}
#pragma CODE_SEG DEFAULT
 

/*
      Follow_Start=Follow_Strt;//equ TRUE start to Follow Line
      Follow_Num=Follow_Numb;//The Follow Line Number equ your give
      direction=Follow_Dir;//equ 0x08 is forward direction, equ 0x04 is back direction 
      first_start=TRUE;   

*/