2019.04.23 广东韶关 中国工程机器人大赛暨国际公开赛

这个“手到病除”名字起的是土了点。。。。不过这么大的工程比赛还是要记录一下,也因为这个比赛初步了解了 stm32 ,不得不说和C简直没法比,当时画场地就画了有一周,一共前前后后做了有4辆车子,前两辆是毕业杨学长留下来的,哎大佬大佬,stm32的系统集成相较于我们现在所学的语言,更加的系统谨慎,层次感鲜明,很适合用来编写工业上的流水作业工程机器人感觉,要不是亲眼所见我都不会相信一个循迹模块里面可以加上一个51芯片来做灰度运算处理,这次大赛的成绩可以说更多的是站在了巨人的肩膀上。

什么是单片机:
一片半导体硅片集成:中央处理单元(CPU)、存储器(RAM、RAM)GPIO、定时器、中断系统、系统时钟电路及系统总线的微型计算机。
它具有微型计算机的属性,因为被称为单片微型计算机,简称单片机。

什么是STM32
新的给予ARM内核的32位MCU系列
--内核为ARM公司为要求高性能、低成本、低功耗的嵌入式应用专门设计的cortex-M内核
--标准的ARM架构

两者对比: STM32单片机程序都是模块化的,接口相对简单些,因为它自身带好多功能,工作速度也快。而51的自身功能少,需要外围元件多,要求对电子熟悉。

STM32系列基于专为要求高性能、低成本、低功耗的嵌入式应用专门设计的ARM Cortex-M3内核。按性能分成两个不同的系列:STM32F103“增强型”系列和STM32F101“基本型”系列。增强型系列时钟频率达到72MHz,是同类产品中性能最高的产品;基本型时钟频率为36MHz,以16位产品的价格得到比16位产品大幅提升的性能,是16位产品用户的最佳选择。两个系列都内置32K到128K的闪存,不同的是SRAM的最大容量和外设接口的组合。时钟频率72MHz时,从闪存执行代码,STM32功耗36mA,是32位市场上功耗最低的产品,相当于0.5mA/MHz。 全新STM32互连型(ConnecTIvity)系列微控制器增加一个全速USB(OTG)接口,使终端产品在连接另一个USB设备时既可以充当USB主机又可充当USB从机;还增加一个硬件支持IEEE1588精确时间协议(PTP)的以太网接口,用硬件实现这个协议可降低CPU开销,提高实时应用和联网设备同步通信的响应速度。

//比赛相关模块代码
Motor;;;

#include "includes.h"
#include "Motor.h"

/*
    PWM相关控制操作
主要是 模拟舵机 和 直流电机
*/

extern __IO uint16_t MOTOR_COUNT_1,MOTOR_SPEED_1;  //测速计数器  测速速度寄存器
extern __IO uint16_t MOTOR_COUNT_2,MOTOR_SPEED_2;

extern Grayscale Front;
extern Grayscale Back;  //灰度传感器 结构体

extern _pid pid_L;
extern _pid pid_R;

extern  int PID_EN;

//extern VL53L0X_Typedef VL53L0X_struct;

 uint16_t MOTOR_SPEED_L;  //目标速度  左
 uint16_t MOTOR_SPEED_R;  //目标速度  右

 
Tracing_typedef Tracing_struct;  //寻迹数据结构体
 
Analog_Steering_typedef Analog_Steering_1; //模拟舵机结构体1
Analog_Steering_typedef Analog_Steering_2; //模拟舵机结构体2


/*
模拟舵机驱动框架:
 
先在BSP.C文件中调用 “ Analog_Steering_Init();  模拟舵机参数初始化 ”

初始化舵机舵机相关参数  包括管脚参数 PWM定时器参数 

再需要设置舵机角度时  调用 void SET_Analog_Steering_Engine(Analog_Steering_typedef *Analog_Steering_Struct,uint16_t Position,uint16_t delay);
函数进行舵机位置及转动速度设置

调用例子:
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,10);
设置1号模拟舵机 位置1250  转角延时10

*/

void Analog_Steering_Init() //模拟舵机参数初始化
{
  Analog_Steering_1.GPIOx=GPIOC;                //端口
  Analog_Steering_1.GPIO_Pin=GPIO_Pin_7;        //PIN
  Analog_Steering_1.TIMx=TIM3;                  //定时器
  Analog_Steering_1._ucChannel=2;               //定时器通道
  Analog_Steering_1._ulFreq=50;                  //频率
  Analog_Steering_1._ulDutyCycle=1250;          //占空比 也就是位置  
  Analog_Steering_1.Last_Position=1250;
    
  Analog_Steering_2.GPIOx=GPIOC;                //端口
  Analog_Steering_2.GPIO_Pin=GPIO_Pin_9;        //PIN
  Analog_Steering_2.TIMx=TIM3;                  //定时器
  Analog_Steering_2._ucChannel=4;               //定时器通道
  Analog_Steering_2._ulFreq=50;                 //频率
  Analog_Steering_2._ulDutyCycle=1000;          //占空比 也就是位置 
  Analog_Steering_2.Last_Position=1000;
  
  bsp_SetTIMOutPWM(Analog_Steering_1.GPIOx, Analog_Steering_1.GPIO_Pin, Analog_Steering_1.TIMx, Analog_Steering_1._ucChannel,Analog_Steering_1._ulFreq,Analog_Steering_1.Last_Position); //舵机1 

   
  bsp_SetTIMOutPWM(Analog_Steering_2.GPIOx, Analog_Steering_2.GPIO_Pin, Analog_Steering_2.TIMx, Analog_Steering_2._ucChannel,Analog_Steering_2._ulFreq,Analog_Steering_2.Last_Position); //舵机2

  
}


void Motor_IO_Init()  //直流电机IO配置
{
      GPIO_InitTypeDef GPIO_InitStructure;
      
      RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
      GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14; 
      GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
      GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; 
      GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;  //开漏输出
      GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
      GPIO_Init(GPIOC, &GPIO_InitStructure); 

      RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG, ENABLE);
      GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; 
      GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
      GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; 
      GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;  //开漏输出
      GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
      GPIO_Init(GPIOG, &GPIO_InitStructure); 

      GPIO_WriteBit(GPIOC, GPIO_Pin_14, Bit_RESET);  //右侧车轮 C14
      GPIO_WriteBit(GPIOG, GPIO_Pin_10, Bit_SET);   //左侧车轮 G10
}




/*
在main 文件中 初始化了一个软件定时器 10 MS 周期  
  MOTOR_COUNT_TMR_Ballback 即为定时器回调函数 
 
  同时 编码器输出线接入单片机管脚上 轮子转动一圈可产生500个脉冲 
  在脉冲的上升沿 会出发单片机外部中断 在中断服务函数中  ("bsp_exti.c"中)   每中断一次 MOTOR_COUNT++

 每 10 MS 读取一次 MOTOR_COUNT 并置零  一次来定时测量电机转速 

*/

void MOTOR_COUNT_TMR_Ballback(void *p_tmr, void *p_arg)   //电机测速定时器回调函数
{
     MOTOR_SPEED_1=MOTOR_COUNT_1;
     MOTOR_COUNT_1=0; 
     
     MOTOR_SPEED_2=MOTOR_COUNT_2;
     MOTOR_COUNT_2=0;                   
}



void Dc_Motor(uint8_t ON,int SPEED,uint8_t Direction)
{
  if(ON==Motor_L) //左侧电机设置
  {
     MOTOR_SPEED_L=SPEED;
     bsp_SetTIMOutPWM(GPIOB, GPIO_Pin_6, TIM4, 1,1000, MOTOR_SPEED_L);
     bsp_SetTIMOutPWM(GPIOB, GPIO_Pin_7, TIM4, 2,1000, MOTOR_SPEED_L);
    if(Direction==Direction_Q)  GPIO_WriteBit(GPIOG, GPIO_Pin_10, Bit_SET);   //左侧 
      else  GPIO_WriteBit(GPIOG, GPIO_Pin_10, Bit_RESET);   //左侧    
  }
  
   if(ON==Motor_R) //右侧电机设置
  {
      MOTOR_SPEED_R=SPEED; //写入速度值
      bsp_SetTIMOutPWM(GPIOB, GPIO_Pin_8, TIM4, 3,1000, MOTOR_SPEED_R);      
      bsp_SetTIMOutPWM(GPIOB, GPIO_Pin_9, TIM4, 4,1000, MOTOR_SPEED_R);
    
    if(Direction==Direction_Q) //确定电机转向
        GPIO_WriteBit(GPIOC, GPIO_Pin_14, Bit_RESET);  //右侧  
      else
        GPIO_WriteBit(GPIOC, GPIO_Pin_14, Bit_SET);  //右侧    
  }
   
}


/*  

PID 恒定速度调节任务

任务从系统初始化时开始运行

PID_EN标志 一开始写入 0 

任务处于  while(!PID_EN) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );   处等待 

当需要电机调速运行时 可通过 Pid_EN();(定义于"pid.h"中)   结构来置位 PID_EN 等于1   同时 写入  Tracing_struct.Direction=Direction_H 或 Tracing_struct.Direction=Direction_Q 设定 运行方向 
例如:
     Tracing_struct.Direction=Direction_H;
     Pid_EN();  
 当需要结束恒速运行时  只需
    Pid_disEN(); 
即可


*/
void MOTOR_task(void *p_arg)   //PID调速任务
{
   OS_ERR err;
   p_arg = p_arg; 
   
   uint32_t PWM;
   
   PID_EN=0;
   
   while(1)
   {
     while(!PID_EN) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );   //等待PID使能置位
     
     while(PID_EN)  //PID调速
       {
            /*   计算输出速度值   目标速度   实际速度      */
          PWM=(int)PID_realize(33,MOTOR_SPEED_1,&pid_R);    Dc_Motor(Motor_R,PWM,Tracing_struct.Direction);  //右侧电机
      
          PWM=(int)PID_realize(33,MOTOR_SPEED_2,&pid_L);    Dc_Motor(Motor_L,PWM,Tracing_struct.Direction);  //左侧电机
          
          OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
       }
     
     Dc_Motor(Motor_L,0,Direction_H); 
     Dc_Motor(Motor_R,0,Direction_H); //关闭电机
     
  
     
   }
  
}


/*

 最优化寻迹任务

if(向前)
{
  if(前面有白线)
    {
      正常寻迹
    }
   else 跑出去了
    {
        倒车
    }
}

if(向后)
{
        if(后面有白线)  正常寻迹
          {
            
            Angle_error    = (Back.Position - Front.Position)*3.35;  //角度偏差
            Position_error = Back.Position*16;     //位置偏差
            
          if(abs((int)Angle_error)>10)  //偏角过大 原地旋转
          {
            if(Angle_error>0)
            {
               顺时针转一下 
            }
            else 
            {
              逆时针转一下
            }
          }
          else 小偏角  微微修正就好 
            {
              计算左右目标差速
              设置左右差速
            }
        }
     else   没有线  倒车
        {
            先停住
            倒车 直到有线为止
        }
}
if(绕圈寻迹)
{
  计算差值 
  设置差值
}

当想让小车停止寻迹时 运行 OSTaskSuspend(&TracingTaskTCB,&err);  挂起寻迹任务 在关闭左右电机即可
当想让小车重新运行时 先写入运行方向 在重启寻迹任务即可

    Tracing_struct.Direction=Direction_H; //写入方向参数  设定寻迹任务方向
    OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务


*/

void Tracing_task(void *p_arg)   //  寻迹任务
{
   OS_ERR err;
   p_arg = p_arg; 

   float Angle_error=0;
   float Position_error=0;
   float K[2]={1.1773,1.0000};

   float Speed_disparity=0;   
   /*
   最优化路径寻迹
   有关最优矩阵 K
   请参阅  “ 吉林大学  基于机器视觉引导的两轮差速转向AGV控制问题的研究 ”
   */
   
   int speed;  //实际
   
     
  OSTimeDly ( 100,OS_OPT_TIME_DLY, & err );
  
   while(1)
   {
     OSTimeDly ( 2,OS_OPT_TIME_DLY, & err );

     /* ***************  向前寻迹    ****************** */
     if(Tracing_struct.Direction==Direction_Q)
     {
        if(Front.SUM>0) //前置传感器 能检测到白线的传感器数量
          {
          /*   灰度检测参数递交  偏角计算  */
          Angle_error    = (Front.Position - Back.Position)*3.35;  //角度偏差
          Position_error = Front.Position*16;     //位置偏差
     
          Speed_disparity=Angle_error*K[0]+Position_error*K[1]; //理论速度差值
          speed=(int)Speed_disparity*60;         //差速值放大 适应小车实际实际情况
          Dc_Motor(Motor_L,1800+speed,Direction_Q);
          Dc_Motor(Motor_R,1800-speed,Direction_Q);
        }
     else 
        {
          Dc_Motor(Motor_L,0,Direction_H);
          Dc_Motor(Motor_R,0,Direction_H);  //先刹车
     
          OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );

          Dc_Motor(Motor_L,5000,Direction_H);
          Dc_Motor(Motor_R,5000,Direction_H);
          while(Front.SUM==0) OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
        }
     }
     /* ***************  向后寻迹    ****************** */
     if(Tracing_struct.Direction==Direction_H)
     {
        if(Back.SUM>0) //后置传感器 能检测到白线的传感器数量
          {
            /*   灰度检测参数递交  偏角计算   */
            Angle_error    = (Back.Position - Front.Position)*3.35;  //角度偏差
            Position_error = Back.Position*16;     //位置偏差
            
          if(abs((int)Angle_error)>10)  //偏角过大 原地旋转
          {
            if(Angle_error>0)
            {
              Dc_Motor(Motor_L,9900,Direction_H);
              Dc_Motor(Motor_R,9900,Direction_Q); 
            }
            else 
            {
              Dc_Motor(Motor_L,9900,Direction_Q);
              Dc_Motor(Motor_R,9900,Direction_H); 
            }
          }
          else 
          {
            Speed_disparity=Angle_error*K[0]+Position_error*K[1]; //理论速度差值
            speed=(int)Speed_disparity*60;         //差速值放大 适应小车实际实际情况
            Dc_Motor(Motor_L,7000+speed,Direction_H);
            Dc_Motor(Motor_R,7000-speed,Direction_H);
          }
        }
     else 
        {
          Dc_Motor(Motor_L,0,Direction_Q);
          Dc_Motor(Motor_R,0,Direction_Q);  //先刹车
     
          OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );

          Dc_Motor(Motor_L,5000,Direction_Q);
          Dc_Motor(Motor_R,5000,Direction_Q);
          while(Back.SUM==0) OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
        }
     } 
       /* ***************  绕圈寻迹    ****************** */
    if((Tracing_struct.Direction==Direction_R))   
      {              /*   灰度检测参数递交  偏角计算   */
            Angle_error    = (Back.Position - Front.Position)*3.35;  //角度偏差
            Position_error = Back.Position*16;     //位置偏差
            Speed_disparity=Angle_error*K[0]+Position_error*K[1]; //理论速度差值
            speed=(int)Speed_disparity*50;         //差速值放大 适应小车实际实际情况
            
            Dc_Motor(Motor_L,0,Direction_H);
            Dc_Motor(Motor_R,7400-speed,Direction_H);
      }
   }
}

/*
void Analog_Steering_Engine(uint8_t ON,uint16_t Position)
模拟舵机位置设定
ON 舵机编号
Position

此函数基本不用 用下面那个

*/
void Analog_Steering_Engine(uint8_t ON,uint16_t Position)
{
  if(ON==1) bsp_SetTIMOutPWM(GPIOC, GPIO_Pin_7, TIM3, 2,50, Position); //舵机1 
  if(ON==2) bsp_SetTIMOutPWM(GPIOC, GPIO_Pin_9, TIM3, 4,50, Position); //舵机2  
}

/*

函数名:void SET_Analog_Steering_Engine(Analog_Steering_typedef *Analog_Steering_Struct,uint16_t Position,uint16_t delay)

功能:模拟舵机慢速转动到目标位置

入口参数:

*       Analog_Steering_typedef *Analog_Steering_Struct:  接入舵机编号数据结构体  用于存储模拟舵机上一位置 GPIO口等
*       uint16_t Position : 舵机目标位置
*       uint16_t delay  转动过程中的延时 以达到减速的目的 

if(SPEED==0) 不进行慢速转动  则直接给定占空比后返回

else 

 从上一位置 逐渐往目标位置转动 每转动一小点位置 就延时一小段时间 在转动 再延时 以达到延时的作用  

调用例子:
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,10);
设置1号模拟舵机 位置1250  转角延时10ms 延时越大 转的越慢

*/

void SET_Analog_Steering_Engine(Analog_Steering_typedef *Analog_Steering_Struct,uint16_t Position,uint16_t delay)
{
     OS_ERR err;
 
  if(delay==0) //不延时  直接设置完就走
  {
    bsp_SetTIMOutPWM(Analog_Steering_Struct->GPIOx, Analog_Steering_Struct->GPIO_Pin, Analog_Steering_Struct->TIMx, Analog_Steering_Struct->_ucChannel,Analog_Steering_Struct->_ulFreq,Position);
    Analog_Steering_Struct->Last_Position=Position;  //保存位置
    return ;
  }
     
   if( Analog_Steering_Struct->Last_Position > Position)  //上一位置比当前位置大 递减的方式
     for(uint16_t i=Analog_Steering_Struct->Last_Position;i>=Position;i-=5) 
     {  /* bsp_SetTIMOutPWM() 占空比设置函数  GPIOx 端口号            GPIO_Pin                  TIMx  定时器x                   _ucChannel   定时器通道                _ulFreq 频率          i 占空比    */
       bsp_SetTIMOutPWM(Analog_Steering_Struct->GPIOx, Analog_Steering_Struct->GPIO_Pin, Analog_Steering_Struct->TIMx, Analog_Steering_Struct->_ucChannel,Analog_Steering_Struct->_ulFreq,    i);  //设置位置
       OSTimeDly (delay,OS_OPT_TIME_DLY, & err); //小间隔延时
     }
   else  //上一位置比当前位置小 递增的方式
     for(uint16_t i=Analog_Steering_Struct->Last_Position;i<=Position;i+=5)
     {
       bsp_SetTIMOutPWM(Analog_Steering_Struct->GPIOx, Analog_Steering_Struct->GPIO_Pin, Analog_Steering_Struct->TIMx, Analog_Steering_Struct->_ucChannel,Analog_Steering_Struct->_ulFreq,i); //设置位置
       OSTimeDly (delay,OS_OPT_TIME_DLY, & err); //小间隔延时
     }
  Analog_Steering_Struct->Last_Position=Position; //保存位置
  
}


下面是代码Operation.c部分,不知道为什么我们训练的时候最好一块的转圈循迹总是出错,把循迹删掉盲走就可以???不知道为啥,先备注出来。

Operation.c
#include "includes.h"
#include "Maisshk.h"

#include "Operation.h"

int BCDE=1;

extern OS_TCB TracingTaskTCB;  //寻迹任务控制块 

extern Grayscale Front;
extern Grayscale Back;      //灰度传感器 结构体 
extern Grayscale Middle; 

extern Uranus_typedef Uranus_Structure;  //姿态数据

extern VL53L0X_Typedef VL53L0X_struct;  //测距数据

extern Tracing_typedef Tracing_struct; //寻迹参数

extern Analog_Steering_typedef Analog_Steering_1; //模拟舵机结构体1
extern Analog_Steering_typedef Analog_Steering_2; //模拟舵机结构体2

extern int8_t L1,L2;
extern int8_t R1,R2;  //中置传感器 标记

extern int PID_flag;

extern int extern_speed;

extern int mode_flag;//光电触发标志位
extern unsigned int flag_gd;
void Main_Operation_task(void *p_arg)  //主流程任务
{
	OS_ERR err;
	p_arg = p_arg;

#if 1
       while(BCDE) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); 

       BCDE=1;

       

        extern_speed = 8000;//向后循迹的速度变量
        beging();
        
        along_Stop(Direction_H,3);
        
        extern_speed = 7000;//向后循迹的速度变量
        Turn_Left();
        
        
        along_Stop(Direction_H,2);  
      
        Turn_Right();
  
        along_Stop(Direction_H,1); 
        
        Turn_Left();
       
        Tack_1(); //抓取第一个
      
       along_Stop(Direction_H,1); 

        Turn_Left();

       along_Stop(Direction_H,1);

        Turn_Left();

        along_Stop(Direction_H,2);

        Turn_Right();

        along_Stop(Direction_H,1);
  
        Turn_Left();
       
       
        Put_1();  //放置第一个
   
        Turn_Left();
       
        along_Stop(Direction_H,1);
        
        Turn_Left();

       along_Stop(Direction_H,1);
        
       Turn_Left();
        
       Take_2(); //抓第二个
        
        Turn_Left();
        
        along_Stop(Direction_H,1); 
        
       Turn_Right();
        
       along_Stop(Direction_H,1);
        
        Turn_Right();
        
        Put_2();  //放第二个
     
        Turn_Right();
        
        along_Stop(Direction_H,1);
       
        Turn_Right();
       
   
     
        along_Stop(Direction_H,1);
        
        Turn_Right();
       
        Take_2();  //抓取第三个
        
        Turn_Right();
        
        along_Stop(Direction_H,1);
        
        Turn_Left();
       
    
        along_Stop(Direction_H,2);
        
       Turn_Left();
      
       along_Stop(Direction_H,1);
        
        Turn_Left();
    
        Put_3();  //放下第三个 
       
        Turn_Left();
        
       along_Stop(Direction_H,1);
       
        Turn_Right();
        
        along_Stop(Direction_H,2); 
        
        Turn_Right();
      
        along_Stop(Direction_H,2); 
        
        Turn_Left();
        
        along_Stop(Direction_H,1);
       
        Turn_Right();
        
       Tack_4(); // 抓取第四个
       
        along_Stop(Direction_H,1);
      
         Turn_Right();
        
        along_Stop(Direction_H,1);
        
        Turn_Right();
         
         along_Stop(Direction_H,2);
         
         Turn_Left();
         
  //tiao: 
    
       along_Stop(Direction_H,2);
      
      Turn_Right();
    
       along_Stop(Direction_H,1);
    
       Turn_Right();
    
        Put_4();
   
           
if(mode_flag==1)
{
  //原来的内容,赛道是连接起来的  
  OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
  Turn_Left(); 
  along_Stop(Direction_H,1);
  //OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
  Turn_Left();
//自己写的实验室调试内容,赛道是断开的
//  C//等待五秒,移动车辆 
//  along_Stop(Direction_H,1); //向前寻迹
//  Turn_Left();  
}

if(mode_flag==2)
{
//原来赛道用的内容 
 Turn_Right(); 
 along_Stop(Direction_H,3);
 Turn_Left();

  //分段测试的开始阶段
// OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );//等待一秒,移动车辆 
// along_Stop(Direction_H,1); //向前寻迹
// Turn_Left(); 
}  
  
  
  
#if 1   /*    宏编译开关    */
 
   SET_Dynamixel_Position(AX_12_NO1 ,403);
   SET_Dynamixel_Position(AX_12_NO2 ,606);
   
   SET_Analog_Steering_Engine(&Analog_Steering_1,750,10);
   SET_Analog_Steering_Engine(&Analog_Steering_2,730,10);
   /////////////////////////////////////////////////////////////////////
 //along_Stop(Direction_Q,1); //向前寻迹                              ////
 //                                                                    //// 
//  Dc_Motor(Motor_L,2000,Direction_Q);////////////////////////////////////
//  Dc_Motor(Motor_R,2000,Direction_Q);////////////////////////////////////
//   OSTimeDly ( 2000, OS_OPT_TIME_DLY, & err );//往前跑一点点,自己写的///
///////////////////////////////////////////////////////////////////////////
  
   //套路开始
   
   //倒车校正方向
   
  Tracing_struct.Direction=Direction_H; //写入方向参数  设定寻迹任务方向,倒车校正方向
  OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
  OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );//延时走路
  OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
  OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
  //Car_Stop();
  //OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );//停车
   along_Stop(Direction_Q,1);//循线来到夹取物前
   
   Dc_Motor(Motor_L,1500,Direction_Q);
   Dc_Motor(Motor_R,1500,Direction_Q);
   OSTimeDly ( 3200, OS_OPT_TIME_DLY, & err );
  /////////////////////////////////////////////////////////////////////////////////////////记得调整循迹速度
  //往前走准备抓取
   if(mode_flag == 1)
   
  Tracing_struct.Direction=Direction_Q; //写入方向参数  设定寻迹任务方向,倒车校正方向
  OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
  if(mode_flag == 1)
  {
  OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );//延时走路
  }
  else if(mode_flag==2)
  {
  OSTimeDly ( 120, OS_OPT_TIME_DLY, & err );//延时走路
  }
  OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
   
   Car_Stop();
   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );//停车
   
   SET_Analog_Steering_Engine(&Analog_Steering_2,1020,10); //夹住瓶子
   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
   
   SET_Dynamixel_Position(AX_12_NO1 ,341);
   SET_Dynamixel_Position(AX_12_NO2 ,820); 
   OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );//抬高一点
   OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
   
   along_Stop(Direction_Q,1);//后改为Q
   
   
   
   //along_Stop(Direction_H,2);///////////////////////////////////////////////////////////
   
   SET_Dynamixel_Position(AX_12_NO1 ,800); //爪子举高
   SET_Dynamixel_Position(AX_12_NO2 ,820);
   OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
   
  // along_Stop(Direction_Q,1);//注释了
   
   
   //  while(GET_Dynamixel_Position(AX_12_NO1)<800) OSTimeDly ( 20, OS_OPT_TIME_DLY, & err ); //等待举起

   
   Dc_Motor(Motor_L,0,Direction_Q);
   Dc_Motor(Motor_R,0,Direction_Q);
   //停车
   
   OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
   Turn_Left(); 
   
 ////这里是原本打算校正一下,看来不用了,但防止会用到,所以不删,保留一下 
  
   //  while(!(R2&&L2))//碰到线是1,没碰到线是0,只要有没碰到线的就进入
  // {
//   
//   
//   
//    while(R1||L1)
//                              //右侧压线左侧未压线即左偏进入
//     {
//       
//      if(R1&&!L1)
//       {
//         Car_Stop(); 
//         OSTimeDly ( 100, OS_OPT_TIME_DLY, & err ); 
//         Dc_Motor(Motor_L,9500,Direction_Q);//Dc_Motor(Motor_R,4500,Direction_H);//Q是往前,以爪子为准,往右校正一下
//         OSTimeDly ( 50, OS_OPT_TIME_DLY, & err );
//         Dc_Motor(Motor_R,9500,Direction_Q);
//         OSTimeDly ( 50, OS_OPT_TIME_DLY, & err );
//         Car_Stop();OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
//       }
//     else if(!R1&&L1)
//       {
//         Car_Stop();
//         OSTimeDly ( 100, OS_OPT_TIME_DLY, & err ); 
//         Dc_Motor(Motor_L,9500,Direction_Q);//Dc_Motor(Motor_R,7500,Direction_Q);//往左校正一下
//         OSTimeDly ( 50, OS_OPT_TIME_DLY, & err );
//         Dc_Motor(Motor_R,9500,Direction_Q);
//            OSTimeDly ( 50, OS_OPT_TIME_DLY, & err );
//         Car_Stop();OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
//       }
//     else  
//      {
//        Car_Stop();
//        OSTimeDly ( 100, OS_OPT_TIME_DLY, & err ); 
////        Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);
////        OSTimeDly ( 250, OS_OPT_TIME_DLY, & err ); 
//       break;
//       }
//     };
//    
//     if((!R1)&&(!L1))//两个都是0即两个都不在线上就出发
//     {
//      Dc_Motor(Motor_L,2500,Direction_Q);Dc_Motor(Motor_R,2500,Direction_Q);
//      OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//      } ;
//     
//   
      // Car_Stop();
  
       while(!(R2&&L2))//碰到线是1,没碰到线是0,只要有没碰到线的就进入
     {
       if(R2&&!L2)//右侧压线左侧未压线即左偏进入
       {
          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
          Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前,以爪子为准,往右校正一下
          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
        }
          if(L2&&!R2)//判断是否右偏
        {
          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
          Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
        }
        if(!(L2||R2))//两个都是0即两个都不在线上就出发
        {Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
        OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
        } 
   Car_Stop();
      
   Dc_Motor(Motor_L,3000,Direction_Q);
   Dc_Motor(Motor_R,3000,Direction_Q);  //向前行进 测距 
   
   while(VL53L0X_struct.distance>90) OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );

   Car_Stop();
   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
   
   SET_Dynamixel_Position(AX_12_NO1 ,603);
   SET_Dynamixel_Position(AX_12_NO2 ,160);  //向前放下
   

   
 
   
   OSTimeDly ( 1500, OS_OPT_TIME_DLY, & err );
   
   SET_Analog_Steering_Engine(&Analog_Steering_2,730,50); //放下瓶子
   
   OSTimeDly ( 1500, OS_OPT_TIME_DLY, & err );
   
  
 
  // Dc_Motor(Motor_L,1000,Direction_H);
   //Dc_Motor(Motor_R,1000,Direction_H); //往回倒
   //OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
   SET_Dynamixel_Position(AX_12_NO1 ,635);  //2号舵机微微同时往上抬
   
   along_Stop(Direction_H,1);
   OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
    while(Back.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
    while(Back.SUM>3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
    BEEP_Function(300);
    
    Turn_Left();  
    
    Dc_Motor(Motor_L,3000,Direction_H);
    Dc_Motor(Motor_R,3000,Direction_H);  
    OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); //再往前走一段
 
    Dc_Motor(Motor_L,0,Direction_Q);
    Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机 
  
    OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); 
   
   SET_Dynamixel_Position(AX_12_NO1 ,160);
   SET_Dynamixel_Position(AX_12_NO2 ,500);  //两台舵机放至最低

   SET_Analog_Steering_Engine(&Analog_Steering_2,1020,10); //爪子成夹起状态
   
    //Turn_Left();    
  
    along_Stop(Direction_Q,1); 
    
    
    
    along_Stop(Direction_H,2);   //现场只有两条白线 到时候要去掉一条
    
    Dc_Motor(Motor_L,3000,Direction_H);
    Dc_Motor(Motor_R,3000,Direction_H);  
    OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); //再往前走一段
    
    Car_Stop();
   
    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); 
    
    Turn_Left();
    
    along_Stop(Direction_R,2);    /* 绕圈  */
    
   // Dc_Motor(Motor_L,4500,Direction_Q);
   // Dc_Motor(Motor_R,7000,Direction_Q);  
    Dc_Motor(Motor_L,3500,Direction_H);
    Dc_Motor(Motor_R,7000,Direction_H);  
    OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); //再往前走一段
    
    Car_Stop();
   
    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
    
    Turn_Right();
    
    extern_speed=5000;  //设定撞击速度

    Tracing_struct.Direction=Direction_H; //写入方向参数  设定寻迹任务方向
    
    OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
    
    while(flag_gd==0) OSTimeDly ( 5, OS_OPT_TIME_DLY, & err );
    
    OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
    OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );//////////////////////////第一次撞击延时,待改动
    Car_Stop();
    
    extern_speed=7000;    //还原寻迹速度
    
    OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
    Dc_Motor(Motor_L,2000,Direction_Q);
    Dc_Motor(Motor_R,2000,Direction_Q);
    //along_Stop(Direction_Q,1);
    while(Front.SUM<=4) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
    Dc_Motor(Motor_L,3000,Direction_Q);
    Dc_Motor(Motor_R,3000,Direction_Q);  
    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往前走一段
    Car_Stop();
    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); 
    Turn_Left();
    
    along_Stop(Direction_R,1);      /* 绕圈 */
    
   Dc_Motor(Motor_L,0,Direction_H);
   Dc_Motor(Motor_R,7000,Direction_H);  
    OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); //再往前走一段
    
   Car_Stop();
   
   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); 
    
    Turn_Left();
    
    along_Stop(Direction_H,2);

    
        /*   过坎程序段   */
    
     extern_speed=4000;  //设定撞击速度

    Tracing_struct.Direction=Direction_H; //写入方向参数  设定寻迹任务方向
    
    OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
    
    while(Back.SUM<=3) OSTimeDly ( 20, OS_OPT_TIME_DLY, & err );
    
    OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
    
    Car_Stop();
    
    extern_speed=5000;    //还原寻迹速度   
    
    
     Tracing_struct.Direction=Direction_H;
     Pid_EN();  //启动PID调速
     
 //    while(Back.SUM<=3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
     
     while(Front.SUM<=3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
       
    
    Pid_disEN();  //关闭PID调速
    
    Tracing_struct.Direction=Direction_H; //写入方向参数  设定寻迹任务方向
  
    OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
    
    while(flag_gd==0)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
    
    OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
    OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );/////////////////////////第二次撞击延时,待改动
    
    Car_Stop();//关闭电机  碰撞顶端柱子
    
    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
    
     /*     往回倒车一段    */   
    
    Tracing_struct.Direction=Direction_Q; //写入方向参数  设定寻迹任务方向
  
    OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
    
    while(Front.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );   //等待前置传感器遇到障碍
    
    OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务  
    
     Tracing_struct.Direction=Direction_Q;
       Pid_EN();                        //启动PID调速
     
      /*   第二次上坎   */ 
     
     while(Back.SUM<=3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
    
    Pid_disEN();                       //关闭PID调速
     
   //过坎结束
    
   along_Stop(Direction_Q,1);
    
    
   Car_Stop();
   
   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
   
   
   if(mode_flag==1)
   {
      Turn_Left();
      
      along_Stop(Direction_H,2);
      
       Turn_Right();
      
   }
   
   if(mode_flag==2)
   {
      Turn_Left();
      
      along_Stop(Direction_H,2);
      
      Turn_Left();
      
   }   
   extern_speed = 8000;//向后循迹的速度变量
   along_Stop(Direction_H,1);
   
   Dc_Motor(Motor_L,3000,Direction_H);
   Dc_Motor(Motor_R,3000,Direction_H); 
   
   while(Front.SUM)  OSTimeDly( 10, OS_OPT_TIME_DLY, & err );
   
   
   OSTimeDly( 1000, OS_OPT_TIME_DLY, & err );
   
   Car_Stop();
   
   for(int i=5;i>0;i--)
   {
      BEEP_Function(300); 
      OSTimeDly( 500, OS_OPT_TIME_DLY, & err );
      
   }
   
   
#endif       
     
   
#endif
       while(1)  OSTimeDly( 10, OS_OPT_TIME_DLY, & err );          


}

void Car_Stop()
{

   Dc_Motor(Motor_L,0,Direction_Q);
   Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
}


void beging()
{
  OS_ERR err;
  Dc_Motor(Motor_L,5000,Direction_H);
  Dc_Motor(Motor_R,5000,Direction_H); //倒车   
  while(Back.SUM<3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );   //走空地 
  while(Back.SUM>=3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );   //走横线
  BEEP_Function(300);
}

/*
void along_Stop(uint8_t Direction,uint8_t Number) 
功能: 让现车走过几条线后停止下来
Direction: 方向 Direction_Q 向前  Direction_H 向后
Number   : 要走过线的数量
*/
int LR=1;
void along_Stop(uint8_t Direction,uint8_t Number) 
{
  OS_ERR err;
  
   Tracing_struct.Direction=Direction; //写入方向参数  设定寻迹任务方向
  
  OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
  
 while(Number--)
  {
    if(Direction==Direction_H)  //倒着走
        {
          while(Back.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
          while(Back.SUM>3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
          BEEP_Function(300);
        }
    
    if(Direction==Direction_R)  //倒着绕圈走
        {
          if(LR)           //LR=1时,为前半圈,LR=0时,为后半圈
          {
          while(!L2)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
          while(L2)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
          BEEP_Function(300);
          LR=0;
          }
          else
          {
          while(!R2)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
          while(R2)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
          BEEP_Function(300);
          LR=1;
          }

        }
    
     if(Direction==Direction_Q) //正着走
        {
          while(Front.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
          while(Front.SUM>3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
          BEEP_Function(300);
        }
   }

 /*
  前/后 寻迹是结束上面的循环之后 先关闭寻迹任务 再往后倒一段
 
  绕圈 是结束上面的循环之后 再往后寻迹一段 再关闭寻迹任务
 
 */
 
  if(Direction==Direction_H)  //倒着走 
  {
    OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
    
    Dc_Motor(Motor_L,7000,Direction_H);
    Dc_Motor(Motor_R,7000,Direction_H);  
    OSTimeDly ( 240, OS_OPT_TIME_DLY, & err ); //再往前走一段
  }
  
  if(Direction==Direction_R)  //倒着绕圈走 
  { 
   // OSTimeDly ( 320, OS_OPT_TIME_DLY, & err ); //再往前走一段
    
    OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
    
  } 
  
  if(Direction==Direction_Q)  //正着走 
  {
    OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
    
    Dc_Motor(Motor_L,3000,Direction_Q);
    Dc_Motor(Motor_R,3000,Direction_Q);  
    OSTimeDly ( 230, OS_OPT_TIME_DLY, & err ); //再往前走一段
  }
      Dc_Motor(Motor_L,0,Direction_Q);
      Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机 
  
  OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); 
  
}




void Turn_Right()  //倒车右转其实就是顺时针转动九十度
{
  OS_ERR err;
  
  int16_t Yaw=0;  //航向缓存
  
  Yaw=Uranus_Structure.Yaw/10; //当前航向
  
  Yaw=(360+(Yaw+82))%360; //计算目标航向
  
  printf("当前航向%d 目标航向%d",Uranus_Structure.Yaw/10,Yaw);
    
  Dc_Motor(Motor_L,8000,Direction_Q);
  Dc_Motor(Motor_R,8000,Direction_H);
  
  while(1) 
  { 
    if(Yaw== (Uranus_Structure.Yaw/10))   break;
    if(Yaw== (Uranus_Structure.Yaw/10+1)) break; 
    if(Yaw== (Uranus_Structure.Yaw/10+2)) break;   
    OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); 
  }
  
   Dc_Motor(Motor_L,0,Direction_Q);
   Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
   
   OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );  
  
}


void Turn_Left() //倒车左转
{
  OS_ERR err;
  
  int16_t Yaw=0;  //航向缓存
  
  Yaw=Uranus_Structure.Yaw/10; //当前航向
  
  Yaw=(360+(Yaw-82))%360; //计算目标航向
  
  printf("当前航向%d 目标航向%d\r\n",Uranus_Structure.Yaw/10,Yaw);
    
  Dc_Motor(Motor_L,8000,Direction_H);
  Dc_Motor(Motor_R,8000,Direction_Q);
  
  while(1) 
  { 
    if(Yaw== (Uranus_Structure.Yaw/10))   break;
    if(Yaw== (Uranus_Structure.Yaw/10+1)) break; 
    if(Yaw== (Uranus_Structure.Yaw/10+2)) break;   
    OSTimeDly ( 1, OS_OPT_TIME_DLY, & err ); 
  }
  
   Dc_Motor(Motor_L,0,Direction_Q);
   Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
   
   OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
  
}


void Tack_1()
{
  OS_ERR err;
  
   SET_Dynamixel_SPEED(AX_12_NO1 ,60);
   SET_Dynamixel_SPEED(AX_12_NO2 ,60);
        
   SET_Dynamixel_Position(AX_12_NO2 ,820);
        SET_Dynamixel_Position(AX_12_NO1 ,160);
      OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );  
   SET_Analog_Steering_Engine(&Analog_Steering_1,1250,10); //1号舵机竖直
   
   SET_Analog_Steering_Engine(&Analog_Steering_2,730,10);
        
                                                    
    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //往后倒一点
    
   Tracing_struct.Direction=Direction_Q;
   OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
        
   while(VL53L0X_struct.distance>120) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
        
   OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
   Dc_Motor(Motor_L,0,Direction_Q);
   Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
        
    BEEP_Function(300);
    
///////////////* 向前到位 准备抓取  */    
    
     SET_Analog_Steering_Engine(&Analog_Steering_2,1020,0); //模拟舵机夹紧

    OSTimeDly (400, OS_OPT_TIME_DLY, & err );  //等待延时
        
    SET_Dynamixel_Position(AX_12_NO1 ,360);  //1号舵机往上微微举起
    //   while(GET_Dynamixel_Position(AX_12_NO1)<250)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );

     OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err ); //往后倒一点
    
     SET_Analog_Steering_Engine(&Analog_Steering_1,750,10); //模拟舵机1旋转 

    Dc_Motor(Motor_L,4000,Direction_H);
    Dc_Motor(Motor_R,4000,Direction_H);  
    OSTimeDly ( 250, OS_OPT_TIME_DLY, & err ); //往后倒一点

    SET_Dynamixel_Position(AX_12_NO1 ,160);
    SET_Dynamixel_Position(AX_12_NO2 ,520);  
  
}

//等待调试
void Tack_4()
{
  OS_ERR err;
   SET_Dynamixel_SPEED(AX_12_NO1 ,40);
   SET_Dynamixel_SPEED(AX_12_NO2 ,40);
   SET_Dynamixel_Position(AX_12_NO1 ,761);
   SET_Dynamixel_Position(AX_12_NO2 ,823);
        
   SET_Analog_Steering_Engine(&Analog_Steering_1,1250,10); //1号舵机竖直
   SET_Analog_Steering_Engine(&Analog_Steering_2,730,10);
        
   OSTimeDly (2500, OS_OPT_TIME_DLY, & err );
        
   Tracing_struct.Direction=Direction_Q;
   OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
        
   while(VL53L0X_struct.distance>190) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );//距离到150为止都是循迹的,小于150之后盲走一小段
   //OSTimeDly (1000, OS_OPT_TIME_DLY, & err );    
   OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
   OSTimeDly (10, OS_OPT_TIME_DLY, & err );
   
   Dc_Motor(Motor_L,2000,Direction_Q);
   Dc_Motor(Motor_R,2000,Direction_Q);
    OSTimeDly (800, OS_OPT_TIME_DLY, & err );
   Dc_Motor(Motor_L,0,Direction_Q);
   Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
        
    BEEP_Function(300);
    
    /* 向前到位 准备抓取  */    
    SET_Dynamixel_Position(AX_12_NO1 ,506);
     SET_Dynamixel_Position(AX_12_NO2 ,160);
      OSTimeDly (500, OS_OPT_TIME_DLY, & err ); 
     
     SET_Dynamixel_Position(AX_12_NO1 ,548);
     OSTimeDly (2500, OS_OPT_TIME_DLY, & err );
      
     SET_Analog_Steering_Engine(&Analog_Steering_2,1020,0); //模拟舵机夹紧

     OSTimeDly (500, OS_OPT_TIME_DLY, & err );  //等待延时

     OSTimeDly (500, OS_OPT_TIME_DLY, & err ); 
     SET_Analog_Steering_Engine(&Analog_Steering_1,750,10); //模拟舵机1旋转 
    OSTimeDly (500, OS_OPT_TIME_DLY, & err ); 
      SET_Dynamixel_Position(AX_12_NO1 ,524);
     OSTimeDly (500, OS_OPT_TIME_DLY, & err ); 
     SET_Dynamixel_Position(AX_12_NO2 ,302); 
    OSTimeDly (500, OS_OPT_TIME_DLY, & err ); 
    
//    SET_Dynamixel_Position(AX_12_NO1 ,524);
//    SET_Dynamixel_Position(AX_12_NO2 ,162); 
//    OSTimeDly (500, OS_OPT_TIME_DLY, & err );
//    
//    along_Stop(Direction_H,1);
//    OSTimeDly (500, OS_OPT_TIME_DLY, & err ); 
//    SET_Dynamixel_Position(AX_12_NO1 ,160);
//    SET_Dynamixel_Position(AX_12_NO2 ,520);
//    
//   OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );  
  
}


void Put_1()
{
   OS_ERR err;
   
    while(!(R2&&L2))//碰到线是1,没碰到线是0,只要有没碰到线的就进入
   {
     if(R2&&!L2)//右侧压线左侧未压线即左偏进入
     {
       Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
       Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前,以爪子为准,往右校正一下
       OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
     }
     if(L2&&!R2)//判断是否右偏
     {
       Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
       Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
       OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
     }
     if(!(L2||R2))//两个都是0即两个都不在线上就出发
     {Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
     OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
   } 
   Car_Stop();
   
     SET_Dynamixel_Position(AX_12_NO2 ,818); //数字舵机举起来
     SET_Dynamixel_Position(AX_12_NO1 ,271);   
    SET_Analog_Steering_Engine(&Analog_Steering_1,1250,5);  //1号舵机竖直
   //Analog_Steering_Engine(2,850);    
   while(GET_Dynamixel_Position(AX_12_NO2)<800)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); 
       
    Dc_Motor(Motor_L,2500,Direction_Q);
    Dc_Motor(Motor_R,2500,Direction_Q);  //走
    
   while(VL53L0X_struct.distance>165) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //测距
        
    Dc_Motor(Motor_L,0,Direction_Q);
    Dc_Motor(Motor_R,0,Direction_Q);   //停 
        
    OSTimeDly ( 100, OS_OPT_TIME_DLY, & err ); 
    
    SET_Dynamixel_Position(AX_12_NO2 ,818); //2号数字舵机往下放一放
    SET_Dynamixel_Position(AX_12_NO1 ,181); 
    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
    
    SET_Analog_Steering_Engine(&Analog_Steering_2,850,30);  //放开物品 
    
    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
    
    Dc_Motor(Motor_L,4000,Direction_H);
    Dc_Motor(Motor_R,4000,Direction_H); 
    
    while(Back.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
    while(Back.SUM>3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
    BEEP_Function(300);
    
    Dc_Motor(Motor_L,7000,Direction_H);
    Dc_Motor(Motor_R,7000,Direction_H);  
    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往前走一段
 
    Dc_Motor(Motor_L,0,Direction_Q);
    Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机 

    SET_Dynamixel_Position(AX_12_NO2 ,500);  //放下数字舵机2 
    
    OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
}
//
//void Put_1()
//{
//   OS_ERR err;
// 
//   
//       while(!(R2&&L2))//碰到线是1,没碰到线是0,只要有没碰到线的就进入
//     {
//       if(R2&&!L2)//右侧压线左侧未压线即左偏进入
//       {
//          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
//          Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前,以爪子为准,往右校正一下
//          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
//        }
//          if(L2&&!R2)//判断是否右偏
//        {
//          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
//          Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
//          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
//        }
//        if(!(L2||R2))//两个都是0即两个都不在线上就出发
//        {Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
//        OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
//        } 
//   Car_Stop();
////    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
////     //Car_Stop();
////   SET_Dynamixel_SPEED(AX_12_NO1 ,40);
////   SET_Dynamixel_SPEED(AX_12_NO2 ,40);
////   
////   OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
////  
////   SET_Dynamixel_Position(AX_12_NO2 ,808);                  ////////////////////////////////
////     SET_Dynamixel_Position(AX_12_NO1 ,310);      
////   SET_Analog_Steering_Engine(&Analog_Steering_1,1250,0);               //1号舵机竖直
////  // SET_Analog_Steering_Engine(&Analog_Steering_2,730,0);
////   
////   Dc_Motor(Motor_L,3000,Direction_Q);Dc_Motor(Motor_R,3000,Direction_Q); 
////   
////   while(VL53L0X_struct.distance>135) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
////    
////   Car_Stop();
////   
////   
////    BEEP_Function(300);
////    OSTimeDly (100, OS_OPT_TIME_DLY, & err );  //等待延时
////    
////    SET_Dynamixel_Position(AX_12_NO2 ,753);
////     OSTimeDly (300, OS_OPT_TIME_DLY, & err );  //等待延时
////                                            ////////////////////////////////
////     SET_Dynamixel_Position(AX_12_NO1 ,238);   
////     OSTimeDly (300, OS_OPT_TIME_DLY, & err );  //等待延时
////    SET_Analog_Steering_Engine(&Analog_Steering_2,730,0);
////     OSTimeDly (300, OS_OPT_TIME_DLY, & err );  //等待延时
////
////    SET_Analog_Steering_Engine(&Analog_Steering_1,750,10); //模拟舵机1旋转 
////    
////    Dc_Motor(Motor_L,4000,Direction_H);
////    Dc_Motor(Motor_R,4000,Direction_H); //倒车
////    
////     while(Back.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
////     while(Back.SUM>3)    OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
////     BEEP_Function(300);
////
////    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //先倒车  再收爪子
////        
////    Dc_Motor(Motor_L,6000,Direction_H);
////    Dc_Motor(Motor_R,6000,Direction_H);  
////    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //再往前走一段
//// 
////    Dc_Motor(Motor_L,0,Direction_Q);
////    Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机 
////   
////    SET_Dynamixel_Position(AX_12_NO1 ,160);
////    SET_Dynamixel_Position(AX_12_NO2 ,520);
////    
////   OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); 
//   
//  SET_Dynamixel_Position(AX_12_NO2 ,808); //数字舵机举起来
//     SET_Dynamixel_Position(AX_12_NO1 ,311);   
//    SET_Analog_Steering_Engine(&Analog_Steering_1,1250,5);  //1号舵机竖直
//   //Analog_Steering_Engine(2,850);    
//   while(GET_Dynamixel_Position(AX_12_NO2)<800)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); 
//       
//    Dc_Motor(Motor_L,2500,Direction_Q);
//    Dc_Motor(Motor_R,2500,Direction_Q);  //走
//    
//   while(VL53L0X_struct.distance>135) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //测距
//        
//    Dc_Motor(Motor_L,0,Direction_Q);
//    Dc_Motor(Motor_R,0,Direction_Q);   //停 
//        
//    OSTimeDly ( 100, OS_OPT_TIME_DLY, & err ); 
//    
//    SET_Dynamixel_Position(AX_12_NO2 ,818); //2号数字舵机往下放一放
//    SET_Dynamixel_Position(AX_12_NO1 ,181); 
//    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
//    
//    SET_Analog_Steering_Engine(&Analog_Steering_2,850,30);  //放开物品 
//    
//    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//    
//    Dc_Motor(Motor_L,4000,Direction_H);
//    Dc_Motor(Motor_R,4000,Direction_H); 
//    
//    while(Back.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
//    while(Back.SUM>3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
//    BEEP_Function(300);
//    
//    Dc_Motor(Motor_L,7000,Direction_H);
//    Dc_Motor(Motor_R,7000,Direction_H);  
//    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往前走一段
// 
//    Dc_Motor(Motor_L,0,Direction_Q);
//    Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机 
//
//     SET_Dynamixel_Position(AX_12_NO1 ,160); 
//    SET_Dynamixel_Position(AX_12_NO2 ,500);  //放下数字舵机2 
//    
//    OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );  
//   
//}





void Take_2()
{
  OS_ERR err;
    while(!(R2&&L2))
   {
     if(R2&&!L2)
     {
      Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
       Dc_Motor(Motor_L,6000,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);
       OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
     }
     if(L2&&!R2)
     {
       Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
       Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,6000,Direction_Q);
       OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
     }
     if(!(L2||R2)){Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
     OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
   } 
   Car_Stop();
   
   OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
  SET_Dynamixel_Position(AX_12_NO2 ,820);
        SET_Dynamixel_Position(AX_12_NO1 ,160);
   //SET_Dynamixel_Position(AX_12_NO2 ,808);////////////////////////////////
     OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );    
   SET_Analog_Steering_Engine(&Analog_Steering_1,1250,0); //1号舵机竖直
   SET_Analog_Steering_Engine(&Analog_Steering_2,730,0);
   
   Dc_Motor(Motor_L,3000,Direction_Q);Dc_Motor(Motor_R,3000,Direction_Q); 
   
   while(VL53L0X_struct.distance>150) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
    
   Car_Stop();
   
    BEEP_Function(300);
    
    /* 向前到位 准备抓取  */    
    
    SET_Analog_Steering_Engine(&Analog_Steering_2,1030,0); //模拟舵机夹紧

    OSTimeDly (400, OS_OPT_TIME_DLY, & err );  //等待延时
        
    SET_Dynamixel_Position(AX_12_NO1 ,260);  //1号舵机往上微微举起
 //   while(GET_Dynamixel_Position(AX_12_NO1)<250)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
    OSTimeDly (800, OS_OPT_TIME_DLY, & err );  //等待延时
    
    
    SET_Analog_Steering_Engine(&Analog_Steering_1,750,10); //模拟舵机1旋转 
    
    Dc_Motor(Motor_L,4000,Direction_H);
    Dc_Motor(Motor_R,4000,Direction_H); //倒车

    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //先倒车  再收爪子
     
    SET_Dynamixel_Position(AX_12_NO1 ,160);
    SET_Dynamixel_Position(AX_12_NO2 ,500);
    
    while(Back.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
    while(Back.SUM>3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
    BEEP_Function(300);
    
    Dc_Motor(Motor_L,5000,Direction_H);
    Dc_Motor(Motor_R,5000,Direction_H);  
    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //再往前走一段
 
    Dc_Motor(Motor_L,0,Direction_Q);
    Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机 
  
    OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );  
}
//////////////////////////////////////////////////////////////
void Put_2()
{
    OS_ERR err;
  
       while(!(R2&&L2))//碰到线是1,没碰到线是0,只要有没碰到线的就进入
     {
       if(R2&&!L2)//右侧压线左侧未压线即左偏进入
       {
          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
          Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前,以爪子为准,往右校正一下
          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
        }
          if(L2&&!R2)//判断是否右偏
        {
          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
          Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
        }
        if(!(L2||R2))//两个都是0即两个都不在线上就出发
        {Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
        OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
        } 
   Car_Stop();
   //Car_Stop();
    
    SET_Dynamixel_Position(AX_12_NO1 ,660);
    SET_Dynamixel_Position(AX_12_NO2 ,160);
    
    SET_Analog_Steering_Engine(&Analog_Steering_1,1250,15); //1号舵机竖直////////////////////////////////////////////////
    
   Tracing_struct.Direction=Direction_Q;
   OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
    
   while(VL53L0X_struct.distance>90) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
        
   OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
   Dc_Motor(Motor_L,0,Direction_Q);
   Dc_Motor(Motor_R,0,Direction_Q);//关闭左右电机
   
   SET_Dynamixel_Position(AX_12_NO1 ,620);
   
   
   
   BEEP_Function(300);
   
   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
   SET_Analog_Steering_Engine(&Analog_Steering_2,750,30); 
   SET_Dynamixel_Position(AX_12_NO1 ,700);//
   OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
   
   Dc_Motor(Motor_L,3000,Direction_H);
   Dc_Motor(Motor_R,3000,Direction_H); //走曲线
   
   along_Stop(Direction_H,1);
   
    SET_Dynamixel_Position(AX_12_NO1 ,160);
    SET_Dynamixel_Position(AX_12_NO2 ,500);
    
   OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );  
}
/////////////////////////////////////////////////////////////
//



void Put_3()
{
   OS_ERR err;

    while(!(R2&&L2))
   {
     if(R2&&!L2)
     {
      Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
       Dc_Motor(Motor_L,7000,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);
       OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
     }
     if(L2&&!R2)
     {
       Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
       Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,7000,Direction_Q);
       OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
     }
     if(!(L2||R2)){Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
     OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
   } 
   Car_Stop();
   
    SET_Dynamixel_Position(AX_12_NO1 ,546);
    SET_Dynamixel_Position(AX_12_NO2 ,160);
    
    SET_Analog_Steering_Engine(&Analog_Steering_1,1250,15); //1号舵机竖直  
        /*  前进 */  
   Tracing_struct.Direction=Direction_Q;
   OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
   
   OSTimeDly ( 1800, OS_OPT_TIME_DLY, & err );
    
   OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
   
   BEEP_Function(300);
   
   Dc_Motor(Motor_L,3000,Direction_Q);
   Dc_Motor(Motor_R,3050,Direction_Q); //走曲线

   OSTimeDly ( 3200, OS_OPT_TIME_DLY, & err );
   
   BEEP_Function(300);
   

   while(VL53L0X_struct.distance>75) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
        

   Dc_Motor(Motor_L,0,Direction_Q);
   Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
   
   BEEP_Function(300);
   
   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
   SET_Analog_Steering_Engine(&Analog_Steering_2,850,30); //
   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
   
   Dc_Motor(Motor_L,3000,Direction_H);
   Dc_Motor(Motor_R,3100,Direction_H); 
   
   OSTimeDly ( 1500, OS_OPT_TIME_DLY, & err );
   
   SET_Dynamixel_Position(AX_12_NO1 ,160); //下放舵机至最低处
   SET_Dynamixel_Position(AX_12_NO2 ,500);
   
   BEEP_Function(300);
   
   OSTimeDly ( 2500, OS_OPT_TIME_DLY, & err );
   
   along_Stop(Direction_H,1);  
}

//void Put_3()
//{
//   OS_ERR err;
//   
//       while(!(R2&&L2))//碰到线是1,没碰到线是0,只要有没碰到线的就进入
//     {
//       if(R2&&!L2)//右侧压线左侧未压线即左偏进入
//       {
//          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
//          Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前,以爪子为准,往右校正一下
//          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
//        }
//          if(L2&&!R2)//判断是否右偏
//        {
//          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
//          Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
//          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
//        }
//        if(!(L2||R2))//两个都是0即两个都不在线上就出发
//        {Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
//        OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
//        } 
//   Car_Stop();
//     
//   //Car_Stop();
//   
//    SET_Dynamixel_Position(AX_12_NO1 ,700);
//    SET_Dynamixel_Position(AX_12_NO2 ,300);
//    
//    SET_Analog_Steering_Engine(&Analog_Steering_1,1250,15); //1号舵机竖直  
//       
////   Tracing_struct.Direction=Direction_Q;
////   OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
////   
////   OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
////    
////   OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
//   
//   BEEP_Function(300);
//   
//   Dc_Motor(Motor_L,3000,Direction_Q);
//   Dc_Motor(Motor_R,3000,Direction_Q); //走曲线
//
//  // OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
//   
//  // BEEP_Function(300);
//   
// //OS_TaskResume(&TracingTaskTCB,&err);   //恢复寻迹任务
//    
//   while(VL53L0X_struct.distance>80) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
//        
//   //OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
//   //while(VL53L0X_struct.distance>95) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
//        
//
//   Dc_Motor(Motor_L,0,Direction_Q);
//   Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
//   
//   
//   
//     SET_Dynamixel_Position(AX_12_NO2 ,160);  
//    SET_Dynamixel_Position(AX_12_NO1 ,600);
//    
//    
//   
//   BEEP_Function(300);
//   
//   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//   SET_Analog_Steering_Engine(&Analog_Steering_2,800,30); //
//   OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//   
//   Dc_Motor(Motor_L,3000,Direction_H);
//   Dc_Motor(Motor_R,3000,Direction_H); 
//   
//  // OSTimeDly ( 2500, OS_OPT_TIME_DLY, & err );
//   
//   SET_Dynamixel_Position(AX_12_NO1 ,160); //下放舵机至最低处
//   SET_Dynamixel_Position(AX_12_NO2 ,500);
//   
//   BEEP_Function(300);
//   
//   //OSTimeDly ( 2000, OS_OPT_TIME_DLY, & err );
//   
//   while(Back.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
//    while(Back.SUM>3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
//    BEEP_Function(300);
//   
//    Dc_Motor(Motor_L,5000,Direction_H);
//    Dc_Motor(Motor_R,5000,Direction_H);  
//    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //再往前走一段
// 
//    Dc_Motor(Motor_L,0,Direction_H);
//    Dc_Motor(Motor_R,0,Direction_H); //关闭左右电机 
//  
//    OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );  
//   // BEEP_Function(300);
//  // along_Stop(Direction_H,1);  
//}

void Put_4()
{
  OS_ERR err;
  
       while(!(R2&&L2))//碰到线是1,没碰到线是0,只要有没碰到线的就进入
     {
       if(R2&&!L2)//右侧压线左侧未压线即左偏进入
       {
          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
          Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前,以爪子为准,往右校正一下
          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
        }
          if(L2&&!R2)//判断是否右偏
        {
          Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); 
          Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
          OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
        }
        if(!(L2||R2))//两个都是0即两个都不在线上就出发
        {Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
        OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
        } 
   Car_Stop();
   
    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );

   Dc_Motor(Motor_L,3000,Direction_Q);
   Dc_Motor(Motor_R,3000,Direction_Q);    //先跑起来
   
    SET_Dynamixel_Position(AX_12_NO2 ,820);  //抬起爪子
    SET_Dynamixel_Position(AX_12_NO1 ,160);
    SET_Analog_Steering_Engine(&Analog_Steering_1,750,15); //1号舵机竖直
 
   while(VL53L0X_struct.distance>170) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
        
  Car_Stop();
  
    OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );
    
    SET_Dynamixel_Position(AX_12_NO2 ,732);
    
    OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
    
    SET_Analog_Steering_Engine(&Analog_Steering_2,770,15);  //放开瓶子
    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//  
//     SET_Dynamixel_Position(AX_12_NO2 ,800);
//     SET_Dynamixel_Position(AX_12_NO1 ,221);
//     OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );//微微上台
      
     Dc_Motor(Motor_L,4000,Direction_H);
     Dc_Motor(Motor_R,4000,Direction_H); //后退
     OSTimeDly ( 1500, OS_OPT_TIME_DLY, & err );
   
    SET_Dynamixel_Position(AX_12_NO2 ,510);  //往下收起数字舵机
     
    SET_Analog_Steering_Engine(&Analog_Steering_2,1010,15);  //模拟舵机爪子夹起
    
    while(Back.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
    while(Back.SUM>3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
    BEEP_Function(300);
    
    Dc_Motor(Motor_L,7000,Direction_H);
    Dc_Motor(Motor_R,7000,Direction_H);  
    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往后走一段
    
    Car_Stop(); 
   //Car_Stop();
   
//   OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
//
//   Dc_Motor(Motor_L,3000,Direction_Q);
//   Dc_Motor(Motor_R,3000,Direction_Q);    //先跑起来
//   
//    SET_Dynamixel_Position(AX_12_NO2 ,300);  //抬起爪子
//    SET_Dynamixel_Position(AX_12_NO1 ,700);
//    
//    SET_Analog_Steering_Engine(&Analog_Steering_1,750,15); //1号舵机竖直
// 
//   while(VL53L0X_struct.distance>150) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
//        
//  Car_Stop();
//  
//    OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );
//    
//     SET_Dynamixel_Position(AX_12_NO2 ,370);  
//    SET_Dynamixel_Position(AX_12_NO1 ,490);
//    
//    OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
//    
//    SET_Analog_Steering_Engine(&Analog_Steering_2,780,15);  //放开瓶子
//    OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//    
//    Dc_Motor(Motor_L,1500,Direction_H);Dc_Motor(Motor_R,1500,Direction_H);
//      OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );
//    
//    
//   
////     SET_Dynamixel_Position(AX_12_NO2 ,800);
////     SET_Dynamixel_Position(AX_12_NO1 ,221);
////     OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );//微微上台
//      
//     Dc_Motor(Motor_L,4000,Direction_H);
//     Dc_Motor(Motor_R,4000,Direction_H); //后退
//     OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
//   
//     SET_Dynamixel_Position(AX_12_NO2 ,520);  //抬起爪子
//    SET_Dynamixel_Position(AX_12_NO1 ,160);
//    SET_Analog_Steering_Engine(&Analog_Steering_1,1250,15); //1号舵机竖直
//    SET_Analog_Steering_Engine(&Analog_Steering_2,1020,15);
//    
//    
//    Dc_Motor(Motor_L,4000,Direction_H);
//     Dc_Motor(Motor_R,4000,Direction_H); //后退
//    
//    while(Back.SUM<=3)   OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //先是在主线上  
//    while(Back.SUM>3)  OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );  //压到横线
//    BEEP_Function(300);
//    
//    Dc_Motor(Motor_L,6000,Direction_H);
//    Dc_Motor(Motor_R,6000,Direction_H);  
//    OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往后走一段
//    
//    Car_Stop(); 
}


on the way to be a Electrical Engineer & Designer......