简 介： 为完成第十七届全国大学生智能汽车竞赛智能视觉组的分类搬运任务，我自主设计了凤凰三分类搬运结构，可以实现对全场地上的目标板进行拾取并分三类存储于转盘储物仓中，同时该结构可以通过控制电磁锁打开仓门实现快速“卸货”，可以快速且完美地完成视觉组的搬运任务。 凤凰四分类搬运结构是为了适应国赛线上赛的规则，基于凤凰三分类结构改进而来，也是昀终参赛版本，由于结构原理相似，本文只介绍四分类转盘储物仓的制作方法与应用。关键词： 智能视觉组，分类存储搬运，凤凰四分类搬运结构

1.1 耗材类

北化 T-1型 502瞬间粘着剂、30×40×0.2㎝及 30×40×0.3㎝三合木板各一张、高级轴承油、面巾纸或棉絮。

1.2 机械类

• 转盘储物仓 ：2英寸不锈钢合页×4、三片式平面推力滚针轴承（内 65外 90高 5㎜）×1、锁扣（3D打印/木制/金属制）×4。
• 机械臂舵机支架 ：万能支架× 2、大长 U型支架×2、短 U型支架×2、杯式轴承× 2、Futaba25T-2㎜舵盘×3。
• 螺母 ：M3普通螺母、M3自锁螺母、M3*10+6铜柱。
• 螺丝 ：M2.5 5、M3 5、M3 6、M3 8、M3 10、M3 12、M3 15、M3 20。
• 垫圈 ：M3塑料垫圈、M3金属垫圈、M4金属大垫圈。

1.3 电子元件类

SPT5410HV-180高压数字舵机×2、SPT5435MS磁编码 360°可控舵机、微型电磁锁（弹舌朝左）×4、电磁铁 KK-P30/22。

单路电磁继电器模块（自制）、四路电磁继电器模块（自制）、 XH2.54-2P单头连接线、XH2.54-6P连接线、舵机延长线。

1.4 制作工具类

铅笔、不锈钢尺、多功能螺丝刀套装、裁纸刀、老虎钳、压线钳、剪刀、电烙铁等。

注：我的所有打孔工具均为螺丝刀，每个孔需更换三种十字刀头才能保证木板不开裂，故建议使用专用打孔工具。

01转盘储物仓

第二章转盘储物仓制作步骤

2.1 切割木板尺寸

2.2 底板

根据下表画好参考线，并挖好预留的孔位：

2.2 内部底座

2.3 左侧侧板

2.4 电磁锁延长线及铜柱预留

2.5 右侧侧板

2.6 固定侧板

由于侧板上需要挂载仓门，所以建议使用面巾纸进行填缝，再滴加 502进行紧固，增加支撑力。

2.7 固定仓门

2.8 锁扣

（1）锁扣制作（3D打印，可自行设计）

（2）锁扣固定

2.9 顶盖

2.10 轴承顶面

2.11 电子元件

2.12 独角仙隔挡

这种隔挡在三分类机构适用，四分类机构则不推荐。因为三分类空间较为宽敞，不会出现卡住目标板无法下落的情况，同时可以更好的将目标板整理为竖直状态。

转盘储物仓是凤凰四分类结构的精髓所在，制作部分就到此结束了。

03储物仓装载

3.1 储物仓支架

储物仓支架应根据个人情况进行制作，注意事项主要有：

（1）留好四个通孔以便固定舵盘；

（2）中心留沉孔以便预留螺丝，同时开好 3㎜通孔以便插入螺丝刀；

（3）预留好一定空间给平面推力轴承；

（4）预留控制线孔位。

▲ 图3.1.1 储物仓支架

3.2 舵盘

舵盘固定在支架前，一定要在中间留好一颗 M3*6的螺丝，用以和舵机连接。

3.3 平面推力轴承

将轴承底面至于支架留好的圆环槽内，可以选择热熔胶固定，也可以选择不固定。

3.4 连接储物仓与支架

连接前需要先将两个控制线穿过预留空位，将舵机初始化到想要的位置，比如初始化为 240°，此时红色储物仓朝前。

最后将连接舵机的螺丝拧紧，转动检察是否松动，上电检查初始化角度是否正常。

▲ 图3.4.1 储物曾与支架连接效果

04机械臂组成

4.1 两轴机械臂

机械臂两个舵机均使用 SPT5410HV-180高压数码舵机，转速高达 0.07sec/60°，舵机转动稳定不抖舵。

在赛前三天的时候我尝试使用 GXServoX20-270极速无刷舵机来替代，转速高达 0.045sec/60°。虽然这款舵机扭矩大速度快，但舵机运行不稳定，容易发生抖舵和以及响应过慢丢失动作导致拾取失败。此外，舵机转速过快也会导致目标板在转动时脱离电磁铁而被扔出去，所以白费了一天一夜的调试，昀终换回了 SPT5410。

机械臂结构简单，主要由标准件组成，上臂是一个大长 U型支架，下臂由两个大长 U型支架和一个短 U型支架连接构成。

机械臂整体由一个 U型 3D打印支架固定于车头的位置，机械臂末端执行机构为电磁铁。

▲ 图4.1.1 两轴机械臂与机械臂固定支架

4.2 电磁铁

电磁铁使用乐清卡卡电气型号 KK-P30/22的电磁铁，参数为 DC12V吸力 15kg。电磁铁使用配套 M4螺丝、弹簧垫圈以及一个 M4大垫圈固定于大长 U型支架中间预留孔中。

机械臂末端电磁铁 KK-P30/22：

▲ 图4.2.1 电磁铁

大家直觉上认为电磁铁接触面越大越有利于拾取目标板，但经过使用多种不同型号电磁铁的实践结果来看，我认为反而是偏长条形的电磁铁更有利于目标板的拾取，下面我将试着给出理由。

（1）电磁铁通电后有磁力的范围仅限于中间部分，也就是绕制线圈内部的铁芯才具有磁力，除此之外均无磁力。

▲ 图4.2.2 电磁铁磁场分布

（2）电磁铁磁力随着吸附距离增加，磁力会急剧减弱；若电磁铁吸盘平面不能与目标板贴合紧密，那么吸附力将大打折扣。

（3）假设一个情景，舵机角度调整不合理导致电磁铁落地时吸盘平面没有正对目标板，那么距离地面远的部分将基本上丧失吸附作用。

（4）电磁铁面积越大，那么它卡在某个角度后，远端距离目标板的距离越长，“浪费”了更多的磁力。

▲ 图4.2.3 电磁铁与目标板成一定的倾角

以上就是我对于电磁铁使用情况的理解，电磁铁的这些特性不仅影响到选型，对于舵机动作的设置也具有指导意义。

05舵机动作设置

5.1 初始化

（1）转盘舵机：先下放机械臂，再将转盘舵机初始化为 220°，此时红色标记的储物仓朝前。

（2）下舵机：抬起贴近转盘，初始化为 95°。

（3）上舵机：尽量靠前但不遮挡摄像头视野，初始化为 65°，方便快速转到正对地面的角度 5°。

steer_init();//舵机初始化 
turn_Angle(firststeer,15,180);//下舵机下放以供转盘复位 
turn_Angle(secondsteer,100,180);//上舵机 
systick_delay_ms(200); 
turn_Angle(thirdsteer,220,360);//中舵机归位：360套件一黑 40\黄 130\红 220\蓝 310 
systick_delay_ms(400); 
turn_Angle(firststeer,95,180);//下舵机归位 
turn_Angle(secondsteer,60,180);//上舵机归位 

5.2 下放拾取目标板

（1）电磁铁通电。

electromagnet_Set(1);

（2）优先转动上舵机，保证上舵机先行到达指定角度（6°），有利于电磁铁与目标板接触面贴合，吸取更加稳定。这一点如果不能保证，就可能出现本文第四章 4.2中提到的情景，导致吸附失败。

turn_Angle(secondsteer,6,180);//上舵机转到最下面

（3）平时调车阶段考虑到对舵机进行保护，可以使用弹簧进行缓冲，但是为了实现放入储物仓时精度要求比较高，我采用了软件延时缓冲的方式。例如电磁铁吸附目标板时需要将下舵机设定角度为25°，下舵机从95°下放到25°过程中，我在45°的位置设置了 150ms的延时作为缓冲，可以起到静音拾取的作用。

（4）比赛阶段可以将静音拾取的缓冲延时去掉，启用宏定义#defineClockUp，此时舵机会快速转动将电磁铁砸在目标板上，对舵机齿轮有损坏风险，但总时长可以减少2s左右。

#ifdef ClockUp turn_Angle(firststeer,25,180);//下舵机不完全触碰地面，起缓冲作用提速改 25 
#else 
turn_Angle(firststeer,45,180);//下舵机不完全触碰地面，起缓冲作用稳定改 45 
#endif
//此处跳过储物仓分类转动步骤 
#ifdef ClockUp systick_delay_ms(150);//提速延长目标板接触时间 
#else 
systick_delay_ms(150);//舵机延时缓冲，提速去掉 turn_Angle(firststeer,25,180);//下舵机下放吸取目标板 systick_delay_ms(80);//稳定改 80 
#endif 

5.3 储物仓分类转动

转盘使用360°磁编码舵机，需给定绝对位置进行控制，我将红色标记的储物仓朝前作为初始化状态220°。

（1）对应关系

每个储物仓需要执行朝前存储、侧方开门两个任务，所以需要对应好角度和电磁锁的控制引脚，初始化状态下对应关系如下：

▲ 图5.3.1 对应关系图

（2）等待间隔

舵机转动不同角度，需要的时间不同，大概分为三个角度差：90°、180°和270°，额定转动时间分别为：210ms、420ms和630ms。

机械臂抬起状态会卡住转盘，所以机械臂需要等待转盘动作完成，虽然不需要从头等到尾，但也大致会分别为四档，无需转动、转动时间短、中、长。

想知道本次转动的角度差，就必须将本次舵机位置和上一次舵机位置作差，其实就是对应编号的差值0、1、2、3，有了转动角度差就可以对应设置不同的延时了。

（3）忽略不同间隔时间的影响

转盘转动的时间其实并不花费比赛时间，因为在电磁铁吸附目标板后，车模其实已经离开前往下一个目标点了，所以后续抬起机械臂将目标板送入储物仓这些是动作均在车模运动或者调整车身位置的过程中完成。

经过实际测试，我发现给出昀长延时也可以在进行下一次任务前完成所有动作，所以我昀终把延时时间都统一了。这样的话，我们就可以略去求角度差这一步，统一进行控制。

5.4 送入储物仓

这一部分都是在车模运动中完成的，不占用比赛时间，所以可以大胆延时。

（1）稍微抬起机械臂等待转盘转动

这个阶段如果将下舵机抬的过高，机械臂就会卡到转盘储物仓，所以需要将机械臂稍微抬起等待几百毫秒，此时由于重心过于靠前，车身会明显扭动。

systick_delay_ms(160); 
turn_Angle(firststeer,50,180);//下舵机抬起等待转盘 
systick_delay_ms(230);

（2）抬高机械臂准备调转目标板方向

目标板面积大，如果机械臂抬得不够高就开始调转目标板方向，会造成目标板一角卡在地面上发生形变。

▲ 图5.4.1 目标板接触地面发生形变

目标板一旦卡到了地面上，就极有可能脱离电磁铁。昀典型的现象就是机械臂明明将电磁铁拾取起来了，目标板却被往前扔了出去，看似是舵机太快或者电磁铁吸力不足导致的，其实本质原因在此。

因此必须将下舵机设置到角度比较大的位置（70°），再给足够的延时让下舵机打到目标值。

turn_Angle(firststeer,60,180);//下舵机抬起避免目标板卡到地面
systick_delay_ms(50);

（3）下放机械臂同时调转目标板方向‘

’为什么刚抬起机械臂又要下放了？因为机械臂上方还有一个调姿摄像头。

抬起机械臂是给目标板留转向的空间，下放机械臂是为了绕开摄像头。若不下放机械臂，任由上舵机转动目标板，势必把目标板打在摄像头上导致目标板被弹飞。

此外，即使绕过了摄像头，目标板在旋转过程中也有可能卡到转盘中的目标板，效果和卡到摄像头一样，会把目标板弹飞。所以需要两个舵机动作配合，在下放机械臂的同时调转目标板方向，等待目标板经过摄像头下方之后再进一步回收。

turn_Angle(secondsteer,100,180);//上舵机只转到一半，避免卡到摄像头 
systick_delay_ms(50); 
turn_Angle(firststeer,40,180);//下舵机下放避免目标板卡到转盘目标板

（4）送入储物仓

上下舵机同时回收，将目标板斜着插入储物仓。仓内有无目标板对于送入储物仓的动作要求有不同要求。

如果是该储物仓第一个放入的目标板，要避免目标板斜切的时候撞到顶盖而弹飞，所以可以将机械臂设置得稍微靠前一点，让目标板自由落入储物仓。因此我通过标志位来设置两个不同的动作。

四个种类都有第一张目标板，是不是该用四个标志位？

我通过一个有趣的数学工具——质数来解决标志位问题。首先是设置一个布尔类型的标志位来标记是否为第一个目标板，其次是使用质数 2、3、5、7累乘来记录不同种类的第一个目标板。

（5）舵机归位

等待舵机停稳后电磁铁断电释放目标板，之后两个舵机回到初始化状态，等待下一次搬运任务。

systick_delay_ms(400);//等待舵机停稳，释放目标板 
electromagnet_Set(0); 
systick_delay_ms(60);
//机械臂归位 
turn_Angle(firststeer,65,180);//下舵机 
turn_Angle(secondsteer,60,180);//上舵机避免遮住摄像头 
systick_delay_ms(80);  
turn_Angle(firststeer,95,180);//下舵机 

5.5 “卸货”任务

（1）机械臂下放

由于卸货时仓门打开后无法重新合上，为了避免仓门卡到机械臂而导致断裂，要求机械臂尽可能地贴近地面，所以昀后执行任务时机械臂基本上是在地面上拖动的。

turn_Angle(firststeer,0,180);//下舵机下放到最低位置 
turn_Angle(secondsteer,110,180);//上舵机 
systick_delay_ms(100);

（2）转盘转动辅助开门

四个种类“卸货”的位置和目标板在转盘上对应的位置是一致的，由于我们将底下那一类目标板留到了车库内释放，所以昀终的“卸货”顺序是按照赛场右边、上边、左边、下边的顺序来的，对应的编号为③②①④。

将转盘转到角度打开仓门就可以释放目标板了，为什么说是转盘转动辅助开门呢？

如果只是静止状态下打开仓门，目标板确实会受重力自动推开仓门下落，但如果目标板还未来得及落下车就往回运动，势必会带到目标板往回运动，可能导致压线判罚。

那为什么不在转动前提前释放目标板？首先此时车模尚未达到边界外，其次未达到设定角度随意打开仓门只会导致目标板落在车上，所以不可取。

为了不拖泥带水，我在打开仓门之后设置舵机朝特定方向转动一定角度将目标板往外甩出去，快速稳定地实现释放目标板，B站网友对此的评价是“干净利落”。

具体转动角度和方向要结合目标板应该落在边界线哪一边来决定，比如上边界外需要往前扔，左边界外需要往左边扔，设置好方向才能保证目标板释放后远离边界线。

▲ 图5.6.1 转动辅助具体角度

以上是舵机动作设置要点讲述，演示视频将会上传哔哩哔哩。

※结  论 ※

本文介绍了制作凤凰四分类搬运结构的详细步骤及原理，同时阐述了机械臂与转盘储物仓配合运动过程的程序设置，希望能够对各位读者起到一定的启发。

无论是视觉组国赛线下赛的三分类还是线上赛的四分类，我们都可以游刃有余地应对，重点在于我们从不固步自封。

从一开始备战第十七届视觉组，我们就没有坐等逐飞提供方案演示，而是自己开创自己的道路；在别人做出他们方案的时候，我们也不会急于去抄袭，而是去思考自己想要的效果，发挥自己的创新思维和创造能力，让别人永远跟不上我们创造的脚步！

附录

1. steer_control.c

#include "steer_control.h" 
//#define ClockUp 
int SteerPosition = 2;//起始位置 
int position = 220; 
int prime = 1; 
bool once_flag = 0; 

/**************************************************************************
函数功能：提供舵机的运作的初始化
入口参数：无
返回值：无
使用说明：在 main函数里面调用一次即可 
**************************************************************************/ 

void steer_init() 
{ 
    pwm_init(steer_one,50,0); 
    pwm_init(steer_two,50,0); 
    pwm_init(steer_three,50,0);//舵机初始化 
} 

//-----------------------------------------------------------------------------------
// @brief舵机旋转角度设置 // @param whichsteer选择舵机：firststeer,secondsteer,thirdsteer 
// @param angle设置的旋转角度 
// @param maxAngle舵机的最大角度（由舵机本身参数决定，选择 180°和270°或360°） 
// @return void 
// Sample usage: turn_Angle(firststeer,90,270)270°舵机旋转到90°位置 
//-----------------------------------------------------------------------------------¬

void turn_Angle(MySteer Whichsteer,int angle,int maxAngle) 
{ 
    switch(Whichsteer) { 
        case firststeer: 
        { 
            if(maxAngle==180) { 
                if(angle<181&&angle>-1) { 
                    pwm_duty(steer_one,(angle*10/9+50)*25); 
                } 
            } else if(maxAngle==270) { 
             if(angle<271&&angle>-1) { 
              pwm_duty(steer_one,(angle*20/27+50)*25); 
    } 
   } else if(maxAngle==360) { 
    if(angle<361&&angle>-1) { 
     pwm_duty(steer_one,(angle*5/9+50)*25); 
    } 
   } else return; 
  } 
  break; 
 case secondsteer: { 
  if(maxAngle==180) { 
   if(angle<181&&angle>-1) { 
    pwm_duty(steer_two,(angle*10/9+50)*25); 
   } 
  } else if(maxAngle==270) { 
   if(angle<271&&angle>-1) { 
    pwm_duty(steer_two,(angle*20/27+50)*25); 
   } 
  } else if(maxAngle==360) { 
   if(angle<361&&angle>-1) { 
    pwm_duty(steer_two,(angle*5/9+50)*25); 
   } 
  } 
  else return; 
 } 
 break; 
 case thirdsteer: { 
  if(maxAngle==180) 
  { 
   if(angle<181&&angle>-1) { 
    pwm_duty(steer_three,(angle*10/9+50)*25); 
   } 
  } else if(maxAngle==270) { 
   if(angle<271&&angle>-1) { 
     pwm_duty(steer_three,(angle*20/27+50)*25); 
   } 
  } else if(maxAngle==360) { 
   if(angle<361&&angle>-1) { 
    pwm_duty(steer_three,(angle*5/9+50)*25); 
   } 
  } else return; 
 } 
 break; 

 default: 
  assert(0); 
  break; 
 } 
}

//电磁铁，0为释放，1为吸附 
void electromagnet_Set(uint8_t choose) { 
 gpio_set(C14,choose); 
}

//将物体吸附 
void getImg() 
{ 
 int next_position = JudgeThirdSteerPosition(SmallClass); 
 int steerTurnErr = next_position -SteerPosition; 
 if(next_position == 0) //不吸取 
  return;

 //放 
 electromagnet_Set(1); 
#ifdef ClockUp 
 turn_Angle(firststeer,25,180);//下舵机不完全触碰地面，起缓冲作用提速改 25 
#else 
 turn_Angle(firststeer,45,180);//下舵机不完全触碰地面，起缓冲作用稳定改 45 
#endif 
 turn_Angle(secondsteer,6,180);//上舵机转到最下面 systick_delay_ms(20);

 //分类转动转盘 
 switch(abs(steerTurnErr)) 
 { 
  case 0: { 
   //systick_delay_ms(80); 
   break; 
  } 
  
  case 1: 
  { 
   switch(next_position) { 
    case 1: position = 310;break; 
    case 2: position = 220;break; 
    case 3: position = 130;break; 
    case 4: position = 40;break; 
   } 
   turn_Angle(thirdsteer,position,360);/ /中舵机 
   break; 
  } 
  
  case 2: { 
   switch(next_position) { 
    case 1: position = 310;break; 
    case 2: position = 220;break; 
    case 3: position = 130;break; 
    case 4: position = 40;break; 
   } 
   turn_Angle(thirdsteer,position,360);//中舵机 
  } 

  case 3: { 
   switch(next_position) { 
    case 1: position = 310;break; 
    case 2: position = 220;break; 
    case 3: position = 130;break; 
    case 4: position = 40;break; 
   } 

   turn_Angle(thirdsteer,position,360);//中舵机 
   break; 
  } 
  } 

#ifdef ClockUp 
 systick_delay_ms(150);//提速改 150 
#else 
 systick_delay_ms(150);//提速去掉 
 turn_Angle(firststeer,25,180);//下舵机下放吸取目标板 
 systick_delay_ms(80);//稳定改 80 
#endif 

 SteerPosition = next_position; 
 switch(SteerPosition){//第一次放置特殊处理 
  
unsigned char JudgeThirdSteerPosition(unsigned char Class) { 
 if(Class*3||Class*10||Class*11) return 1; 
 else if(Class*4||Class*8||Class*13) return 2; 
 else if(Class*1||Class*6||Class*14) return 3; 
 else if(Class*2||Class*9||Class*12) return 4; 
 else return 0; 
}

 
//pullClass: 1左,2上,3右,4下 
void pulloutStart(unsigned char pullClass) 
{ 
 turn_Angle(firststeer,0,180);//下舵机 
 turn_Angle(secondsteer,110,180);//上舵机 
 systick_delay_ms(100); 
 switch(pullClass) 
 { 
  case 1: { 
   turn_Angle(thirdsteer,160,360);//中舵机 178 
   break ; 
  } 
  case 2: { 
   turn_Angle(thirdsteer,260,360);//中舵机 220 
   break ; 
  } 
  case 3: { 
   turn_Angle(thirdsteer,280,360);//中舵机 268 
   break ; 
  } 
  case 4: { 
   turn_Angle(thirdsteer,350,360);//中舵机 310 
   break ; 
  } 
 } 
} 

void pulloutEnd(unsigned char pullClass) 
{ 
 switch(pullClass) { 
  case 1: 
  { 
   gpio_set(C15,1); 
   turn_Angle(thirdsteer,200,360);//中舵机右转甩出目标板 220 
   systick_delay_ms(100); 
   gpio_set(C18,0); 
   break ; 
  } 
  case 2: { 
   gpio_set(C18,1); 
   turn_Angle(thirdsteer,178,360);//中舵机左转甩出目标板 
   systick_delay_ms(100); 
   gpio_set(C26,0); 
   break ; 
  } 
  case 3: { 
   gpio_set(C26,1); 
   turn_Angle(thirdsteer,270,360);//中舵机左转甩出目标板 220 
   systick_delay_ms(120); 
   gpio_set(B28,0); 
   break ; 
  } 
  case 4: { 
   gpio_set(B28,1); 
   turn_Angle(thirdsteer,130,360);//中舵机左转甩出目标板 268改 130 //
   systick_delay_ms(100); 
   break ; 
  } 
 } 
} 

void SteerUp() { 
 //systick_delay_ms(200); 
 turn_Angle(thirdsteer,170,360);//中舵机 220红改 130黄 
 systick_delay_ms(100); 
 turn_Angle(secondsteer,110,180);//上舵机 
 turn_Angle(firststeer,15,180);//下舵机 
 gpio_set(C15,0); 
} 

void putToCar() 
{ 
 int next_position = JudgeThirdSteerPosition(SmallClass); 
 if(next_position == 0)//不收回 return;
 //收 
 systick_delay_ms(160); 
 turn_Angle(firststeer,50,180);//下舵机抬起等待转盘 s
 ystick_delay_ms(230); 
 turn_Angle(firststeer,60,180);//下舵机抬起避免目标板卡到地面 
 systick_delay_ms(50); 
 turn_Angle(secondsteer,100,180);//上舵机只转到一半，避免卡到摄像头
 systick_delay_ms(50); turn_Angle(firststeer,40,180);//下舵机下放避免目标板卡到转盘 

 if(once_flag == 1) {//第一个放置特殊处理 
  turn_Angle(secondsteer,170,180);//上舵机往后放 
  systick_delay_ms(220); 
  once_flag=0; 
  turn_Angle(firststeer,74,180);//下舵机往后放 
  systick_delay_ms(50); 
  turn_Angle(secondsteer,180,180);//上舵机往后放 
 } 
 else { 
  turn_Angle(secondsteer,140,180);//上舵机往后放 
  systick_delay_ms(160); 
  turn_Angle(firststeer,77,180);//下舵机往后放 76 
  systick_delay_ms(50); 
  turn_Angle(secondsteer,180,180);//上舵机往后放 
 } 
 systick_delay_ms(400);//等待舵机停稳，释放目标板 
 electromagnet_Set(0); 
 systick_delay_ms(60);
 //机械臂归位 
 turn_Angle(firststeer,65,180);//下舵机 
 turn_Angle(secondsteer,60,180);//上舵机避免遮住摄像头 
 systick_delay_ms(80); 
 turn_Angle(firststeer,95,180);//下舵机 
}