task_swerve.c

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

#include "includes.h"
#define EXT extern
#include "main.h"
#pragma CODE_SEG Task_SwerveCode	//save the Task2 code in banked flash
//#pragma STRING_SEG TASKSTRINGSEG	//save the Task2 string in banked flash
void Task_Swerve(TASKDATA* pdata);
uchar Angle_count;//转弯数线定位
void Task_Swerve(TASKDATA* pdata)  //转弯任务
{
	#if OS_CRITICAL_METHOD == 3
	OS_CPU_SR cpu_sr;
	#endif
	INT8U err;
	
	uchar i=0;
	for(;;)
	{		
		OSTimeDlyHMSM(0,0,0,50);
	  if(Key_TaskSwerve_EN)
	    {
              {
                if(Angle_RL==Angl_Ri)
                  {
                    directionR=0x04;
                    directionL=0x08;
                  }
                else
                  {
                    directionR=0x08;
                    directionL=0x04;
                  }
                OS_ENTER_CRITICAL();//这些数据数要同时处理的,不能产生实时中断
                RightSpeed=17;//数到白线的时候使电机速度为0
                RightSpeedL=0;
                LeftSpeed=17;
                LeftSpeedL=0;
                Speed_Comm();
                OS_EXIT_CRITICAL();//这里会产生任务调度                   
              }
        if(angle==Angl_90)//转90度角
          {
              {
                 do    //判断传感器现在是不是有在转弯启动时候在白线上的
                  {
                   OSTimeDly(1);//延时10ms判断一次,这个时间长度应该不会太慢了 
                   ATD0_Get_Rog();
                  }
                 while((AD_bData0[0]>140)||(AD_bData0[1]>140)||(AD_bData0[2]>140)||(AD_bData0[3]>140)
                    ||(AD_bData0[4]>140)||(AD_bData0[5]>140)||(AD_bData0[6]>140));
              }
            Angle_count=1; //转90度弯只要累计一次转弯到线就可以了
            while(Angle_count!=0)
              {
                 OSTimeDly(1);
                 ATD0_Get_Rog();
                 if(Angle_RL==Angl_Le)
                  {
                   if(AD_bData0[3]>140)
                    {
                      OSTimeDly(1);//延时10ms再接测
                      ATD0_Get_Rog();
                      if(AD_bData0[3]>140)Angle_count--;//如果间隔10ms还接测到在白线则任务转弯完成
                    }                   
                  }
                 else if(Angle_RL==Angl_Ri)
                  {
                   if(AD_bData0[3]>140)
                    {
                      OSTimeDly(1);//延时10ms再接测
                      ATD0_Get_Rog();
                      if(AD_bData0[3]>140)Angle_count--;//如果间隔10ms还接测到在白线则任务转弯完成
                    }                   
                  }
              }
          }
        else if(angle==Angl_18)//转180度角
          {
              {
                 do    //判断传感器现在是不是有在转弯启动时候在白线上的
                  {
                   OSTimeDly(1);//延时10ms判断一次,这个时间长度应该不会太慢了 
                   ATD0_Get_Rog();
                  }
                 while((AD_bData0[0]>140)||(AD_bData0[1]>140)||(AD_bData0[2]>140)||(AD_bData0[3]>140)
                    ||(AD_bData0[4]>140)||(AD_bData0[5]>140)||(AD_bData0[6]>140));
              }
            Angle_count=2; //转90度弯只要累计一次转弯到线就可以了
            while(Angle_count!=0)
              {
                 OSTimeDly(1);
                 ATD0_Get_Rog();
                 if(AD_bData0[3]>140)
                  {
                    OSTimeDly(1);//延时10ms再接测
                    ATD0_Get_Rog();
                    if(AD_bData0[3]>140)
                    {
                       Angle_count--;//如果间隔10ms还接测到在白线则任务转弯完成
                       if(Angle_count!=0)
                        {
                       do    //等待转到90度的时候离开白线
                        {
                         OSTimeDly(1);//延时10ms判断一次,这个时间长度应该不会太慢了 
                         ATD0_Get_Rog();
                        }
                       while((AD_bData0[0]>140)||(AD_bData0[1]>140)||(AD_bData0[2]>140)||(AD_bData0[3]>140)
                          ||(AD_bData0[4]>140)||(AD_bData0[5]>140)||(AD_bData0[6]>140));                          
                        }
                    }
                  }
              }
            
          }
        /*
        OS_ENTER_CRITICAL();//这些数据数要同时处理的,不能产生实时中断
        RightSpeedL=25; //防止两次速度相同被通信函数屏蔽
        LeftSpeedL=25;
        RightSpeed=30;//数到白线的时候使电机速度为0
        LeftSpeed=30;
        if(directionL==0x04)directionL=0x08;//改变左电机方向
          else directionL=0x04;
        if(directionR==0x04)directionR=0x08;//改变右电机方向
          else directionR=0x04;
        Speed_Comm();
        OS_EXIT_CRITICAL();//这里会产生任务调度  
        OSTimeDlyHMSM(0,0,0,200);
        */
        OS_ENTER_CRITICAL();//这些数据数要同时处理的,不能产生实时中断
                    if(direction==0x04)direction=0x08;//停止方向
                    else if(direction==0x08)direction=0x04;
                    if(directionL==0x04)directionL=0x08;//改变左电机方向
                    else if(direction==0x08)direction=0x04;
                    if(directionR==0x04)directionR=0x08;
                    else if(directionR=0x08)directionR=0x04;//改变右电机方向
        RightSpeedL=0;//防止再次发送的值大于0
        LeftSpeedL=0;
        RightSpeed=10;//数到白线的时候使电机速度为0
        LeftSpeed=10;
        Speed_Comm();
        OS_EXIT_CRITICAL();//这里会产生任务调度                   
        OSTimeDly(70); 

        RightSpeedL=10;//防止再次发送的值大于0
        LeftSpeedL=10;
        RightSpeed=0;//数到白线的时候使电机速度为0
        LeftSpeed=0;
        Speed_Comm();
               
        Follow_Start=FALSE;//循线任务关闭
	      OSTimeDlyHMSM(0,0,0,100);
        Key_TaskStart_EN=TRUE;//查找地图下一步打开
	      Key_TaskSwerve_EN=FALSE;
	      Key_TaskStart_EN=TRUE;
	    }
	}
}


#pragma CODE_SEG DEFAULT
//#pragma STRING_SEG DEFAULT