苏 州 市 职 业 大 学

课程设计说明书

名称 会追光的机器人 　　　

20XX年12月31日 至 20XX年1月4日 共 1 周

院 　系 电子信息工程系

班 级 10电气3

姓 名 **

学 号 **

第一章 绪论

1.1课程设计任务背景

机器人由机械部分、传感部分、控制部分三大部分组成．这三大部分可分成驱动系统、机械结构系统、感受系统、机器人一环境交互系统、人机交互系统、控制系统六个子系统现在机器人普遍用于工业自动化领域，如汽车制造，医疗领域，如远程协助机器人，微纳米机器人，军事领域，如单兵机器人，拆弹机器人，小型侦查机器人（也属于无人机吧），美国大狗这样的多用途负重机器人，科研勘探领域，如水下勘探机器人，地震废墟等的用于搜查的机器人，煤矿利用的机器人。如今机器人发展的特点可概括为：横向上，应用面越来越宽。由95%的工业应用扩展到更多领域的非工业应用。像做手术、采摘水果、剪枝、巷道掘进、侦查、排雷，还有空间机器人、潜海机器人。机器人应用无限制，只要能想到的，就可以去创造实现； 纵向上，机器人的种类会越来越多，像进入人体的微型机器人，已成为一个新方向，可以小到像一个米粒般大小；机器人智能化得到加强，机器人会更加聪明

1.2课程设计的要求

设计一个机器人系统，该机器人可以是轮式、足式、车型、人型，也可以是仿其他生物的，但该机器人应具备的基本功能为：能够灵活行进，能感知光源、转向光源并跟踪光源；另外还应具备一项其他功能，该功能可自选（如亮灯、按钮启动、红外接近停止等）。

具体要求如下：

1、 根据功能要求进行机械构型设计，并用实训套件搭建实物。

2、 基于实训套件选定满足功能要求的传感器；

3、 设计追光策略及运动步态；

4、 用NorthStar设计完整的机器人追光程序；

5、 调试；

6、 完成课程设计说明书，内容：方案设计、硬件搭建过程（附照片）、控制算法流程、程序编写、调试结果、心得体会。

第二章硬件设计

机器人的硬件主要包括主控板、电机以及传感器。我们需要搭建的系统就是一个能自动平衡的机器人，并不需要能够载人，所有Segway的系统对我们而言是有参考价值的，根据设计是系统要求，该方案的选材及控制如下。

2.1 结构设计

1) MultiFLEX2-AVR控制器，1块；

2) 多功能调试器和线，1套；

3) 光强传感器，2个；

4) 舵机，8个；

5) 连接件，若干。

舵机：

控制器：

2.2电机驱动

驱动系统是向机械结构系统提供动力的装置。采用的动力源不同，驱动系统的传动方式也就不同。驱动系统的传动方式主要有四种：液压式、气压式、电气式和机械式。电力驱动是目前使用最多的一种驱动方式，其特点是电源取用方便，响应快，驱动力大，信号检测、传递、处理方便，并可以采用多种灵活的控制方式，驱动电机一般采用步进电机或伺服电机，目前也有采用直接驱动电机，但是造价较高，控制也较为复杂。

电动机也称电机（俗称马达），在电路中用字母“M”（旧标准用“D”）表示。它的主要作用是产生驱动转矩，作为用电器或各种机械的动力源。电动机按结构及工作原理可分为直流电动机，异步电动机和同步电动机。同步电动机还可分为永磁同步电动机、磁阻同步电动机和磁滞同布电动机。异步电动机可分为感应电动机和交流换向器电动机。直流电动机按结构及工作原理可分为无刷直流电动机和有刷直流电动机。

舵机是遥控模型控制动作的动力来源，不同类型的遥控模型所需的舵机种类也随之不同。

舵机主要是由外壳、电路板、无核心马达、齿轮与位置检测器所构成。其工作原理是由接收机发出讯号给舵机，经由电路板上的判断转动方向，再驱动无核心马达开始转动，透过减速齿轮将动力传至摆臂，同时由位置检测器送回讯号，判断是否已经到达定位。位置检测器其实就是可变电阻，当舵机转动时电阻值也会随之改变，藉由检测电阻值便可知转动的角度。一般的伺服马达是将细铜线缠绕在三极转子上，当电流流经线圈时便会产生磁场，与转子外围的磁铁产生排斥作用，进而产生转动的作用力。

伺服电动机又称执行电动机，在自动控制系统中，用作执行元件，把所收到的电信号转换成电动机轴上的角位移或角速度输出。分为直流和交流伺服电动机两大类，其主要特点是，当信号电压为零时无自转现象，转速随着转矩的增加而匀速下降，伺服电机内部的转子是永磁铁，驱动器控制的三相电形成电磁场，转子在此磁场的作用下转动，同时电机自带的编码器反馈信号给驱动器，驱动器根据反馈值与目标值进行比较，调整转子转动的角度。伺服电机的精度决定于编码器的精度（线数）。

2.3 传感器

传感器是自动化系统和机器人技术中的关键部件，作为系统中的一个结构组成，其重要性变得越来越明显。传感器技术它综合了多方面的知识，在近几年体现尤为突出。人类具有五种感觉(视觉、听觉、触觉、嗅觉、味觉)。机器人需要通过传感器得到这些感觉信息。目前机器人只具有视觉、听觉和触觉，这些感觉是通过相应传感器得到的。

机器人传感器根据检测对象的不同可分为内部传感器和外部传感器。

a.内部传感器：用来检测机器人本身状态（如手臂间角度）的传感器。多为检测位置和角度的传感器。

b.外部传感器：用来检测机器人所处环境（如是什么物体，离物体的距离有多远等）及状况（如抓取的物体是否滑落）的传感器。具体有物体识别传感器、物体探伤传感器、接近觉传感器、距离传感器、力觉传感器，听觉传感器等。

2.3.1光强传感器

光强传感器是一种感应光的强弱并进行显示、处理的测量装置。使它可以完成多种光学实验，诸如比较光强和距离的关系；研究光的干涉、衍射、偏振；在不同光源下测量光的相对强弱；研究不同光源的明、暗变化等。常用于生物、化学实验中。光强传感器对可见光波长的光照强度很敏感，其核心元件是一只光敏电阻其输出的信号为与光强相关的模拟信号，光强传感器对可见光波长的光照强度很敏感，其核心元件是一只光敏电阻，光强传感器的外观如图2.3所示。

。

光传感器是利用光敏元件将光信号转换为电信号的传感器，它的敏感波长在可见光波长附近，包括红外线波长和紫外线波长。光传感器不只局限于对光的探测，它还可以作为探测元件组成其他传感器，对许多非电量进行检测，只要将这些非电量转换为光信号的变化即可。光传感器是目前产量最多、应用最广的传感器之一，它在自动控制和非电量电测技术中占有非常重要的地位。最简单的光敏传感器是光敏电阻，当光子冲击接合处就会产生电流。

2.3.2光强传感器原理

利用光敏元件可将光信号的特点，将柴油机燃烧室内着火燃烧时火焰光辐射信号转换为电信号而反映出柴油机的燃烧情况。下图2.4为光强传感器的原理图。

光强传感器应用

在农业生产、气象环保等工作中，光照强度是环境监测中的重要参数。尤其是在温室大棚中，光强对作物有着决定性的作用。所以光强传感器应用于室内光照度的测定，光源强度与距离间关系的研究。

2.4硬件搭建

1、安装底盘和舵机架，安装舵机时需要注意其两端并不完全对称，需要其实际情况来装配舵机，以保证轮子的对称分布。

2、安装舵机，将舵机装入舵机架中用螺丝钉固定。

3、安装橡胶轮，用LZ4连接件安装轮子，并用螺丝钉固定。

4、修改ID号，取出多功能调试器和电源，连接好左前轮舵机。

查看端口号并打开串口，“我的电脑”点右键，选择“管理”?选择“设备管理器”?选择“端口COM 和LPT”，即可看到端口号“USB Serial Port (COM1)”。

5、组装机械手，在这里组装的是一个3自由度的机械手，即手掌、腕关节和肘关节。

6、安装光敏传感器、红外接近传感器、控制器和LED灯到上底板上。

7、接线，将光敏传感器IO10~IO10，红外接近传感器接到IO9，LED灯分别接到IO0~IO3。将轮子上的4个舵机两两相连，。连接好多功能调节器，控制器和PC机，将直流稳压电源连接到控制器，搭建好的轮形机器人如下图2.5所示。

第三章 软件设计

3.1 步态设计

3.1.1步态分析：

轮式机器人前进分为五步，

第一步，前进，；

第二步，左转；

第三步，右转；

第四步，前进；

第五步，重复第一步，实现循环。

追光的步态设计：

在获取了两侧的光强数值后，便可以开始设计程序的逻辑框架。将情况分为3种：

状况一，左侧光比右侧光强；

状况二，左侧光比右侧弱，

状况三，左右侧光强近似相等。这三种情况在逻辑上较好区分，并且覆盖了所有可能出现的状态。在程序的主程序里，用三个条件来区分这三种情况。

作为左右光强比较的中间变量，对两侧光强的差值Diff进行计算：Diff=Left-Right,

三种状况的区分将围绕差值Diff进行：

状况一，左侧光强比右侧光强：Diff＞50

状况二，左侧光强比右侧光弱：Diff＜50

状况三，左侧光强与右侧光接近相等：Diff＜=50且Diff＞=50

在判断条件里，使用50这个数值，是考虑到不同的光强传感器采集同样的光照强度数据可能存在着差异，因此给出一个区间而不是一个数据

3.1.2程序逻辑图：

根据该项目的设计要求在画程序流程图时应对程序有一定的分析，分析后得到如下图3.1所示的程序逻辑图。

3.2 用NorthStar设计的程序

根据要求编写机器人追光的程序逻辑图但在编写程序时应注意舵机中参数的设计和舵机调试是ID参数的设置。

机器人追光程序如图3.2所示

实验结果分析

本次实验的机器人虽然是轮式的不是很像人但它可以通过光传感器感应光判断光的强弱，向光强的方向转弯运行，编写的程序就相当于机器人的大脑，光传感器就似与机器人的眼睛，通过光传感器的获取信息程序来处理获取的信息最终按照原先设定好的程序向光强的方向运行。

第四章 总结

通过本次课程设计我们熟练的掌握了机器人ID的设置方法，并且通过对两足机器人的动作设计使其达到行走的效果，我们进行了小组讨论等形式，进一步掌握了机器人行走的工作原理，为编写程序提供了更清晰的思路，通过本次试验，我们更加熟练的掌握了NorthSTAR软件的应用，加深了对机器人学习的兴趣。本次课程让我学到了很多东西，不仅是知识方面，还有动手方面等。随着科技的发展，机器人的应用也越来越普遍了。所以在当今对机器人的了解也是必须的。

在没接触这门课的时候，我对机器人并不是很了解，慢慢的接触之后，我明白了机器人是自动执行工作的机器装置。它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动。它的任务是协助或取代人类工作的。

在整个设计过程中，从组装到调试我们都花了很长的时间，特别是组装。在组装之前必须得想好先装哪个部件，如果安装的顺序错了。那后面的就进行不下去了。在我们一组人员的同心协力下，我们幸好没走多少弯路。至于编程，因为以前我们做过类似的机器人，所以下手比较容易，只要对几个舵机进行正确的调试就可以了。这次设计让我受益匪浅。

我想无论做什么，只要有了兴趣，并付诸于行动，付出了汗水，就会得到回报，只要努力了，就一定能学会！

第五章 参考文献

◆ 王建华，俞孟祺，李众.智能控制基础.北京：科学出版社，1998

◆ 张建民.工业机器人.北京：北京理工大学出版社，1998

◆ 蔡自兴著.机器人学.北京:清华大学出版社，2000

◆ 张福学.机器人学智能机器人传感器技术.北京：电子工业出版社，1996

◆ 吴立德，计算机视觉.上海：复旦大学出版社，1993

◆ 王耀南.智能控制系统--模糊逻辑.长沙：湖南大学出版社，1996

◆ 许大中，贺益康.电机控制.杭州：浙江大学出版社，1999

◆ （日）见仁尚志，永守重信著.直流伺服电动机.陈忠，许上明，程树康译.上海：上海科学技术出版社，1986

◆ （日）大熊繁.机器人控制.卢柏英译.北京：科学出版社，2002

◆ 胡佑德，曾乐生，马东升.伺服系统原理与设计.北京：北京理工大学出版社，1993

目录

目录..... 1

第一章绪论..... 2

第二章：硬件设计..... 3

2.1硬件介绍.... 3

2.1.1传感器介绍.... 3

2.1.2舵机介绍.... 5

2.2结构设计.... 6

2.2.1 机器人结构示意图.... 6

2.3 驱动器的选择.... 7

2.3.1驱动方式.... 7

2.3.2电动机驱动.... 8

第三章软件设计..... 9

3.1步态设计.... 10

3.2控制策略.... 16

第四章设计总结..... 17

参考文献：... 18

第一章绪论

随着社会的不断发展，各行各业的分工越来越明细，尤其是在现代化的大产业中，有的人每天就只管拧一批产品的同一个部位上的一个螺母，有的人整天就是接一个线头，就像电影《摩登时代》中演示的那样，人们感到自己在不断异化，各种职业病逐渐产生，于是人们强烈希望用某种机器代替自己工作，因此人们研制出了机器人，用以代替人们去完成那些单调、枯燥或是危险的工作。由于机器人的问世，使一部分工人失去了原来的工作，于是有人对机器人产生了敌意。“机器人上岗，人将下岗。”不仅在我国，即使在一些发达国家如美国，也有人持这种观念。其实这种担心是多余的，任何先进的机器设备，都会提高劳动生产率和产品质量，创造出更多的社会财富，也就必然提供更多的就业机会，这已被人类生产发展史所证明。任何新事物的出现都有利有弊，只不过利大于弊，很快就得到了人们的认可。比如汽车的出现，它不仅夺了一部分人力车夫、挑夫的生意，还常常出车祸，给人类生命财产带来威胁。虽然人们都看到了汽车的这些弊端，但它还是成了人们日常生活中必不可少的交通工具。英国一位著名的政治家针对关于工业机器人的这一问题说过这样一段话：“日本机器人的数量居世界首位，而失业人口最少，英国机器人数量在发达国家中最少，而失业人口居高不下”，这也从另一个侧面说明了机器人是不会抢人饭碗的。

机器人技术建立在多学科发展的基础上，具有应用领域广，技术新，学科综合与交叉性强等特点。传统的机器人技术涉及机械，电子，自动控制等学科；现代机器人技术则综合了更加广泛的学科和技术领域，如计算机技术，仿生学，生物工程，人工智能，材料，结构，微机械，信息工程，遥感等。各种各样的机器人不但已经成为现代高科技的应用载体，而且自身也发展成为一个相对独立的研究与交叉应用领域，形成了特有的理论研究和学术发展方向，具有鲜明的学科特色。

可以预见，机器人技术将会渗透到未来生活的方方面面，而且从瞬息万变的社会发展中已经可以切身的感受到----机器人的时代已经悄悄来临。

第二章：硬件设计

2.1硬件介绍

2.1.1传感器介绍

根据检测对象的不同可分为内部传感器和外部传感器。 　　

a.内部传感器：用来检测机器人本身状态（如手臂间角度）的传感器。多为检测位置和角度的传感器。 　　

b.外部传感器：用来检测机器人所处环境（如是什么物体，离物体的距离有多远等）及状况（如抓取的物体是否滑落）的传感器。具体有物体识别传感器、物体探伤传感器、接近觉传感器、距离传感器、力觉传感器，听觉传感器等。

本课题外部仅应用到了红外接近传感器，所以这里只对它进行介绍。红外接近传感器俗称光电开关。它是利用被检测无对光线的遮挡或反射有同步回路选通电路，从而选择物体有无的。光电开关将输入电流在发射器上转换为光信号射出，接受其再根据接收到的光线的强弱或有无目标物体进行探测。工作原理如下图所示。多数光电开关选用的是波长接近可见光的红外线光波型，因此也称为红外开关。

由于红外线是不可见光，红外探头体积小巧，隐蔽性非常高，所以各种规格的红外开关，红外测距传感器厂常用于安放保卫领域。在好莱坞电影里，我们常常看到金库，博物馆里有一条条红色的光线，达到门应用各种手段避开这些光线，最终盗得各种财宝。实际上红外线是不可见的，电影里采用了普通红色光线冒充红外线，以烘托紧张的故事情节，我们对此大可以笑而过，不可收到她的误导。

图2.1.1（a）红外接近传感器工作原理

光电开关可以分类如下：

1.漫反射是光电开关：它是一种集发射器和接收器于一体的传感器，当有被检测物体被检测物体经过时，物体将光电开关发射器发射的足够量的光线反射到接收器，于是光电开关便产生了开关信号。当被检测物体的表面光亮或其反光率极高时，漫反射式的光电开关是首选的检测模式。

2.经反射式的光电开关：它集发射器与接收器于一体，光电开关发射器发出的光线经过反射镜反射回接收器，当被检测物体经过且完全阻断光线时，光电开关就产生了检测开关信号。

3.对射式光电开关：它包含了在结构上相互分离且光轴相对放置的接收器和发射器，发射器发出的光线直接进入接收器，当被检测物体进入发射器与接收器之间且阻断光线时，光电开关就产生可开关信号。当检测物体为不透明时，对射式开关是最可靠的检测装置。

4.槽式光电开关：它通常采用标准的U字型结构，其发射器和接收器分别位于U型槽的两边，并形成一光轴，当被检测物体经过U型槽且阻断光轴时，光电开关就产生了开关量信号。槽式光电开关比较适合检测高速运动的物体，并且它能分辨透明与半透明物体，使用安全可靠。

5.光纤式光电开关：它采用塑料或玻璃光线传感器来引导光线，可以对距离远的被检测物体进行检测，通常光线传感器分为对射式和漫反射式。

本设计所使用的红外光电开关型号为E18-B0，规格数据为：

VCC：5V。

工作电流：小于100mA。

输出形式：NPN三极管OC输出。

封装形式：工程塑料。

图2.1.1（b）红外接近传感器

红外接近传感器是开关量传感器，接IO0~IO11的任意一个接口都可以通过NorthSTAR进行疏枝读取和编程。由于输出是开关量，只能判断在测量距离内有无障碍物，不能给出障碍物的实际距离。但是该传感器带有一个灵敏度调节旋钮，可以调节传感触发的距离。

2.1.2舵机介绍

舵机，顾名思义，是控制多面的电机。舵机的出现最早是作为遥控模型控制多面、油门等机构的动力来源，但是由于舵机具有很多优秀的特性，在制作机器人时也时常能看到它的应用。舵机最早出现在航模运动中。在航空模型中，飞行机的飞行姿态是通过调节发动机和各个控制舵面来实现的。不仅在航模飞机中，在其他的模型运动中都可以看到它的应用，如船模上用它来控制舵，车磨重用他来转向等。

一般来讲，多极主要由舵盘、减速齿轮组、位置反馈点危机、直流电机、控制电路板等几部分组成。

 舵机是一种位置伺服的驱动器，转动范围一般不能超过1800，适用于那些角度不断变化并可以保持的驱动当中，如机器人的关节、飞机的舵面等。不过也有一些特殊的舵机，转动范围可达到5圈之多，主要用于模型帆船的收帆，俗称帆舵。

2.2结构设计

机器人的机械、电气和控制结构千差万别。这次的双足追光机器人有眼利用的是红外接近传感器，手臂利用两个舵机来驱动，双足用四个舵机驱动。把手臂、双足、和眼睛固定在控制器上。

2.2.1 机器人结构示意图

图2.2.1 机器人结构示意图

 2.2.2 完成的机器人

图2.2.2 完整机器人

2.3 驱动器的选择

2.3.1驱动方式

   驱动方式有三种：液压式、气动式、电动式。

   液压驱动方式的输出力和功率更大，能构成伺服机构，常用于大型机器人关节的驱动。气动式驱动多用于开关控制和顺序控制的机器人。本次课程设计采用的是电动机驱动。

2.3.2电动机驱动

    电动机驱动可分为普通交流电动机驱动，交直流伺服电动机驱动和步进电动机驱动。中型或重型机器人。伺服电动机和步进电动机输出力矩相对小，控制性能好，可实现速度和位置的精确控制，适用于中小型机器人。交、直流伺服电动机一般用于闭环控制系统，而步进电动机则主要用于开环控制系统，一般用于速度和位置精度要求不高的场合。

直流伺服电动机结构和原理与普通直流电动机的结构和原理没有根本区别。按照励磁方式的不同，直流伺服电动机分为永磁式直流伺服电动机和电磁式直流伺服电动机。永磁式直流伺服电动机的磁极由永久磁铁制成，不需要励磁绕组和励磁电源。电磁式直流伺服电动机一般采用他励结构，磁极由励磁绕组构成，通过单独的励磁电源供电。    按照转子结构的不同，直流伺服电动机分为空心杯形转子直流伺服电动机和无槽电枢直流伺服电动机。空心杯形转子直流伺服电动机由于其力能指标较低，现在已很少采用。无槽电枢直流伺服电动机的转子是直径较小的细长型圆柱铁芯，通过耐热树脂将电枢绕组固定在铁芯上，具有散热好、力能指标高、快速性好的特点。 控制方式： 直流电动机的控制方式有两种：一种称为电枢控制，在电动机的励磁绕组上加上恒压励磁，将控制电压作用于电枢绕组来进行控制；一种称为磁场控制，在电动机的电枢绕组上施加恒压，将控制电压作用于励磁绕组来进行控制。交流伺服电机的结构

交流伺服的电动机结构主要可分为两部分，即定子部分和转子部分。其中定子的结构与旋转变压器的定子基本相同，在定子铁心中也安放着空间互成90度电角度的两相绕组。其中一组为激磁绕组，另一组为控制绕组，交流伺服电动机一种两相的交流电动机。 交流伺服电动机使用时，激磁绕组两端施加恒定的激磁电压Uf，控制绕组两端施加控制电压Uk。当定子绕组加上电压后，伺服电动机很快就会转动起来。 通入励磁绕组及控制绕组的电流在电机内产生一个旋转磁场，旋转磁场的转向决定了电机的转向，当任意一个绕组上所加的电压反相时，旋转磁场的方向就发生改变，电机的方向也发生改变。 为了在电机内形成一个圆形旋转磁场，要求激磁电压Uj和控制电压UK之间应有90度的相位差，常用的方法有： 　　

1）利用三相电源的相电压和线电压构成90度的移相 　　

2）利用三相电源的任意线电压

3）采用移相网络 　

4）在激磁相中串联电容器

（二）交流伺服电机的优良特性

1 控制精度高 　　

步进电机  步进的步距角一般为1．8。(两相)或0．72。(五相)，而交流伺服电机的精度取决于电机编码器的精度。以伺服电机为例，其编码器为l6位，驱动器每接收2 =65 536个脉冲，电机转一圈，其脉冲当量为360‘/65 536=0，0055 ；并实现了位置的闭环控制．从根本上克服了步进电机的失步问题。 　　

2 矩频特性好 　　

步进电机的输出力矩随转速的升高而下降，且在较高转速时会急剧下降，其工作转速一般在每分钟几十转到几百转。而交流伺服电机在其额定转速(一般为2000r/min或3000r/rain)以内为恒转矩输出，在额定转速以E为恒功率输出。 　　                            　　

3 加速性能好 　　步进电机空载时从静止加速到每分钟几百转，需要200―400ms：交流伺服电机的加速性能较好．

由于电枢控制的特性好，电枢控制中回路电感小，响应快，在自动控制系统中多采用电枢控制。

在会追光的双足机器人中，我们选用的是，电动机驱动。

第三章软件设计

3.1步态设计

双足机器人前进分为五步，第一步，左足前进，右手臂朝后；

第二步，左足恢复中位状态，手臂也恢复中位状态；

第三步，右足朝前，左手臂朝后；

第四步，右足和左手臂恢复中位；

第五步，重复第一步，实现循环。

追光的步态设计：

在获取了两侧的光强数值后，便可以开始设计程序的逻辑框架。将情况分为3种：

状况一，左侧光比右侧光强；状况二，左侧光比右侧弱，状况三，左右侧光强近似相等。这三种情况在逻辑上较好区分，并且覆盖了所有可能出现的状态。在程序的主程序里，用三个条件来区分这三种情况。

作为左右光强比较的中间变量，对两侧光强的差值Diff进行计算：Diff=Left-Right,

三种状况的区分将围绕差值Diff进行：

状况一，左侧光强比右侧光强：Diff>50

状况二，左侧光强比右侧光弱：Diff=-50)&&(Diff50)

        {

            MFSetServoPos(1,512,512);

            MFSetServoPos(2,512,512);

            MFSetServoPos(3,512,512);

            MFSetServoPos(4,512,512);

            MFSetServoPos(5,480,512);

            MFSetServoPos(6,512,512);

            MFSetServoPos(7,512,512);

            MFSetServoPos(8,512,512);

            MFSetServoPos(9,512,512);

            MFSetServoPos(10,512,512);

            MFServoAction();

            DelayMS(2000);

            MFSetServoPos(1,512,512);

            MFSetServoPos(2,480,512);

            MFSetServoPos(3,512,512);

            MFSetServoPos(4,512,512);

            MFSetServoPos(5,520,512);

            MFSetServoPos(6,650,512);

            MFSetServoPos(7,512,512);

            MFSetServoPos(8,512,512);

            MFSetServoPos(9,512,512);

            MFSetServoPos(10,512,512);

            MFServoAction();

            DelayMS(2000);

            //forward 1

            MFSetServoPos(1,512,512);

            MFSetServoPos(2,480,512);

            MFSetServoPos(3,600,512);

            MFSetServoPos(4,512,512);

            MFSetServoPos(5,520,512);

            MFSetServoPos(6,650,512);

            MFSetServoPos(7,512,512);

            MFSetServoPos(8,512,512);

            MFSetServoPos(9,512,512);

            MFSetServoPos(10,512,512);

            MFServoAction();

            DelayMS(2000);

            //2

            MFSetServoPos(1,512,512);

            MFSetServoPos(2,512,512);

            MFSetServoPos(3,512,512);

            MFSetServoPos(4,512,512);

            MFSetServoPos(5,480,512);

            MFSetServoPos(6,512,512);

            MFSetServoPos(7,512,512);

            MFSetServoPos(8,512,512);

            MFSetServoPos(9,512,512);

            MFSetServoPos(10,512,512);

            MFServoAction();

            DelayMS(2000);

        }

        if (Diff