基于图像处理的番茄采摘机器人的设计毕业论文1 精品-USB迷|专注于互联网分享

2024年3月27日发(作者：令荣)

青岛农业大学

毕业论文（设计）

题目: 基于图像处理的番茄采摘机器人的设计

姓名：

学院：机电工程学院

专业：电气工程及其自动化

班级：

2010.02

学号： ********

指导教师： **

2014 年 06 月 16 日

摘要 ................................................................................................................................................................... I

ABSTRACT ....................................................................................................................................................... II

1 绪论 .................................................................................................................................................................. 1

1.1

研究的背景及意义 ................................................................................................................................... 1

1.2

国外研究现状 ........................................................................................................................................... 1

1.3

国内研究现状 ........................................................................................................................................... 2

1.4

主要研究内容 ........................................................................................................................................... 3

2 采摘机器人硬件系统设计 .............................................................................................................................. 5

2.1

系统整体方案设计 ................................................................................................................................... 5

2.2

双目立体摄像机的选型 ........................................................................................................................... 7

2.3

图像处理核心芯片的选型 ....................................................................................................................... 7

2.4

下位机控制器选型与电路设计 ............................................................................................................. 13

2.5

采摘机械手自由度的降维方案和驱动设计 ......................................................................................... 18

2.6

滑台限位和采摘手接触检测和设计 ..................................................................................................... 20

3 双目视觉定位模型及摄像机参数标定 ........................................................................................................ 22

3.1

双目视觉定位模型 ................................................................................................................................. 22

3.2

摄像机标定方法 ..................................................................................................................................... 24

3.3

标定结果及分析 ..................................................................................................................................... 26

4 图像采集和预处理 ........................................................................................................................................ 29

4.1

图像采集 ................................................................................................................................................. 29

4.2

图像裁剪和二值化处理 ......................................................................................................................... 29

4.3

图像滤波处理 ......................................................................................................................................... 31

4.4

番茄果实边缘检测与轮廓提取 ............................................................................................................. 32

4.5

图像显示调试方法设计 ......................................................................................................................... 34

5 番茄果实的特征点和形心参数的提取 ........................................................................................................ 35

5.1

番茄果实圆周上特征点获取的方法设计 ............................................................................................. 35

5.2

计算番茄果实的圆心和半径的方法设计 ............................................................................................. 35

6 立体匹配和三维坐标计算 ............................................................................................................................ 37

6.1

立体匹配 ................................................................................................................................................. 37

6.2

番茄果实的空间三维坐标的计算 ......................................................................................................... 37

7 上下位机通讯与下位机采摘设计 ................................................................................................................ 39

7.1

上位机与下位机串行通讯协议设计 ..................................................................................................... 39

7.2

上位机与下位机串行通讯寄存器配置 ................................................................................................. 39

7.3

上位机和下位机串行通讯程序 ............................................................................................................. 40

7.4

下位机对番茄果实定位和采摘 ............................................................................................................. 41

8 软件开发环境配置 ........................................................................................................................................ 42

8.1

CCS开发环境配置.................................................................................................................................. 42

8.2

IAR开发环境配置 .................................................................................................................................. 45

9 样机试验和总结 ............................................................................................................................................ 47

9.1

采摘机器人样机试验 ............................................................................................................................. 47

9.2总结和展望 .............................................................................................................................................. 49

参考文献 ............................................................................................................................................................ 51

致谢 ................................................................................................................................................................ 54

附录 ................................................................................................................................................................ 55

附录1 基于OPENCV的张正友标定算法程序（部分） ............................................................................ 55

附录2 DSP 的主程序和图像采集程序（部分） .......................................................................................... 58

附录3 图像裁剪程序 ....................................................................................................................................... 61

附录4 图像阈值分割程序 ............................................................................................................................... 62

附录5 中值滤波程序 ....................................................................................................................................... 63

附录6 索贝尔边缘检测程序 ........................................................................................................................... 64

附录7 番茄果实圆周上特征点获取的程序（部分） ................................................................................... 65

附录8 计算番茄果实的圆心和半径的算法（部分） ................................................................................... 67

附录9 番茄果实的空间三维坐标定位的算法（部分） ............................................................................... 69

附录10 上位机TMS320DM642的串行通讯寄存器配置及串行通讯程序 ................................................ 73

附录11 下位机MSP430F149的串行通讯程序 ............................................................................................ 76

附录12 下位机对番茄果实定位和采摘的算法（部分） ............................................................................. 78

基于图像处理的番茄采摘机器人的设计

摘要

目前的番茄采摘基本上都是依赖于人工作业而导致劳动力成本高、劳动强度大，而现有的采摘机器

人的研究基本上都是停留在理论层面，而极个别物化的成果都是基于计算机，从而导致系统体积过大、

功耗高和成本高。为解决以上问题，本文提出并开发了一套基于DSP的番茄采摘机器人。

本文的主要设计内容包括基于DSP的采摘机器人系统的方案设计、各个硬件电路的设计、以及基

于汇编语言、C语言和VC++三种编程语言的软件设计。本文的主要贡献为：（1）提出利用DSP代替计

算机实现番茄图像的采集、处理和果实的空间三维定位，并进行了验证；（2）提出一种采摘机械手降维

的方法，解决了机器人建模复杂且实现困难的问题，并结合三维滑台实现并完成了对空间中番茄的准确

抓取和采摘的功能。

试验表明，本文研发的系统能够实现对番茄果实的准确定位和采摘，具有操作简单、体积小巧、功

耗低、性价比高等优点。本系统的研发对于提高番茄的采摘效率，减少劳动力、降低农民的劳动强度和

采摘成本具有重要的实际意义，也为精准农业的发展提供了一种新的思路和方法。

关键字：番茄采摘；图像处理；DSP；双目立体视觉

Design of Tomato Picking Robot Based on Image Processing

Abstract

At present, the tomato harvest work mostly depends on the artificial operation, which causes the problems

of high-cost, high labor intensity etc. But now, the research of picking robot mainly focuses on the theoretical

research. Few materialized productions, which use the computer as the controller, have the characteristics of

big-system volume, high power-consumption, and high cost etc. To solve the above problems, this paper

designs a tomato picking robot based on the DSP.

The main design contents of this paper include the scheme design of robot picking robot based on DSP,

the hardware circuit design and software design based on the assembler language, C language and VC++

language. The main contributions of this paper include two aspects: (1) a novel scheme is proposes that DSP is

used to take place of the computer to complete the tomato image acquisition, image process and

three-dimension localization. (2) an novel approach for reducing dimensions is proposed to reduce the picking

manipulator dimensions, which solves the difficulty in robot modeling and realization method. Combing with

the three-dimension slipway, the grasping and picking for tomatoes are finished in the space.

This experiment tests indicate that the designed system can realize the accurate positioning and picking

for tomatoes, as well as the system has many advantages of the simple operation, compact size, low power and

high cost-performance ratio. The development of this system not only has the important meanings in

improving the tomato picking efficiency, reducing human labors, reducing the labor intensity of farmers and

the picking cost, but also providing a new idea and method for the development of precision agriculture.

Key words：tomato picking, image process, DSP, binocular stereo vision

青岛农业大学机电工程学院本科毕业设计（论文）

1 绪论

1.1 研究的背景及意义

番茄(Tomato)，又名西红柿、或者洋柿子。其内部含有丰富的蛋白质、维生素，以及

胡萝卜素等营养物质。西红柿具有减肥瘦身、消除人们的疲劳等功效

[1-3]

。据不完全统计，

全世界番茄总产量约为5000万吨/年，而我国则占到了约700万吨/年

[4]

。

番茄采摘作业是当前果蔬生产过程中比较费时和费力的环节。目前，番茄采摘主要依

赖于人工作业，由果农直接将番茄从植株上采摘下来。然而人工采摘作业存在成本偏高、

劳动强度大、而且采摘很不及时等弊端。同时，当前我国人口老龄化严重，农业劳动人口

因为“进城”而骤减

[5]

。而随着自动化技术的发展，自动采摘作业逐渐代替人类进行作业，

可以大大减少采摘人员的劳动强度。因此，进行番茄采摘作业自动化的研究对于社会具有

重要的现实意义

[6]

。

然而，番茄的大小和颜色呈现非规则、非一致等特性，其生长环境的复杂性和农田环

境的非结构化等特点共同决定了采摘设备的开发有一定的难度，而且由于番茄果实生长环

境的背景复杂，加之番茄果实生长密集，果实之间的遮挡问题很严重，给图像处理带来许

多困难。虽然目前已有学者进行基于机器视觉方面的研究，但其目前的研究基本上是停留

在某一方面理论层次的研究，如单纯的双目定位、机械手采摘路径优化等，而进行实际应

用开发的研究特别少。即使这样，当前极个别的物化的应用研究都是利用了基于PC上位

机的OpenCV，即首先利用计算机视觉库进行图像处理，然后进一步通过控制采摘机械手

对果实进行采摘，但是这样就使得采摘设备存在开发成本高、体积大和功耗高等缺点，给

自动化采摘作业的推广应用带来新问题。

因此，开发一套基于DSP的低功耗、小体积、低成本的番茄采摘机器人，对提高番茄

采摘劳动生产率、降低农民的劳动强度和采摘成本，提高我国精准农业设施的现代化和智

能化水平、加快农业科学进步具有重要的现实意义。

1.2 国外研究现状

采摘机器人要实现精确的采摘，最重要的就是确定果实在空间三维坐标中的精确位

置，而果实三维空间中的位置需要利用机器视觉来完成。机器视觉已有二三十年的发展历

史，其功能及适用范围随着当今科技的快速发展而不断应用和完善。采摘机器人是农业机

青岛农业大学机电工程学院本科毕业设计（论文）

器人中一种特别重要的机器人，很多发达国家现在已经在采摘机器人领域有了较大的发

展。比如美国、荷兰、以色列等西方国家在此领域有较为成熟的发展

[7-15]

。

20世纪80年代，美国麻省理工学院的，从计算机科学的角度出发，将神经生

理学、数学以及心理物理学融为一体，提出了视觉计算理论，该理论是双目视觉的前提条

件，也为以后机器视觉采摘机器人的发展奠定了基础

[16-18]

。

1994年，英国Silsoe研究院的科学家从图像识别出发，研制出了蘑菇采摘机器人，该

机器人可以通过图像处理自动测量并判断蘑菇的大小和空间三维坐标，进而选择性的进行

采摘工作

[19-20]

。

1996年，日本冈山大学的Kondo N通过图像识别，研发了基于图像处理的番茄采摘机

器人。该机器人使用摄像头采集图像，经过图像处理后，识别出成熟的番茄，使用采摘机

械手实现对果实的准确采摘

[21-22]

。

2000年，以色列国家的科学家研发了世界上第一台甜瓜采摘机器人。该机器人利用黑

白图像处理的方法进行甜瓜的识别和空间定位。同时，该机器人还能根据甜瓜的圆形和椭

圆形等几何形状特征的特殊性来增加识别成功的概率。经过实验验证，该采摘机器人可以

自主完成大部分的甜瓜识别及采摘工作

[23]

。

1.3 国内研究现状

从国内采摘机器人的发展趋势和成就来看，尽管我国在采摘机器人的研究领域起步较

晚，较发达国家有不少差距，而且当前大部分的工作还主要集中在实验室的仿真和试验阶

段，但不少研究人员也开始也取得了一定的成果

[24-30]

。

2004年，中国农业大学的张铁中等人通过色彩空间参照表，提出了适用于水果采摘机

器人视觉系统果实目标提取的图像分割算法。通过对比试验发现，采用该算法分别对草莓

和西红柿等果实的图像在Lab、HSV、YCbCr色彩模型下进行实验，取得了理想的效果

[31-32]

。

2009年潍坊学院的宋键等人根据茄子生长的空间分布信息，采用了基于直方图的固定

阈值法实现了对灰度图像进行小区域分割，完成了对茄子果实的轮廓、质心等参数的判断。

试验证明，该方法对茄子等作物的识别率较高，而且系统本身工作稳定，但是缺点是耗时

过长

[33-34]

。

2010年河北农业大学的司永胜等人通过对不同光照情况下拍摄的苹果图像进行识别，

利用归一化的红绿色差算法获得苹果了轮廓图像。实验结果证明：该识别算法的准确识别

青岛农业大学机电工程学院本科毕业设计（论文）

率可以达到90%以上。同时，采用随机圆环法，实现了准确地提取果实的圆心和半径参数

[35-36]

。

2014年常州大学的吕继东等人为了缩短系统对图像识别的时间，提高苹果果实的识别

率，利用动态阈值分割的方法，通过改进的去均值归一化积法实现了快速跟踪目标果实，

并进行了不同阈值分割方法下果实识别的对比性试验。试验结果证明，该方法大大减少了

苹果采摘机器人采摘过程处理时间，而且识别率也较之前的方法有所改善

[37-38]

。

1.4 主要研究内容

本文主要是通过利用DSP控制双目摄像机采集图像，并经过二值化、滤波处理、索贝

尔边缘处理、形心确定、特征点匹配、三维重建等步骤实现对番茄的空间三维定位，然后

将番茄果实的空间三维坐标等参数传送至下位机，下位机进而通过控制三维滑台和采摘机

械手实现对番茄的抓取和采摘工作。

本设计的主要研究内容如下：

1、对采摘机器人的总体硬件方案进行选择和设计

系统硬件主要包括数字信号处理器型号的选择、摄像机型号的选取、控制器的选择以

及其它硬件电路的选型和设计。本设计还对三维滑台类型和长度进行选取并组装，选取合

适的驱动器及配套的驱动电源，利用控制器实现对滑台的控制。同时，需要选择合适的采

摘机械手并对采摘手的采摘头进行改装设计，选择合适的舵机驱动器并利用下位机控制器

实现对采摘机械手对果实的抓取和释放。同时，本设计还需对滑台限位传感器和采摘机械

手接触传感器进行选型和设计等。

2、获取双目摄像机的内参数和外参数

由于摄像机标定的结果是立体视觉的前提，它决定了后续番茄果实空间三维定位的精

确度。双目摄像机标定主要是通过两摄像机对外界的标定板进行拍摄若干幅图片后，通过

利用C++编写的上位机标定程序计算出摄像机各自的内外参数的过程。

3、利用DSP控制双目摄像机进行图像采集并对采集的图像进行二值化处理、边缘处

理、中值滤波等预处理操作

通过利用DSP实现对视频解码芯片和视频编码芯片的控制，从而实现对视频采集后的

输入解码和编码输出控制，然后DSP可以对采集后图像进行预处理，包括图像的阈值分割、

索贝尔边缘处理、中值滤波等操作，为后续的空间三维定位奠定基础。

青岛农业大学机电工程学院本科毕业设计（论文）

4、利用DSP实现对特征点的匹配、三维重建，并能简单的进行三维坐标的计算并将

计算结果输送至控制器

利用DSP对左右两个摄像机采集后的图像视频进行初步处理后确定番茄果实的特征

点、形心等参数，找到相对应的匹配点，并根据上述步骤中摄像机标定得到的内外参数，

利用双目定位数学模型，根据三维重建初步计算番茄的空间三维坐标，并能通过串行通讯

将其传送到下位机控制器。

5、控制器能与DSP通讯并根据接收到的数据实现对三维滑台和采摘机械手的控制，

从而实现对番茄果实的精确定位、抓取和采摘

下位机控制器MSP430F149能通过串口接收来自DSP的三维坐标信息及采摘信息，并

能将番茄果实的空间三维坐标转化为采摘机械手的空间三维坐标。同时，下位机控制器能

根据接收的番茄果实空间三维坐标数据控制三维滑台工作，使得三维滑台移动到待采摘的

番茄正前方，然后下位机控制器可以控制采摘机械手实现对番茄的抓取和采摘工作，最后

将番茄送入集果箱。

青岛农业大学机电工程学院本科毕业设计（论文）

2 采摘机器人硬件系统设计

2.1 系统整体方案设计

本设计中，基于图像处理的采摘机器人主要包括上位机模块和下位机模块。

上位机模块主要包括：TMS320DM642图像处理模块、图像采集模块、视频解码模块、

电源模块、视频编码模块、显示模块和串行通讯模块等。

下位机模块主要包括：MSP430F149主控模块、电源模块、串行通讯模块、三维滑台、

采摘机械手、传感器模块、滑台驱动器、舵机驱动器和灯光补偿模块等。

上位机和下位机主要通过RS232串行通讯模块进行数据的传输。上位机中的图像采集

模块采集待采摘区域的图像后，通过视频解码模块将视频的模拟信号转换为数字信号送入

TMS320DM642图像处理模块，TMS320DM642图像处理模块一方面通过对数字信号进行

处理和分析，实现对番茄果实进行提取轮廓、形心位置确定、立体匹配和计算番茄果实的

三维空间坐标等处理和将计算后的数字信号传输至视频编码模块，视频编码模块将处理后

的番茄果实的图像再次转换为模拟信号并送至显示器进行显示以便开发人员调试；另一方

面，TMS320DM642图像处理模块将计算出的番茄果实的空间三维坐标的数据通过串行通

讯发送至下位机MSP430F149主控模块。在这个过程中，电源模块为上位机整个子模块提

供电能。

下位机中的MSP430F149主控模块通过串行通讯口接收到来自TMS320DM642发送的

番茄果实三维空间坐标数据后提取坐标的有效值，然后通过滑台驱动器驱动三维滑台运动

至带采摘番茄果实的正前方位置，然后，MSP430F149主控模块控制舵机驱动器驱动采摘

机械手对番茄果实进行准确抓取和采摘，最后将番茄果实送入集果箱中。在整个下位机工

作过程中，电源模块为下位机的整套系统提供电能，同时碰撞传感器和触碰传感器实时检

测三维滑台是否到达端点，触碰传感器实时检测采摘机械手在对番茄果实进行抓取时，机

械手的两个手掌是否已经接触到番茄果实。灯光补偿模块能够使使TMS320DM642图像处

理模块更好的对外界的图像进行处理，减少外界光源对系统的干扰。

本设计的番茄采摘机器人的整体结构框图如图2-1。

青岛农业大学机电工程学院本科毕业设计（论文）

图像采集

视频解码

电源模块灯光补偿

电

源

模

块

TMS320DM642

图像处理模块

RS232

碰撞检测

压力检测

采摘机械手

MSP430F149

主控模块

视频编码

滑台驱动器舵机驱动器

显示器

三维滑台

上位机部分

下位机部分

图2-1 番茄采摘机器人的整体结构框图

基于图像处理的采摘机器人的设计流程图如图2-2所示。主要步骤包括上下位机的硬件

搭建、双目摄像机内外参数的标定等。其中，硬件搭建还包括DSP与MSP430F149之间的串

行通讯，数字图像采集中还包括将采集的模拟信号转换为数字信号送入DSP中等

[39]

。

采摘机器人调研和立题

上下位机硬件搭建

双目摄像机标定

数字图像采集

图像二值化处理

图像中值滤波

图像索贝尔边缘处理

图像特征点确定

番茄果实立体匹配

番茄果实三维坐标计算

采摘机械手动作和定位

番茄果实抓取和采摘

结束一次采摘工作

图2-2 基于图像处理的采摘机器人的设计流程图

青岛农业大学机电工程学院本科毕业设计（论文）

2.2 双目立体摄像机的选型

摄像机的参数决定了后续图像处理的精度，考虑到性能和价格两方面因素，本设计选

用索尼生产的MJW短枪摄像机，该摄像机采用了最新的DSP数字处理技术，CCD尺寸为

1/3英寸，有效像素PAL：720×576(440K)，NTSC：769×494(380K)，具有自动白平衡(AWB)

和增益补偿控制(AGC)功能。

由于摄像机在生产时的工艺问题，很容易造成两个摄像机的参数不同，如基线长度、

CMOS面积大小和畸变系数，因此需要在后续的软件设计中需要对其内外参数进行测定。

另外，由于该摄像机可以变焦，因此在进行图像处理前需要对摄像机的镜头进行测试，即

将摄像机的焦距调整到合适的位置，使之采集的图像清晰，便于后续图像的处理工作。

2.3 图像处理核心芯片的选型

在当前的图像处理领域，基于下位机硬件的器件主要有FPGA、ARM、DSP以及ARM

与DSP组合的平台。

方案一：FPGA（现场可编程门阵列），采用硬件逻辑描述作为开发语言，器件本身

的运行速度较快，但是对开发人员的专业知识要求过高，而且程序移植较为困难。

方案二：ARM(Advanced RISC Machines)，一般用在控制领域和嵌入式领域，比较擅

长做时序控制类、嵌入式类的工作，不适合做大容量的数字计算工作。

方案三：DSP(Digital Signal Processor)，即：数字信号处理器，它一般适用于做数字信

号处理运算方面的工作，而且实时性较好。另外数字信号处理器具有体积小、功能强、成

本低等特点，并且有的具有专用的协处理器用于图像的处理工作。DSP芯片内部采用的是

哈弗结构，也就是程序和数据分开管理的方式，从而使得程序处理效率较高。

综合以上方案，我们采用TMS320DM642作为图像处理的核心芯片。TMS320DM642

是美国TI公司专门为视频处理领域设计的芯片，它具有强大的计算能力和丰富的片内设

备，因此成为多种视频和图像处理应用的首选。

TMS320DM642芯片可以提供三种最高主频：500M、600M和720M，其相应的指令

周期为：2ns、1.67ns和1.39ns。而且，其本身就有8个处理单元，在满负荷运行时可以完

成8个指令/周期。因为使用的是C64x内核，因此其具备了128kbit的L1P高速程序缓存，

128kbit的L1D高速数据缓存，2Mkbit的L2高速联合缓存的片内外设。它具有64个独立

的EDMA通道，可以很方便的实现与外界数据的快递交换。

青岛农业大学机电工程学院本科毕业设计（论文）

图像处理核心芯片TMS320DM642主要包括时钟系统、电源供电电路、复位电路、视

频解码电路、视频编码电路以及串口通讯电路等。下面我们一一介绍：

时钟电路：

TMS320DM642有多个时钟源来满足不同的内核和外设的需求，它通过时钟芯片提供

了六个不同频率的时钟源，分别是：50M的TMS320DM642时钟，25M的以太网芯片时钟，

100M的SDRAM时钟，20M的异步通讯芯片时钟，3.57M的看门狗时钟，4.31818M的视

频解码芯片时钟，27M的视频编码芯片时钟。

TMS320DM642的内核可以工作在600MHz频率上，甚至超频后能在720MHz的频率

上工作，但是DSP的外部频率只有50MHz。因此，我们可以通过时钟锁相电路(PLL)来获

得倍频，再通过分频获得多种不同频率的时钟供DSP的片内外设使用。

TMS320DM642时钟电路图如图2-3所示：

图2-3 TMS320DM642时钟电路图

青岛农业大学机电工程学院本科毕业设计（论文）

电源供电电路：

TMS320DM642要求系统必须为其提供1.4伏和3.3伏电压值的电源。其中，CPU内

核工作在1.4伏，而且，DSP内核对供电电源的稳定性和可靠性要求很高。DSP在进行工

作时，特别是图像处理时，主频可以达到最高的720MHz，此时CPU内核消耗的能量起伏

非常大，而且随运算量变化的幅度变化急剧，很可能在短时间内达到安倍级。因此，CPU

内核对它的供电部分具有很高的要求和限制。但是因为转换效率的问题，一般选择可以承

受较大电流的开关电源。开关电源具有最大的特点是：即使外界的负载变化很大，其依旧

能输出纹波系数较小的电压，一般情况下可以满足高速DSP这种对输入电压有较高要求的

处理器。TMS320DM642的外设工作在3.3V电压，这个电压的要求相对没有那么严格，因

此可以通过开关电源或一般的稳压电路提供。

TMS320DM642对电源的具体要求如表2-1所示。

表2-1 TMS320DM642对电源的具体要求

参数

CPU核心电压CVDD(V)

外围电压DVDD(V)

参数

CPU核供电电流(mA)

外围I/O供电电流(mA)

最小值

1.36

3.14

电流值

890

210

典型值最大值

1.44

1.4

3.33.46

出现该电流值的条件

CPU核心电压CVDD为1.4V,且DM642

工作在600MHz时的时钟频率

外围电压DVDD为3.3V,且DM642工作

在600MHz时的时钟频率

TMS320DM642电源供电电路图如图2-4所示：

复位电路：

信号，DSP开始正常启动。

视频解码电路设计：

TMS320DM642复位电路图如图2-5所示：

青岛农业大学机电工程学院本科毕业设计（论文）

图2-4 TMS320DM642电源供电电路图

的复位电路较为简单：系统上电后，该芯片首先检测电源的电压，如果正常，则给出复位

号必须具备一定时间的低电平和跳变周期。本设计中选用的是TPS3823-33，该芯片所搭建

TMS320DM642不仅对电压和电流有一定要求，对复位信号也有要求。它要求复位信

图2-5 TMS320DM642复位电路图

TMS320DM642芯片最多能采集6路视频图像信号。在本设计中，我们集成了四个视

视频编码输出电路设计：

TVP5150视频解码电路图如图2-6所示：

输出格式为通用的BT656，具有封装面积小、超低功率等优点。

青岛农业大学机电工程学院本科毕业设计（论文）

图2-6 TVP5150视频解码电路图

视频，而且可以自动判断视频输入信号的制式，因此不需要我们自己去控制并转换。

芯片，这是一款高性能、低功耗的视频解码芯片，支持两路复合视频的视频输入，其视频

入到芯片的视频信号的电压调整到0-1V之间。当然，TVP5150可以采集PAL或者NTSC

芯片坏掉我们依然可以使其正常工作。本设计中视频解码芯片采用TI公司生产的TVP5150

在上述设计的电路中，一般视频电路的输入电路，为了降低功耗和噪声，需要控制输

本设计中，视频编码芯片我们采用的是菲利普公司的SAA7121，该芯片输入为标准的

AV转接头接在显示器上。与TVP5150一样，SAA7121也使用I

C总线来设置工作参数和

8位BT656数字视频数据流，输出为PAL制复合视频CVBS信号，该信号可以直接通过

频解码芯片，这样可以实现4路视频的实时采集功能，即使试验中有一路或两路视频解码

反馈状态信息，且也只能作为从设备。SAA7121视频解码输出电路图设计如图2-7所示：

串口通讯电路：

讯电路图如图2-8如下：

青岛农业大学机电工程学院本科毕业设计（论文）

图2-7 SAA7121视频解码输出电路图

因为我们选择RS232作为DSP和MSP430F149之间的通讯方式。尽管TMS320DM642上

的异步收发器，可以分别为接收和发送提供64个字节的FIFO（数据结构中的先进先出序

列），而且支持DMA方式的数据传输，使得数据通讯非常方便。TMS320DM642的串口通

能的扩展中，我们使用了TI公司的TL16C752B异步通讯收发器，该芯片包含了两路独立

视频捕获口提供的McBSP功能可以作为串口通讯口，但是因为所有的视频捕获口已经被

视频的两路输入和一路输出耗尽，因此我们只能通过其它方式扩展串行通讯接口。在该功

TMS320DM642提供了一路RS232串行通讯口，考虑到MSP430F149也有串行通讯口，

别复杂系统的需求。

2.4 下位机控制器选型与电路设计

2.4.1 下位机核心模块的选择

频偏低，功能偏弱，而且外设较少，I/O引脚偏少。

青岛农业大学机电工程学院本科毕业设计（论文）

图2-8 TMS320DM642的串口通讯电路图

信接口，因此可以很方便的与本设计中的DSP和舵机驱动器等模块连接和通讯。

很丰富，I/O口较多，非常适合复杂系统的控制。但是价格一般较高，开发难度较大。

方案二：采用MSP430系列单片机。该系列单片机内部集成了较丰富的外设，特别是

方案一：采用STC89C52系列单片机。该系列单片机价格便宜，性能稳定。但是其主

能够产生PWM波形并拥有硬件乘法器。并且其一般拥有较多的I/O口，可以满足不是特

方案三：采用TMS320F28系列DSP。该系列的DSP拥有较高的主频，而且片内外设

且超低功耗模式，在低功耗模式下，最小的工作电流仅为1.3mA。由于它有三个UART通

片机有着60K的寻址范围，同时带有16为总线寻址结构，芯片内核工作电压为3.3V。而

综合以上方案，我们选择了TI公司的MSP430F149作为下位机的核心控制器。该单

青岛农业大学机电工程学院本科毕业设计（论文）

2.4.2 下位机的功能分配及总体方案设计

在本设计中，MSP430F149承担了下位机的主要检测和控制功能，主要体现在以下几

个方面：

1．通过串行通讯接收来自TMS320DM642的三维坐标信息并将数据存入数组中；

2．根据接收到的三维坐标信息驱动三维滑台对番茄果实进行准确定位；

3．驱动采摘机械手对番茄果实进行准确抓取和采摘；

4．驱动液晶显示模块对接收到的三维坐标信息进行实时显示并显示当前的采摘区域；

5．驱动4×4按键模块，能够接收到矩阵键盘的键入信息并实现对采摘机器人的手动

控制；

6．实时检测碰撞传感器的触点闭合信息，并能对外界的信息做出反应，如：在滑台

到达顶点时，及时停止相应的滑台运动。

本设计所用到MSP430F149单片机的I/O多达40多个，每个I/O口都着自己的功能。

具体的各个引脚功能分配如表2-2所示，MSP430F149总体方案电路图设计如图2-9所示。

表2-2 MSP430F149引脚分配

I/O口名称

RST

XT2IN

XT2OUT

P4.0/TB0

P4.1/TB1

P3.0/STE0

P4.2/TB2

P4.3/TB3

P3.1/SIMO0

P4.4/TB2

P4.5/TB3

P3.2/SOMI0

P4.5/TB5

P4.6/TB6

P4.7/TB6

P5.0/STE1

P5.1/SIMO1

P5.2/SOMI1

P5.3/UCLK1

P5.4/MCLK

P3.4/UTXD0

P3.5/URXD0

功能分配

复位引脚

晶振

X滑台脉冲产生

X滑台使能控制

X滑台方向控制

Y滑台脉冲产生

Y滑台使能控制

Y滑台方向控制

Z滑台脉冲产生

Z滑台使能控制

Z滑台方向控制

4×4按键行4

4×4按键行3

4×4按键行2

4×4按键行1

4×4按键列1

4×4按键列2

4×4按键列3

4×4按键列4

采摘手串口接收

采摘手串口发送

I/O口名称

P3.6/UTXD1

P3.7/URXD1

P1.0/TACLK

P1.1/TA0

P1.2/TA1

P1.3/TA2

P1.4/SMCLK

P2.0/A0

P2.1/A1

P2.2/A2

P2.3/A3

P2.4/A4

P2.5/A5

P2.6/A6

P2.7/A7

P3.0

P3.1

P3.2

P3.3

P6.0

P6.1

P6.2

功能分配

DM642串口发送

DM642串口接收

液晶指令/数据 R/S

液晶读/写 R/W

液晶使能 E

液晶通讯方式 PSB

液晶复位RST

液晶数据DB0

液晶数据DB1

液晶数据DB2

液晶数据DB3

液晶数据DB4

液晶数据DB5

液晶数据DB6

液晶数据DB7

X滑台最左侧传感器

X滑台最右侧传感器

Y滑台最上侧传感器

Y滑台最下侧传感器

Z滑台最前侧传感器

Z滑台最后侧传感器

采摘手触碰传感器

2.4.3 下位机串行通讯电路设计

的两个舵机，以便实现采摘机械手对果实的抓取和采摘。

据。另外，它还提供了1uA的关断模式，有效地降低了系统的功耗。

芯片具有两路接收器和两路驱动器，也就是可以同时接受两路数据，同时可以发送两路数

MSP430F149与上位机TMS320DM642的通讯，主要接收来自上位机的番茄果实的三维坐

标信息并用于为采摘机械手对果实的精确定位和抓取；第二路主要用于控制采摘机械手上

MAX232串行通讯电路图如图2-10所示。

青岛农业大学机电工程学院本科毕业设计（论文）

图2-9 MSP430F149总体方案电路图

在本设计中，使用了MSP430F149其中的两路串行通讯功能，第一路主要用于

在本设计中，我们选用了MAX3232芯片作为MSP430F149的串行通讯芯片，MAX3232

青岛农业大学机电工程学院本科毕业设计（论文）

图2-10 MAX232串行通讯电路图

2.4.4 4×4矩阵键盘电路设计

在本设计的初期，当番茄采摘机器人处于死区或者番茄机器人因为某些故障而发生停

止或失控行为时，为了实现对三维滑台、采摘机械手等的手动调整，也为后期工作人员操

作提供方便，我们设计了4×4矩阵键盘。这样系统不仅可以通过自动控制，而且具有手动

微调等功能，提高了系统的实用性和安全性。本设计中的4×4矩阵键盘电路图设计如图2-11

所示。

图2-11 4×4矩阵键盘电路图

该4×4矩阵键盘的盘上一共16个数字和字母等，每个按键都有自己的功能，具体的

功能分配如表2-3所示。

青岛农业大学机电工程学院本科毕业设计（论文）

表2-3 4×4矩阵键盘功能分配表

键盘序号键码

功能分配

X方向滑台左移

X方向滑台右移

X方向滑台停止

采摘机械手张开

Y方向滑台上移

Y方向滑台下移

Y方向滑台停止

采摘机械手闭合

键盘序号键码

S10

S11

S12

S13

S14

S15

S16

功能分配

Z方向滑台前移

Z方向滑台后移

Z方向滑台停止

执行一次采摘动作

XY滑台工进

XY滑台工退

关闭串行通讯

打开串行通讯

2.4.5 LCD12864液晶显示模块电路设计

在本设计中，为了使人机信息互换更加人性化，让开发人员在程序调试过程中更加方

便。我们在控制柜上安装了一块LCD12864液晶显示屏。在系统运行中，通过12864液晶

来显示MSP430F149接收到的三维坐标信息、采摘区域等参数，提高了系统参数的可读性。

在本设计中，我们将LCD12864液晶安装在控制柜的正面，方便用户察看和读取数据。

本设计中的LCD12864液晶电路图如图2-12所示，LCD12864液晶工作时的图像如图

2-13所示。

图2-12 LCD12864液晶电路图

图2-13 LCD12864液晶工作时的图像

青岛农业大学机电工程学院本科毕业设计（论文）

2.5 采摘机械手自由度的降维方案和驱动设计

2.5.1 采摘机械手自由度的降维方案设计

采摘机械手在进行对番茄果实进行准确抓取时，如果要定位空间中的某一个三维坐标

点，实现采摘机械手在空间中的任意变换姿态，则需要至少六个自由度才能完成，即：采

摘机械手的旋转（基座）、大臂的转动（肩关节）、小臂的转动（肘关节）、手腕的上下摆

动（腕摆动关节）和左右旋转（腕旋转关节）、采摘手的张合（手爪关节）。当然在数学建

模和仿真时，只需对前四个关节进行分析即可。即使这样，由于自由度的数目问题，在进

行数学建模时仍然比较困难，仍需要一系列的正运动学方程、连杆变换、矩阵变换以及逆

运动学位姿的求解等知识。

为了降低求解和设计难度，本设计提出了一种降维方法，即利用三维滑台的方式代替

六自由度的采摘机械手，由于三维滑台中的每一维滑台决定了采摘手在三维空间中的位

置，即：X方向滑台决定了采摘手在三维空间中的x的坐标值大小，Y方向滑台决定了采

摘手在三维空间中的y的坐标值大小，Z方向滑台决定了采摘手在三维空间中的z的坐标

值大小。只要能够确定三维滑台的每一维滑台运动的增量大小，就可以很容易的计算出采

摘手在三维空间中的位置变换关系。

2.5.2 三维滑台选型、受力方向分析和措施

滑台具有重量轻、精度高、精度可逆性等优点，现在被广泛的应用在PCB雕刻机、打

印机、ATM、印刷机等设备中，它们在其中承担了非常重要的角色。滑台主要分为同步带

滑台和滚珠丝杠滑台，我们在选用时可以根据负荷大小，受载荷方向、冲击和振动大小等

情况来选择。

本设计中，X方向的水平滑台主要受垂直方向的压力，另外，由于采摘机械手进行不

平衡的运动，因此还要受到前后方向的压力。由于X方向上的同步带滑台的托盘面积狭小，

为了增强整个系统的稳定性，我们选择在平行X方向上增加一根光轴，这样大大增加了系

统的稳定性，也提高了系统的抗扰性能。在X方向的滑台上，为了提高整套系统的移动速

度和精度，我们选用了静音同步带滑台，长度为2000mm。

Y方向上的水平滑台主要受水平的压力，由于Y方向上的滑台只能竖直放置，并且竖

直与X方向上的托盘的接触截面积较小，因为我们采用安放“L”形状支架的方式增强系

统的稳定性。在Y方向的滑台上，由于采摘机器在该方向上不会大幅度移动，但是不管采

青岛农业大学机电工程学院本科毕业设计（论文）

摘机器在工作或非工作期间都需要承受垂直方向的力，并且考虑到系统的成本，我们选用

了滚珠丝杠滑台，长度为800mm。

Z方向上的滑台主要收到采摘机械手的不平衡运动的力，由于放置问题，因为我们主

要采用尽量自平衡的方法，即：将Z方向的滑台的中心点尽量放置在Y方向滑台的滑块上，

这样在大多数情况下，Z方向上的滑台的受力是均匀的，这样间接地增强了系统的自平衡

性。在Z方向的滑台上，由于采摘机器在该方向上实现带动采摘机械手的运动，因此需要

考虑运动速度，因此我们选用了同步带静音直线滑台，长度为900mm。

2.5.3 三维滑台的驱动方案设计

在本设计中，由于侧重点为系统整体的设计思路和图像处理，因此在无关紧要的模块

上，为了节省时间，并没有去深入的设计每一个模块电路。本设计中选择的滑台的步进电

机驱动器为已经开发好的成品，型号为：HYQD40-H5742。该驱动器是一款专业的两相步

进电机驱动器，可以对步进电机正反转控制，而且可以通过3位拨码开关选择步进电机的

8档细分控制，通过3位拨码开关可以选择步进电机的6档电流控制。该驱动器非常适合

驱动57、42型两相、四相混合式步进电机。

在HYQD40-H5742驱动器的控制面板上，详细的列出了信号线的接口定义，接口主要

分为三部分，分别为：信号输入端、电机绕组连接和工作电压的连接。HYQD40-H5742驱

动器的接口定义如表2-4所示。

表2-4 HYQD40-H5742驱动器的接口定义

接口种类

接口定义

CP+

CP-

U/D+

U/D-

EN+

EN-

A+

A-

B+

B-

VCC

GND

功能分配

脉冲信号输入正端

脉冲信号输入负端

电机正反转输入正端

电机正反转输入负端

电机脱机控制正端

电机脱机控制负端

连接电机绕组A+相

连接电机绕组A- 相

连接电机绕组B+相

连接电机绕组B- 相

连接直流电源正极

(10V

连接直流电源负极

信号输入端

电机绕组连接

工作电压连接

本设计中，我们采用的是共阳极接法实现步进电机驱动器的配置：分别将CP+，DIR+，

EN+连接到下位机的供电电源上，CP-端子接入脉冲输入信号，DIR-端子接入方向信号，

青岛农业大学机电工程学院本科毕业设计（论文）

EN-端子接入使能信号，由于该驱动器内部已经含有光耦隔离，因此在控制器和驱动器的

连接之间并没有再考虑安装光耦。控制器MSP430F149和HYQD40-H5742驱动器的连接

示意图如图2-14所示。

公共正

CP+

CP-

DIR+

DIR-

EN+

EN -

MSP430

控

制

器

脉冲

方向

脱机

公共地

H5742

驱

动

器

图2-14 驱动器和控制器的连接示意图

2.6 滑台限位和采摘手接触检测和设计

本设计中，当三维滑台在X、Y、Z方向上运动到最两端时，如果不加保护措施，很

容易造成步进电机堵转，轻者导致电机发热，重则烧毁电机。因此需要在每个滑台的最端

部安装传感器，实现对系统的安全保护。同样的，在采摘机械手上，由于机械手的两爪在

对番茄果实进行抓取时，采摘机器人并不知道何时接触到果实，何时停止机械爪的闭合动

作，很容易破坏番茄果实，造成采摘失败。因此也有必要在两机械爪的一侧安装传感器，

实现对番茄果实的接触感应，从而实现对番茄果实的保护。

方案一：采用接近开关。接近开关又称近接传感器，它可以侦测出物体的存在与否，

以便让控制器了解运动物体的位置和有无情况。该类型传感器用途非常广泛，最大的优点

是可以实现物体的非接触感测，缺点是检测所需的时间周期较长。

方案二：采用碰撞传感器。碰撞传感器又叫限位开关，一般安装在相对静止或者运动

的物体上。当相对运动的物体接近限位开关时，开关上方的压片可以接触到限位开关的接

点，从而引起闭合的触点断开或者断开的触点闭合，进而引起控制器对碰撞传感器检测信

号的变化。该类型的传感器价格低廉，响应周期很短，缺点是检测的运动物体必须和传感

器进行接触才能动作。

综合以上方案，由于滑台运动到端点时，需要控制器及时发送动作指令来控制滑台停

止运动，然而控制器从发出指令到滑台停止运动又需要一定的时间，而这段时间滑台还是

在向前运动的，因此很容易造成滑台步进电机和齿轮传动机构的损害。因此，我们需要考

虑响应周期较短的传感器。本设计中采用方案二中的碰撞传感器实现对滑台的限位检测。

同样道理，在采摘机械手的接触检测中，为了简单和安全起见，依旧利用的是开关式碰撞

机械手停止闭合的指令，从而有效的保护了番茄果实的完整性。

青岛农业大学机电工程学院本科毕业设计（论文）

本设计中的三维滑台和采摘机械手限位检测电路图如图2-15所示。

传感器，当番茄果实接触到碰撞传感器的弹性膜片时，控制器通过检测信号并且发出控制

图2-15 三维滑台和采摘机械手限位检测电路图

青岛农业大学机电工程学院本科毕业设计（论文）

3 双目视觉定位模型及摄像机参数标定

3.1 双目视觉定位模型

3.1.1 双目视觉定位数学模型

基于双目立体视觉定位的数学模型如图3-1所示。双目立体视觉定位系统由左右两部摄

像机组成。暂定两摄像机的光心分别放置在图中的O

，O

处，图中的L，R分别代表左

侧摄像机和右侧摄像机。在三维空间中任意一点P，坐标记作P(x,y,z)，假设该P点在左右

两摄像机成像平面中投影的点为P

点和P

点，记作P

)和P

)，这两个像点其实是

三维空间中任意一点P点的像，也叫“共轭点”。通过这两个共轭点P

和P

，分别作它们

与各自对应的摄像机的光心O

和O

的连线，两连线的交点就是三维空间中的象点P(x,y,z)，

这就是双目视觉定位原理

[40]

。

P(x,y,x)

图3-1 双目立体视觉定位数学模型

3.1.2 图像坐标系、摄像机坐标系和世界坐标系的选取

图像坐标系(Pixel coordinate system)

本设计中，双目摄像机采集的视频和图像通过模拟信号的方式经高速数字信号处理器

转换成为数字图像，并通过数字信号处理器实现处理和计算。转换后的每幅数字图像在数

字信号处理器的存储器中是以A×B的二维数组的方式进行存放的，A行B列的数字图像

中的每个元素称之为像素(pixel)，该像素的数值大小即为该图像点的灰度大小

[41]

。

如图所示，我们在采集的一幅图像上以左上角作为坐标原点定义一个二维的直角坐标

青岛农业大学机电工程学院本科毕业设计（论文）

系u，v。在图像中的随机一个像素点的坐标记作(U

)，其中U

就是该像素点在图像中

的x坐标值的大小，V

就是该像素点在图像中的y坐标值的大小。因此该像素点的坐标

)就是以像素为单位的图像坐标系。图像坐标系的表示形式如图3-2所示。

)

图3-2 图像坐标系的表示形式

一般情况下，原点O

处于图像坐标系的中心位置，也就是处于图像的中间位置，但是

可能由于摄像机横向畸变、径向畸变等某些原因，造成原点O

并不处于图像坐标系的中心

位置，而是发生一些偏离。我们假定每个像素点在x轴和y轴的物理尺寸分别是d

，d

，

那么图像中任意一个像素点在两个坐标系(x,y)和(u,v)下具有如下关系式：



uU



(3-1)





vV





将以上公式化成齐次坐标和矩阵的形式，具体公式表达如下：













































(3-2)













然后，将上述齐次坐标和矩阵的形式再次转换为逆阵的形式，公式如下：



























u









(3-3)

v

















青岛农业大学机电工程学院本科毕业设计（论文）

摄像机坐标系(Camera coordinate system)

摄像机成像的几何关系图如图3-3所示。其中，O

为摄像机本身位置的坐标系，

摄像机的光心位置为O

，摄像机的焦距大小为O

。图中，x轴平行于X

轴，y轴平行

于Y

轴。摄像机的光轴为Z

，光轴Z

和图像平面呈垂直关系，并且光轴Z

与图像平面

的交点就是图像坐标系的原点O

[42]

。

P(x,y,z)

(u,v)

)

图3-3 摄像机成像的几何关系图

世界坐标系(World coordinate system)

在三维的空间中，将摄像机安装在三维滑台上后，由于三维滑台是不断移动的，造成

摄像机的位置在空间中也是不固定的，因此我们需要选择一个基准的坐标系来描述它在世

界中的位置，我们选取的这个基准坐标系就叫做世界坐标系，它包括X

，Y

，Z

三个坐

标轴。

假设空间三维中的一点，表述为P点，那么该点在上述中的摄像机坐标系和世界坐标

系下的坐标为(x

,1)，(x

,1)。并且，该摄像机坐标系和世界坐标系还具有如下关

系：





















M





(3-4)



























3.2 摄像机标定方法

3.2.1 标定原理

摄像机标定最主要的作用就是为了确定左右两个摄像机自身的相对位置，内外参数值

青岛农业大学机电工程学院本科毕业设计（论文）

以及建立成像模型，以便能确定空间三维坐标系中的目标点与其在图像成像平面上的像点

间具有的一一对应关系。

当前的摄像机标定方法主要有：张正友标定法、自标定法、直接线性变换法等。因为

时间和精力有限，考虑到难度和精度方面，因此本设计使用了张正友提出的摄像机标定算

法

[43]

。

3.2.2 基于OpenCV的张正友标定方法

OpenCV，也就是：Open Source Computer Vision Library。它是Intel发行的基于开源的

跨平台计算机视觉库，可以运行在Linux、Windows等操作系统上。它主要由C函数和少

量C++类构成，同时提供了Python、Ruby、MATLAB等语言的接口，里面含有图像处理

和计算机视觉方面的通用算法，用户只需对其调用即可完成对图像等的处理工作。

在摄像机定标前，首先打印一张高精度的5×7棋盘格的图像，将其粘贴在一块大小合

适的KT板上，这里所说的高精度应该尽量使用高精度的打印机，因为棋盘格的精度直接

决定了双目摄像机的参数标定的精确度。本设计的摄像机标定采用自制的5×7格标定板，

标定板的实物图片如图3-4所示。

图3-4 标定板的实物图片

本设计的双目摄像机标定图例如图3-5所示，包括（a）左视图和（b）右视图。

青岛农业大学机电工程学院本科毕业设计（论文）

(a) 左摄像机 (b) 右摄像机

图3-5 双目摄像机标定图例

3.2.3 基于OpenCV的张正友标定算法程序

这里需要强调的是：由于张正友标定算法是基于2D模型的，如果棋盘摆放的不平整，

肯定会造成很大的误差，因此在进行棋盘的摆放时，倾斜角度不能太多。由于版面有限，

基于OpenCV的张正友标定算法程序见附录1。

3.3 标定结果及分析

将标定板放置在不同位置使用两摄像机分别采集20幅图像，然后利用开发的上位机

标定程序对左右两个摄像机进行内外参数标定。摄像机标定结果包含了没幅图像中的角点

坐标、摄像机内外参数等数据，标定的结果保存在.TXT文本中，如图3-6所示：

青岛农业大学机电工程学院本科毕业设计（论文）

图3-6 摄像机标定结果

其中，外参数采用第六副标定板的外参数作为其外参数，标定结果如表3-1所示：

表3-1 摄像机内外参数标定结果

项目左摄像机

右摄像机

内参数

0343.312



878.662





960.707283.27

















-0.9710340.09050490.221139









0.05529870.985484-0.160507







-0.232455-0.143629-0.961943





0395.427



879.896





966.407260.967

















-0.9673440.1252430.220364









0.09026460.982617-0.162224







-0.236851-0.137036-0.961833





外参数

(用第1幅

标定板的

外参数作

为其外参

数)





74.5146 -4.88451 321.536



-0.573308





30.3509 -10.7302 323.168



-0.545897

0.491921

畸变系数

1.27065

-0.00846964

-0.00418755

-0.000758315

-0.0021794

青岛农业大学机电工程学院本科毕业设计（论文）

上图中，在标定外参数时，采用的是第一幅标定板的外参数作为摄像机的外参数，这

样尽管对于实际的摄像机参数有一定误差，但是对于本设计的精度要求已经足够了。

图中的

为摄像机的内参数，

与

为摄像机的外参数

，

与

为摄像机的畸变系数。

青岛农业大学机电工程学院本科毕业设计（论文）

4 图像采集和预处理

4.1 图像采集

本设计中的视频采集端口使用的是TMS320DM642的VP0和VP1。视频采集的测试包

括视频采集口寄存器的配置，TVP5150的配置，EDMA的使用和I

C总线的设置。

由于TMS320DM642上已经集成了FVID模块，该模块是DSP为用户DSP/BIOS程序

提供的API函数，以实现帧图像的获取和显示。FVID模块所提供的设备驱动API函数与

其它设备驱动不同，因为具有掌握数据缓冲区，也就是Cache的的所有权，可以对Cache

全权控制读写，使用FVID的应用程序则可以按需求对缓冲区进行合理分配。

DSP的主程序和图像采集程序见附录2。本设计中的双目摄像机采集的原始图像对如

图4-1所示。

(a)左图像 (b)右图像

图4-1 双目摄像机采集的原始图像对

4.2 图像裁剪和二值化处理

4.2.1 图像裁剪方法说明

由于在前期的试验中发现，如果使用600MHz的TMS320DM642进行处理两路720×576

的图像，加上边缘处理、特征点和形心位置确定、三维重建等一系列操作后，系统的实时

性变得很差，因此我们拟采用将图像进行裁剪处理，即：对一个720×576的图像，只对其

中的280×280的中间区域进行处理，这样既能满足系统工作的要求，又能大大减少图像处

理的工作量，减轻DSP的工作负担，还能明显的提高系统工作的实时性。图像裁剪处理图

像区域示意图如图4-2所示，图像裁剪处理后液晶屏幕上图像显示如图4-3所示，图像裁

青岛农业大学机电工程学院本科毕业设计（论文）

剪程序见附录3。

整幅图像720×576

待处理区域280×280

280576

番茄果实

280

720

图4-2 图像裁剪处理图像区域示意图

图4-3 图像裁剪处理后液晶屏幕上图像显示

4.2.2 图像二值化处理方法说明

在前期的图像裁剪过程中，由于使用的是RGB的彩色图像，里面含有三个通道的颜

色，DSP需要对图像中的每一个通道分别处理和判断，使得CPU工作时非常吃力，这使

得图像处理的实时性大大降低。因此我们需要对彩色的图像进行二值化处理，也就是阈值

分割。阈值分割其实就是事先人为设定一个阈值，然后让DSP根据设定的阈值进行自动判

断，根据这个分水岭将图像中所有的像素灰度值强行设置为0或者255，这样使得图像的

特征更加明显，方便对图像后续的处理和开发。

本设计中，通过多次试验发现，将图像二值化处理的阈值设定在145左右时，效果最

为良好。本设计的图像阈值分割后的图像对如图4-4所示，图像阈值分割程序见附录4。

青岛农业大学机电工程学院本科毕业设计（论文）

(a) 左图像 (b) 右图像

图4-4 图像阈值分割后的图像对

4.3 图像滤波处理

由于摄像机在对双路视频采集时，会受到各种各样噪声的干扰从而产生噪声，这样对

后续的特征点和形心的判定、三维定位等操作影响很大，甚至会造成采摘机器人的误判。

而图像滤波的最主要的目的就是降低双目摄像机采集图像的噪声，当前图像滤波的主流方

法有：中值滤波和均值滤波。

方案一：采用中值滤波。中值滤波技术属于非全局性的平滑滤波，而且都是非线性的。

在早期，该技术主要存在于一维信号的处理中，后来随着数字信号处理技术的发展，该技

术被推广到二维空间中。基于某些因素，中值滤波不会出现线性处理造成的图像细节模糊，

而且对滤除脉冲干扰及颗粒噪声尤其有效。

方案二：采用均值滤波。均值滤波是非常典型的线性滤波算法，它的主要原理是：在

要进行滤波的图像上对所需处理的目标像素给一个特定的模板，该模板包括了它周围的临

近像素值大小，通过使用给定的目标像素为中心的四周围的8个像素点，构成一个滤波模

板，当然，该模板已经去掉了目标像素本身像素值。然后再用给定模板上的所有像素的平

均值代替原来的位置处像素的像素值。均值滤波最大的缺点就是不能较好地保护图像原来

的细节，在对图像进行去噪的同时也一起破坏了图像的细节部分，从而使得图像变得模糊，

最终导致不能很好地去除噪声点。

综合以上考虑，为了既能对电磁干扰造成的图像噪声和番茄果实背景长生的噪声起到

很好的抑制作用，又能保全大部分图像的细节部分，本设计中选择了方案一中的中值滤波

进行图像的滤波和去噪处理。

中值滤波使用一种滑动窗口，将窗口中所有点的灰度值进行增序排列，取代数中值赋

给窗口中心点。本系统一次对连续的八个像素点进行中值滤波处理，由于背景噪点的存在，

青岛农业大学机电工程学院本科毕业设计（论文）

可能经过一次中值滤波效果不太理想，此时可以对番茄果实的图像进行二次甚至多次中值

滤波处理，可以得到较为干净的图像对。当然经过中值滤波的次数越多，DSP所承担的工

作量越大，负担越重，导致系统的实时性越差。本设计中，经过一次中值滤波后的图像对

如图4-5所示，经过两次中值滤波后的图像对如图4-6所示，中值滤波程序见附录5。

(a) 左图像 (b) 右图像

图4-5 经过一次中值滤波后的图像对

(a) 左图像 (b) 右图像

图4-6 经过两次中值滤波后的图像对

4.4 番茄果实边缘检测与轮廓提取

边缘检测是番茄采摘机器人对番茄果实图像处理中最基础的一部分，地位十分重要。

边缘检测的本质就是采用检测算法，提取出待处理的物体的和背景间的交界线的过程。当

前的边缘检测的主流方法主要有：Roberts算子、Sobel算子和Canny算子。

方案一：采用Roberts算子。Roberts算子，也叫罗伯茨算子。它是一种最简单的边缘

检测算法，本质是利用局部差分算子的方法来寻找边缘，采用与对角线方向相邻的两象素

之差的值作为近似梯度幅值来实现边缘的检测。该算法检测物体的垂直边缘的效果要好于

斜向边缘，定位精度较高，但是该算子对噪声比较敏感。

青岛农业大学机电工程学院本科毕业设计（论文）

方案二：采用Sobel算子。Sobel算子的本质是一阶微分算子，它的主要原理是利用像

素邻近区域的内像素的梯度值来计算每一个像素的梯度值，然后根据之前设定的阈值大小

来比较并取舍，从而得到一个新的图像边缘轮廓。Sobel算子的算法较为简单，而且对图

像的噪声具有一定抑制作用。

方案三：采用Canny算子。Canny算子可以找到一个最优的途径，来尽可能减少真实

边缘的漏检率。它对边缘点的检测和实际边缘点的位置基本具有一致性，准确率很高。但

是一般在使用Canny 算法时，必须首先使用高斯滤波器对图像进行平滑滤波。

综合本设计的实际情况，考虑到本设计的图像中的噪声较多，而且DSP本身担任的计

算任务繁重，因此我们采用对噪声抑制效果较好、算法简单的Sobel算子实现对番茄果实

的边缘处理工作。Sobel算子的具体处理过程描述如下：

1．首先利用3×3的高斯滤波器对图像进行滤波处理；

2．然后通过对图像中的每一个像素，利用下面的公式计算梯度大小

；

其中，

Msqrt(sxsxsysy)

。

与

具体的卷积模板可以用以下矩阵表示：





101











sx



202

sy00







101







121





3．最后根据设定的阈值和求得的像素灰度作比较后取舍。即：对于图像中的每个像

素点，如果梯度值小于设定的阈值，则将该像素点的梯度强行设置为零。

本设计中经过索贝尔边缘算法处理后的图像对如图4-7所示，索贝尔边缘检测程序见

附录6。

(a) 左图像 (b) 右图像

图4-7 经过索贝尔边缘算法处理后的图像对

青岛农业大学机电工程学院本科毕业设计（论文）

4.5 图像显示调试方法设计

4.5.1 图像显示调试方法设计

由于在进行图像处理调试时，当DSP检测或者显示坐标时比较抽象，因为我们采用画

点的函数将图像处理的结果及时显示在液晶显示器上，便于程序的调试工作。这里所说的

点其实是由一个个像素组成的方形白色区域，代码见下一小节，其中DrawPoint中的x为

所画点的x坐标，y为所画点的y坐标，lenth为所画方形点的边长。本设计中图像显示调

试方法的图例如图4-8所示。

图4-8 图像显示调试方法的图例

4.5.2 图像显示调试方法的代码

void DrawPoint(int x ,int y ,int lenth)

{Int m,n;

for(m=x-(lenth/2);m

}

{for(n=y-(lenth/2);n

{*(capFrameBuf[curInput]->1 + n + 720 * m)=0xff;

}

青岛农业大学机电工程学院本科毕业设计（论文）

5 番茄果实的特征点和形心参数的提取

5.1 番茄果实圆周上特征点获取的方法设计

由于本设计的图像处理的背景考虑的较为简单，首先在较为纯净的背景下处理番茄的

图像，经过图像阈值分割、索贝尔边缘处理、中值滤波后，图像中的杂点较少，画面较为

干净，为圆上的寻点提供了有利条件。由于本设计中需要确定番茄果实轮廓在图像坐标系

中的形心位置，因此我们首先需要确定番茄果实轮廓线上的特征点，本设计中番茄果实圆

周上特征点的获取方法时根据外界环境中的光线以及前期进行的索贝尔边缘处理过程中

番茄果实轮廓线的线径来确定的，检测方法如下：

1．我们首先检测图像中的白色区域点，当DSP检测到第一个点时，这里的第一个点

需要满足直径至少是10个像素的颗粒点。当检测到符合条件的颗粒点时，记为true，否则

为false；

2．然后DSP向后推移n个像素点，这里的n要小于我们待采摘的番茄的直径大小，

紧接着再检测相同类型的颗粒点，如果检测到符合条件的颗粒点时，记为true，否则为false；

3．紧接着，DSP在第1步的基础上，向下推移n个像素点，这里的n要小于我们待

采摘的番茄的直径大小，然后再检测相同类型的颗粒点，如果检测到符合条件的颗粒点时，

记为true，否则为false；

4．最后如果函数第3步的处理结果为true时，说明改点是番茄圆形上的第一点，返

回Bool型中的true供下一步使用。

5．重复步骤1、2、3，完成剩下两个点的确定工作。

经过上述步骤可以可以确定番茄果实的圆周上不同位置处的三个特征点，并将该三点

的图像坐标参数等存入内存中，供下一步番茄果实的形心确定提供基础。

番茄果实圆周上特征点获取的程序见附录7。

5.2 计算番茄果实的圆心和半径的方法设计

由于本研究最终目的就是为了服务于番茄采摘机器人，所以除了上述特征参数以外，

还有很重要的一点，那就是提取果实的质心并计算半径。本设计中，确定番茄果实的圆心

和半径方法的示意图如图5-1所示。其中，A点、B点和C点为上述中所求的圆上的特征

点，求取圆心坐标的步骤如下：

青岛农业大学机电工程学院本科毕业设计（论文）

1．由A点和B点求得直线L

的函数式并求得直线L

的斜率k

；

2．由A点和C点求得直线L

的函数式并求得直线L

的斜率k

；

3．过点A和点B的中心点M作垂直直线L

的函数L

，此时直线L

的斜率k

4．过点A和点C的中心点N作垂直直线L

的函数L

，此时直线L

的斜率k

5．求直线L

和直线L

的交点O即为圆心的坐标；

6．求取A点到O点的直线距离即为圆的半径R。

；

图5-1 确定番茄果实圆心和半径方法的示意图

由上述中计算圆心和半径的方法，通过使用变量等方法将描述性的语言转换为C代码，

从而通过DSP实现对圆心位置和半径大小的准确快速计算。具体的计算番茄果实的圆心和

半径的算法见附录8

。

青岛农业大学机电工程学院本科毕业设计（论文）

6 立体匹配和三维坐标计算

6.1 立体匹配

立体匹配算法是立体视觉算法中比较困难的一步，它的主要原理是：首先建立一个能

量代价函数，通过此函数的最小化来估计图像中的像素点视差值。它的本质是通过找到空

间特征点在左右两个摄像机上的投影特征点。但是在实际进行立体匹配时，由于受到图像

处理背景、光照强度等因素的作用，立体匹配的算法特别复杂。当前，根据立体匹配中的

匹配基元的不同，现有的立体匹配种类主要有：区域内匹配、特征值匹配和相位值匹配三

大类

[

]

。

在本设计中，单是利用DSP对左右两个摄像机采集的番茄图像中所有点进行立体匹配

工作量已经很大，而且图像立体匹配的难度特别大，需要对一个像素点或者图像块进行识

别，这些难度和所需的精力对于一个本科生来说几乎不可能。因此，为了寻求简单和节省

时间，我们使用了在之前的步骤中已经求取出的番茄果实上的特征点和形心，利用求得的

特征点和形心作为匹配点，不仅满足了系统对精确的要求，而且也大大简化了算法难度。

6.2 番茄果实的空间三维坐标的计算

通过对摄像机标定并完成番茄果实上简单特征点的立体匹配后，下一步工作就是恢复

番茄果实的三维空间坐标信息了。当然，双目视觉立体定位的数学模型在第3节已经给出。

在该模型下，如果想要计算番茄果实的空间三维坐标，我们首先通过投影变换矩阵，然后

利用最小二乘法的方式求解

[

]

。

在上面的章节中，已经计算出的两幅图像的圆心

与

，其对应的图像坐标分别为

)

，

)

，根据摄像机成像模型可以得到以下公式：















l11



M















l21













l31



l12

l22

l32

l13

l23

l33





l14









(6-1)

l24







l34















r14









(6-2)

r24







r34

























r11

r12



M







r22rr









r21













r31

r32



r13

r23

r33

青岛农业大学机电工程学院本科毕业设计（论文）

上式中，

：

左摄像机的投影矩阵，

：右摄像机的投影矩阵，

点的坐标

(x,y,z)

即为该点的空间三维坐标值。

在上式中，我们分别消去

、

，可以得到关于

和

以及

和

的式子，如下所示：

l11

xm

l12

ym

l13

zm

l14



u



mxmymzm

l31l32l33l34



m

l21

xm

l22

ym

l23

zm

l24

v



mxmymzm



l31l32l33l34

(6-3)





u

r11

xm

r12

ym

r13

zm

r14



r31

xm

r32

ym

r33

zm

r34



v

r21

xm

r22

ym

r23

zm

r24



r31

xm

r32

ym

r33

zm

r34



化上式为矩阵形式：

APb

其中：



l31

m

l11



mvm

l21

A



l31l



r31

m

r11





r31

m

r21

l32

m

l12

l32

m

l22

r32

m

r12

r32

m

r22

l33

m

l13



l33

m

l23



，

P



r33

m

r13



r33

m

r23





l14

m

l34





mmv



l34l





，

b



l24



r14

m

r34





mmv

r34r



r24

最后，依照最小二乘法，可以解得关于坐标点

的

空间三维坐标值，求解关系式为：

P(A

1

本设计中，TMS320DM642计算出的番茄果实空间三维坐标值如图6-1所示。随后，

我们又将左右摄像机的内外参数、番茄果实形心的图像坐标等数据利用上述的公式在

MATLAB下进行了矩阵运算，验证了DSP计算出的数据和计算机计算的数据一致的事实。

图6-1 TMS320DM642计算出的番茄果实空间三维坐标值

番茄果实的空间三维坐标定位的算法见附录9。

青岛农业大学机电工程学院本科毕业设计（论文）

7 上下位机通讯与下位机采摘设计

7.1 上位机与下位机串行通讯协议设计

本设计中，上位机TMS320DM642与下位机MSP430F149之间的数据通讯采用RS232

串行异步通讯，传输时数据低位在前，高位在后。波特率设置为9600 bit/s。为保证通信电

路的畅通，提高数据传输可靠性，在进行数据传输前增加握手协议机制。具体通讯流程描

述如下：

1．DSP发送三维坐标数据前，首先发送开始标志位0x73，对应的ASCII字符为小写

字母“s”，等待MSP430回传字符，如果回传字符为“s”，则说明通信电路畅通，握手成

功；

2．DSP向下位机发送X坐标的起始标志位0x78，对应的ASCII字符为小写字母“x”；

3．DSP向下位机继续发送X坐标的坐标值，数据位数为8位；

4．DSP向下位机发送X坐标的结束标志位0x58，对应的ASCII字符为小写字母“X”；

5．重复上述步骤2、步骤3、步骤4，可以完成三维坐标中Y和Z坐标数据的发送，

只是对于Y坐标的起始标志位和结束标志位为0x79和0x59，对应的ASCII字符为“y”

和“Y”，Z坐标的起始标志位和结束标志位为0x7A和0x5A，对应的ASCII字符为“z”

和“Z”，Y坐标和Z坐标的有效数据都是八位。

6．DSP向下位机发送结束标志位0x65，对应的ASCII字符为“e”，下位机接收到后

认为DSP的发送过程结束，下位机不在接收三维坐标数据，直到下一次接收到数据开始标

志位“s”。

7.2 上位机与下位机串行通讯寄存器配置

由于DSP主要将图像处理后的数据，包括是否采摘、番茄的三维坐标等信息传输至

MSP430F149，DSP和MSP430F149之间的数据传输率较小，因此本设计采用串行通讯方

式RS232进行传输。TMS320DM642板卡中有两路UART接口，在设计中，UART的接口

标准配置为RS232，波特率为9600，数据位为8位，校验位为None，停止位为1位，采

用查询工作方式。

TMS320DM642串行通讯寄存器配置情况如下：

由于TMS320DM642使用的是TL16C752B进行串行通讯，因此在进行串行通讯时，

青岛农业大学机电工程学院本科毕业设计（论文）

系统软件设计主要包括DSP的初始化、TL16C752B的初始化设置和数据的收发三方面。

因此，在对TL16C752B寄存器和数据访问之前要对它进行初始化，包括设置波特率、设

置寄存器的状态位等一些参数。TL16C752B寄存器的初始化包括设置：先进先出(FIFO)控

制寄存器，即：FCR；线路控制寄存器，也即：LCR；除数锁存寄存器，即：DLL、DLH；

中断使能控制寄存器，即：IER。在进行读取数据时，本设计采用的是查询方式，即：判

断线状态寄存器LSR中第0位数据的值大小，如果该位为1，则说明RHR中至少存在一

个数字可以读出；反之，如果该位为0，说明RHR中不存在数字，不需要读取。在进行写

入数据是，本设计采用的依然是查询方式，即：判断线状态寄存器LSR中的第5位数据的

值大小，如果该位为1，则说明该寄存器中没有数，可以向里面写入数据；反之，如果该

位为0，说明里面已经存在数字，不能向里面写入数字。

MSP430F149串行通讯寄存器配置如下：

MSP430F149具有同步串口模式（SPI）和异步通信模式（UART）。作异步通信时，P3.4、

P3.5、P3.6和P3.7第二功能分别是UTXD0、URXD0、UTXD1和UTXD2。

MSP430F149配置寄存器设置工作模式过程如下：

1．设置单片机的I/O口的第二功能口作为串口收发引脚；

2．使能串口的收发功能；

3．选择传输时每帧数据的位数为7位或8位；

4．选择单片机的波特率发生器的时钟源；

5．配置波特率；

6．使用软件清除串口的复位位，即：SWRST。

当然，如果采用中断方式进行串行通讯，那么还需要使能接受、发送中断，包括总中

断和串行中断的开关。

7.3 上位机和下位机串行通讯程序

在进行串行通讯时，将TMS320DM642的串口和MSP430F149的串口使用串行通讯线

连接，为了保证串行通讯的同步性，上位机和下位机都是采用了9600bps的波特率，数据

位设置为8位，奇偶校验位为None，停止为设置为1位，都是采用循环查询的方式对数据

写入和读取。应当说明的是，由于系统中步进电机较多，三维滑台在工作时产生的电磁波

会对串行通讯线干扰，从而导致串行通讯的数据偶尔发生错误，解决的方法是采用带有磁

环的串行通讯线进行连接，磁环可以降低电磁辐射对通讯的干扰，增强系统通讯的稳定性。

青岛农业大学机电工程学院本科毕业设计（论文）

另外，还可以尽可能的缩短串行通讯线的长度来减少电磁波对系统串行通讯的干扰。

上位机TMS320DM642的串行通讯寄存器配置及通讯程序见附录10。

下位机MSP430F149的串行通讯寄存器配置及通讯程序见附录11。

7.4 下位机对番茄果实定位和采摘

在本设计中，系统启动后，上位机TMS320DM642控制双目摄像机对前方的待采摘区

域进行图像采集，然后将采集的图像送入DSP进行图像处理，包括：图像二值化、图像数

字滤波、索贝尔边缘处理、番茄果实形心和特征点确定、立体匹配、三维重建以及空间三

维坐标计算等操作，DSP将数据处理后的结果通过串行通信发送至下位机MSP430F149中，

下位机MSP430F149在接收到来自上位机TMS320DM642的数据后，提取番茄果实空间三

维坐标值的X坐标和Y坐标的有效值，将其转换为滑台上步进电机的脉冲个数，进而控制

三维滑台对番茄果实进行空间定位，进而通过Z坐标值大小控制采摘机械手伸长的长度，

最后控制采摘机械手对番茄果实进行抓取和采摘，并将果实送入集果箱中，完成一次采摘

工作。

在系统运行过程中，三维滑台端点处的接触传感器实时检测滑台是否已经走到端点，

如果走到端点处会将检测结果发送至MSP430F149，下位机MSP430F149进行控制滑台停

止动作，保护了系统的安全性；同时，在采摘机械手对番茄果实进行抓取过程中，传感器

实时检测采摘机械手是否接触到番茄果实，当番茄果实接触到碰撞传感器的弹性膜片时，

控制器通过检测信号并且发出控制机械手停止闭合的指令，从而有效的保护了番茄果实的

完整性。另外，系统在运行过程中，LCD12864会实时显示番茄果实的空间三维坐标值和

采摘的位置区域等信息，方便工作人员查看采摘数据。另外当番茄采摘机器人处于死区或

者番茄机器人因为某些故障而发生停止或失控行为时，工作人员可以使用矩阵键盘实现对

三维滑台、采摘机械手等的手动调整，这样系统不仅可以通过自动控制，而且具有手动微

调等功能，提高了系统的实用性和安全性。

下位机对番茄果实定位和采摘的算法见附录12。

青岛农业大学机电工程学院本科毕业设计（论文）

8 软件开发环境配置

8.1 CCS开发环境配置

8.1.1 Code Composer Studio 开发环境概述

CCS，也叫Code Composer Studio，它是美国德州仪器公司(TI)研发的代码开发和调试

套件。由于TI公司在产品线中主推数字信号处理器（DSP）和微处理器（MCU），如本设

计中用到的TMS320DM642和MSP430F149，CCS便是供用户开发和调试DSP和MCU程

序的集成开发软件。

在本设计中，我们采用的是CCS3.3，该版本的CCS支持所有的开发周期的阶段，当

然该版本的软件最大的确定就是不能在win7的操作系统中使用，现在只支持windows XP

操作系统，具体的CCS3.3所支持的开发周期如图8-1所示。同时，TI公司的CCS3.3也为

开发人员提供了一些基础性的代码生成工具，这些工具具有编写、代码分析和仿真调试功

能。

设计

概念性规划

编程和编译

创建工程文件、

编写源程序和配

置文件

调试

语法检查、探

测点设置和日

志保存等

分析

实时调试、统

计和跟踪

图8-1 CCS3.3所支持的开发周期

在进行开发设计时，必须完成下述工作：

1．首先安装板卡驱动程序。按照说明书所说的，按照步骤对办卡的驱动程序一步步

安装到合适的位置。

2．安装CCS3.3开发环境。这一步也需要遵循安装说明书进行安装。

3．运行CCS3.3开发环境并且安装程序Setup CCStudio v3.3。

CCS3.3主要由Host和Target组成，具体的CCS3.3构成及接口如图8-2所示。

青岛农业大学机电工程学院本科毕业设计（论文）

图8-2 CCS3.3构成及接口图

8.1.2 XDS510-USB2.0仿真和下载

TMS320DM642提供了在线仿真支持，该功能的研发使得CCS可以很容易地对程序进

行执行和实时监控。本设计采用的是XDS510-USB2.0仿真器，该仿真器具有USB2.0接口，

即插即用，向下兼容USB2.0全速，以及USB1.1接口。同时，该仿真器所有对外I/O都采

用了ESD保护芯片，可以避免开发人员的身体静电通过JATG间接损坏DSP芯片。该仿

真器已经可以完全克服目标板掉电后造成的系统死机现象，完美的解决了目标板掉电后不

能重起CCS开发环境的问题。最后它独特的USB供电功能，无需外接电源，携带方便，

仿真传输很快，尤其是对于没有并口的笔记本电脑来说，更加方便现场调试以及生产线使

用。

同样的，与MSP430UIF一样，在使用之前按照说明书安装驱动程序，然后进行相应

的设置，设置的前提是电脑中安装了CCS软件，用户安装了CCS开发环境后，会同时自

动安装Setup CCStudio的软件，双击打开后，运行CCS Setup程序，点击CCS Setup的窗

口中间栏Platform，选择“TDS510USB emulator”，具体的Setup CCStudio配置方式如图

8-3所示。

青岛农业大学机电工程学院本科毕业设计（论文）

图8-3 Setup CCStudio配置方式

然后点击“File/Import/Browse”,浏览到“C:CCStudio_v3.1driversimport”目录，选

择TDS510usb_。此时在左边“My System”下会出现C64xx Rec1.1

TDS510USB 2.0 Emulator，选中CPU_1，点击鼠标右键，选择Properties，出现Properties

属性对话框，如图8-4所示。

图8-4 Properties属性对话框

选择GEL文件：

定位到C:CCStudio_。点击

“OK”完成。

最后，点击“Save&Quit”按钮，保存设置，退出并启动CCS3.1。

青岛农业大学机电工程学院本科毕业设计（论文）

8.2 IAR开发环境配置

8.2.1 IAR概述

IAR EW是瑞典IAR System公司推出的一款可以编译MSP430单片机代码的嵌入式软

件， IAR EW可以开发8位，16位以及32位的处理器，在用一个集成开发环境中可对不

同的CPU应用程序进行的开发。IAR EW还为用户提供了仿真功能，用户可以通过MSP430

仿真器对程序进行监控。

IAR EW已为MSP430系列MCPU的开发提供简单而灵活的开发环境。IAR EW由于

其性能稳定可靠，广泛应用在MSP430单片机程序开发中，在科教、研发和生产，并占有

重要地位。IAR开发环境的界面如图8-5所示。

图8-5 IAR开发环境的界面

关于程序具体的编译和链接过程如下所示：

1．选中workspace中utilities.c文件。

2．选择主菜单中的Project / Compile，当程序在编译结束后，系统会在消息窗口中出

现程序的编译信息，如：0 errors，2 warnings。具体的消息窗口中的编译信息界面如图8-6

所示。应当说明的是warning并不影响程序的执行效果。

青岛农业大学机电工程学院本科毕业设计（论文）

图8-6 消息窗口中的编译信息界面

8.2.2 MSP430UIF程序烧写仿真器

MSP-FET430UIF仿真器支持MSP430 FLASH型全系列单片机的在线仿真调试和程序

下载。采用USB接口，完全兼容并口的MSP430JTAG调试工具，是性能稳定、高性价比

的MSP430系类单片机的开发工具。同时，该仿真器完全兼容TI生产的MSP-FET430UIF

开发工具；并且用户可以选择在线升级进行更新固件，当程序开发完成时，可以通过烧断

熔丝的方式对程序进行加密和保护，有利于维护成果产权。

在使用MSP430UIF仿真器前，需要先对其安装驱动程序，将MSP430UIF连接到笔记

本电脑的USB口。在进行驱动安装时应该确保笔记本电脑中已经安装了支持MSP430UIF

驱动的IAR for MSP430软件。然后，依照硬件安装向导的提示进行逐步完成所有的安装过

程。最后通过设备管理器可以查看该驱动是否已经正确安装。

安装完MSP430UIF驱动之后，在程序仿真和下载前需要进行设置，在IAR环境下，

选择“工程”/“Option”，在“Debugger”里面选择FET Debugger，这样就选择了USB FET

下载器。同样，在“Connection”里面选择Texas Instrument USB-IF，这样就选择了TI公

司的仿真器进行下载和仿真。。具体的MSP430UIF设置仿真和下载方式如图8-7所示。

图8-7 MSP430UIF设置仿真和下载方式

青岛农业大学机电工程学院本科毕业设计（论文）

9 样机试验和总结

9.1 采摘机器人样机试验

试验目的：

在完成了以上所有的步骤后，需对基于图像处理的采摘机器人进行试验，主要的目的

如下：

1．验证所搭建的DSP硬件电路、下位机MSP430F149硬件电路等是否正常工作；

2．验证三维滑台、采摘机械手等的设计是否合理，并对其进行驱动实验，验证其工

作的可靠性和稳定性；

3．通过DSP输出的模拟视频信号输出到显示器上，验证DSP对采集的双路视频并进

行图像处理后的效果是否满足后续的要求；

4．在显示器上查看DSP是否能准确定位番茄果实轮廓上的特征点以及是否能够准确

的确定番茄果实的形心坐标；

5．利用MATLAB的矩阵功能，通过输入特定的图像坐标系上点的坐标，根据双目立

体视觉定位数学模型和公式，计算出空间三维坐标值，并与DSP计算出的空间三维坐标值

作对比，验证其正确性和准确度；

6．验证TMS320DM642和下位机MSP430F149之间能否进行正常的通讯，并且机械

手能否进行对某一个番茄果实进行准确的采摘；

7．

试验三维滑台的六个端点处碰撞传感器的灵敏度，察看当滑台运动到端点时，系

统的相应运动的滑台是否进行了保护动作而停止运动；

样机试验条件：

在2014年05月24-29日，在实验室进行综合了试验。考虑到在进行设计时采摘机械

手张开的尺寸较为狭窄，仅为50mm所有；而且市场上较难买到直径小于50mm的番茄果

实。同时，为了实验的效率，提高实验的可重复性，我们采用直径为40mm的乒乓球模拟

番茄果实进行试验。

由于本设计在图像处理时仅仅考虑较为理想的情况，暂时没有考虑背景的复杂度。所

以在进行试验时，为了减少外界背景对采摘系统的干扰量，采摘区域的背景直接采用纯黑

的底色。同时，由于实验室内灯光较暗，为了提高图像处理的清晰度，加入灯光补偿模块。

最后由于室内气流较为稳定，本次试验并没有考虑有风导致番茄果实随意摆动的情

青岛农业大学机电工程学院本科毕业设计（论文）

况。即使这样我们也提出了一种较为简单的方法来抵抗外界因为有风而导致的双目立体定

位不准确的方法，即：在采摘机械手的前方位置加入挡风屏障，实现对番茄果实局部位置

的遮挡，减少外界气流对其的影响。

本次试验所用的基于图像处理的采摘机器人实物图如图9-1中的（a）和（b）所示。

(a)

(b)

图9-1 基于图像处理的采摘机器人实物图

青岛农业大学机电工程学院本科毕业设计（论文）

表9-1 采摘试验的结果统计

参数

果实数目

正确识别个数

正确采摘个数

识别时间

采摘时间

平均识别率

环境条件

弱光

3.1

68%

强光

1.6

96%

平均值

38.5

2.35

82%

由上表中的试验结果分析可知，基于图像处理的番茄采摘机器人的平均识别率达到了

82%，当然这只是在弱光和强光下的平均识别结果，在强光下的识别率96%已经可以达到

实际的应用要求；由结果知道，采摘机器人在强光下的识别率要远远大于在弱光下的识别

率，分析可能的最主要原因就是弱光不利用DSP对图像采集，而且使得DSP在后续的图

像识别时造成误差，同时延长了图像识别的时间，从而影响了后续的试验结果，因此我们

提出设想，为了进一步提高采摘机器人的识别率，可以让其尽量在晚上工作，而且加入红

外灯光补偿模块，这样采摘机器人周围的光照强度趋于一个强度，对于提高图像处理的精

准度具有很大的作用。

9.2总结和展望

本设计通过使用DSP代替原有的需要依赖OpenCV进行图像处理的计算机，利用低成

本、小体积的数字信号处理器实现了双目摄像机图像采集。同时，通过对采集的图像进行

二值化处理、滤波处理、索贝尔边缘处理、特征点和形心判定、特征点匹配、三维重建等

操作，最终实现了对番茄的空间初步三维定位和采摘。

本设计完成的主要工作有：

1．进行了系统整体的硬件方案的选择和设计；

2．通过VC实现了对双目摄像机进行内部参数的获取和校正；

3．利用DSP控制双目摄像机进行双路视频采集并对采集的图像进行了阈值分割、索

贝尔边缘处理、中值滤波等操作，并通过处理后的图像找到了形心；

4．实现了利用DSP对特征点的简单的匹配、三维重建处理，并能简单的进行三维坐

标的计算和将计算结果输送至控制器；

5．实现了DSP与控制器之间的通讯，下位机能根据通讯数据实现对三维滑台和机械

青岛农业大学机电工程学院本科毕业设计（论文）

手的控制；

6．实现了DSP和控制器能协调工作，完成对某一区域内番茄的采摘工作。

本系统在选型的时候就考虑到了农业工作环境的复杂性，并且尽量提高系统的稳定性

能和工作效率。本设计的研发虽然已经能实现简单的三维定位工作，但由于采摘环境的复

杂性、多变性以及干扰因素较多，因此本设计还具有很多不足之处，主要表现在以下几个

方面：

1．由于本设计的出发点之一就是抛弃掉原有的基于工控机的OpenCV，寻找一种相对

低成本的处理器实现对图像的处理，因此，考虑到本设计的成本问题，本设计选择了较为

低端的C6000系列DSP，而没有去选择成本较高的达芬奇系列的DSP+ARM双核处理器。

如果在试验前期不考虑成本问题，只是为了更好的试验，可以考虑更高速度的DSP，这样

系统的整体工作效率将会大大提高。另外，如果使用两块较为低端的DSP同时对图像进行

处理，之后进行信息的融合也不失为一种好的方法，这样既能提高的运行的效率有不至使

开发成本过高。

2．本设计中选用的双目摄像机是一般的720×576分辨率的彩色模拟摄像机，该摄像

机的CCD尺寸不精确，而且该摄像机的位置必须由用户自己安装固定，这样造成了两摄

像机的光轴不平行的问题，同时，由于两摄像机的制造工艺的问题，造成左右两台摄像机

的焦距及其它内部参数不一定相等，造成了误差较大。如果预算充足，可以选择购买专用

的双目视觉摄像机。最后，在摄像机标定时，采用的是自制的5×7格的标定板，由于打印

机等分辨率有限，使得标定板的规格精度有限，如果采用专业制作的标定板，可以提高摄

像机标定的精度，进而提高采摘机器人的三维定位精度。

3．在进行双目摄像机标定时，由于使用的是基于2D模型的张正友方法标定算法，如

果棋盘摆放的不平整，肯定会对标定结果造成很大的影响。而且通过算法说明手册可以发

现：标定板上的平整度的影像远远大于噪声的影响。若购买精确度能达到0.01mm的专业

标定板，将能更好的对摄像机的畸变能标定和校正，使系统单位定位的精度进一步提高。

4．本设计的最大的问题在于：因为DSP处理速度的限制以及个人能力和时间非常有

限，在进行对番茄果实图像处理时，只是使用了简单的纯色背景，并没有在较为复杂的农

业环境中试验。同时由于立体匹配算法较为复杂，本设计只是通过求得番茄果实的外形轮

廓的特征点信息作为匹配点，从而造成立体匹配的工作精度和适用面广度不够，因此这也

是下一版机器亟待解决的问题。

青岛农业大学机电工程学院本科毕业设计（论文）

参考文献

[1] 陈潭. 番茄的食疗功效[J]. 烹调知识, 2002, 11: 54.

[2] 苏蕾, 徐厚平. 番茄红素的营养保健功效[J]. 食品与药品, 2007, 07: 66-68.

[3] 冯积广. 番茄的保健功能及创新菜肴设计[J]. 四川烹饪高等专科学校学报, 2012, (4): 23-24.

[4] 程力, 韩葆颖. 我国番茄产业如何应对市场新挑战[J]. 农产品加工·综合刊, 2013, (6): 14-15.

[5] 梁勤安, 余庆辉, 冯斌等. 新疆加工番茄机械化收获发展对策的研究[J]. 新疆农业科学, 2007, 02:

225-230.

[6] 姬江涛,郑治华,杜蒙蒙等. 农业机器人的发展现状及趋势[J]. 农机化研究, 2014, 02: 1-4+9.

[7] Xiangjun Zou, Haixin Zou, Jun Lu. Virtual manipulator-based binocular stereo vision positioning system

and errors modelling. [J]. Mach. Vis. Appl., 2012, 23: 43-63.

[8] Takahashi T, Zhang Shuhuai, Fukuchi H. Measurement of 3-D locations of fruit by binocular stereo

vision for apple harvesting in an orchard[C]. CIGR 5th World Congress, Hyatt Regency Chicago,

Chicago, Illinois, USA, 2002: 1-10.

[9] Van Henten E J, Hemming J, Van Tuijl B A J, et al. Anautonomous robot for harvesting cucumbers in

greenhouses[J]. Autonomous Robots, 2002, 13: 241-258.

[10] Bulanon D M, Kataoka T, Okamoto H, et al. Determining the 3-D location of the apple fruit during

harvest[C]//Proceeding soft Automation Technology for off-Road Equipment, Kyoto, Japan, 2004: 91-97.

[11] Tanigaki K, Fujiura T, Akase A . Cherry-harvesting robot[J]. Computers and Electronics in Agriculture,

2008, 63: 65-72.

[12] He Bei, Liu Gang. Apple maturity discrimination and position[C]//IFIP International Federation for

Information Processing, 2008: 969-976.

[13] Jiménez A R, Jain A K, Ceres R. Automatic fruit recognition: A survey and new results using

Range/Attenuation images[J]. Pattern Recognition, 1999, 32(10): 1719-1736.

[14] Jiménez A R, Ceres R, Pons J L. A vision system based on alaser range -finder applied to robotic fruit

harvesting[J].Machine Vision and Applications, 2000, 11: 321-329.

[15] Lee K H, Ehsani R. Comparison of two 2D laser scanners for sensing object distances, shapes, and

surface patterns[J]. Computers and Electronics in Agriculture, 2008, 60: 250-262.

[16] Anoop Kulkarni, R K Shevgaonkar, S C Sahasrabudhe. EDGE DETECTION USING SCALE SPACE

KNOWLEDGE[A]. IEEE Beijing Section、Northern Jiaotong dings of 1993 IEEE

Region 10 Conference on “Computer,Communication,Control and Power Engineering”

(TENCON'93/Beijing) Volume 2 of 5[C]. IEEE Beijing Section、Northern Jiaotong University:,1993:5.

[17] 姚国正,汪云九. 及其视觉计算理论[J]. 国外自动化, 1984, 06: 55-57.

[18] D. Marr, T. Poggio, Ellen C. Hildreth,W. Eric L. Grimson.A computational theory of human stereo

vision[J]. Science, 1991, 263-295.

[19] NOBLER, REEDJN, MILESS, etal. Influence of mushroom strains and population density on the

performance of a robotic harvester[J]. J Agric Engng Res, 1997, 68: 215-222.

青岛农业大学机电工程学院本科毕业设计（论文）

[20] REEDJN, MILESSJ, BUTLERJ, etal. Automatic mushroom harvester development[J]. J Agric Engng Re

s, 2001, 78(1): 15-23.

[21] Kondo N, Ting K C. Robotics for plant production[J]. Artificial Intel ligence Review, 1998, 12: 227-243.

[22] Kondon N, Monta M, Ogawa Y. Cutting providing system and vision algorithm for robotic

chrysanhemum cutting sticking system[C]//Proceeding of the International Workshop on Robotics and

Automated Machinery Bioproductions, 1997, 7-12.

[23] EDANY, ROGOZIND, FLASHT, etal. Robotic melon harvesting[J]. Robotics and Automation, 2000,

16(6): 831-835.

[24] 项荣, 应义斌, 蒋焕煜, 彭永石. 基于双目立体视觉的番茄定位[J]. 农业工程学报, 2012, 05:

161-167.

[25] 李寒, 王库, 曹倩, 殷晶晶. 基于机器视觉的番茄多目标提取与匹配[J]. 农业工程学报, 2012, 05:

168-172.

[26] 赵静, 何东健. 果实形状的计算机识别方法研究[J]. 农业工程学报, 2001, 02: 165-167.

[27] 刘小勇. 番茄收获机械手机构分析及双目定位系统的研究[D]. 东北农业大学, 2006.

[28] 王沈辉. 机器人采摘番茄中的双目定位技术研究[D]. 江苏大学, 2006.

[29] 薛长松. 基于TMS320DM642的双目视觉控制系统研究[D]. 河南大学, 2007.

[30] 郑小东,赵杰文,刘木华. 基于双目立体视觉的番茄识别与定位技术[J]. 计算机工程, 2004, 22:

155-156+171.

[31] 张铁中, 林宝龙, 高锐. 水果采摘机器人视觉系统的目标提取[J]. 中国农业大学学报, 2004, 02:

68-72.

[32] 张凯良, 杨丽, 王粮局, 张丽霞, 张铁中. 高架草莓采摘机器人设计与试验[J]. 农业机械学报, 2012,

09: 165-172.

[33] 宋健, 孙学岩, 张铁中, 张宾, 徐丽明. 开放式茄子采摘机器人设计与试验[J]. 农业机械学报, 2009,

01: 143-147.

[34] 宋健. 基于神经网络的茄子采摘机器人视觉识别方法[J]. 潍坊学院学报, 2011, 06: 90-93.

[35] 司永胜, 乔军, 刘刚, 刘兆祥, 高瑞. 基于机器视觉的苹果识别和形状特征提取[J]. 农业机械学报,

2009, 08: 161-165+73.

[36] 司永胜, 乔军, 刘刚, 高瑞, 何蓓. 苹果采摘机器人果实识别与定位方法[J]. 农业机械学报, 2010,

09: 148-153.

[37] 吕继东. 苹果采摘机器人视觉测量与避障控制研究[D]. 江苏大学, 2012.

[38] 吕继东, 赵德安, 姬伟等. 苹果采摘机器人对振荡果实的快速定位采摘方法[J]. 农业工程学报,

2012, 13: 48-53.

[39] 王珂玮. 基于双目立体的目标测量[D]. 吉林大学, 2013.

[40] 张海涛. 数码相机双目定位的数学建模[J]. 数学学习与研究(教研版), 2009, 05: 106-107.

[41] 刘晶晶. 基于双目立体视觉的三维定位技术研究[D]. 华中科技大学, 2007.

[42] 任龙. 基于DM642的立体视觉测量系统研究与实现[D]. 西安电子科技大学, 2013.

[43] 蔡珲. 视觉测量中的摄像机标定与三维重建方法研究[D].哈尔滨工业大学,2013.

青岛农业大学机电工程学院本科毕业设计（论文）

[44] 佘科. 基于DSP的双目视觉算法研究与系统设计[D]. 哈尔滨工程大学, 2011.

[45] 彭永石. 基于双目立体视觉技术的果蔬采摘机器人视觉系统的研究[D]. 浙江大学生物系统工程与

食品科学学院, 2008.

青岛农业大学机电工程学院本科毕业设计（论文）

致谢

转眼间，我的大学四年生活即将结束。经过半年多的学习与努力，历经诸多坎坷，本

人的毕业设计《基于图像处理的番茄采摘机器人的设计》终于完成了。在设计的过程中除

了巩固了之前已经掌握的专业知识外，通过遇到新的问题也接触了很多新的知识，在这个

过程中喜忧参半。喜的是在设计过程中学习了许多新知识，学会了很多解决问题的新思路、

新方法。忧的是自己不会的东西太多太多，剩下的时间更需要抓紧时间为自己充电。

首先，我要特别感谢指导我的李娟老师。感谢您带领我走进了实验室，带我进入了一

个广阔的研究领域，更感谢您为我提供了科研条件。本毕业设计从设计选题，到设计方案

的确定，以及在设计过程中李老师都给出了很多指导与鼓励，尤其是当我在设计过程中遇

到方向不明确，不知下一步如何进行下去的时候，李老师不但给予指导，而且给予肯定和

鼓励，使我重新获得前进的方向与动力。而且李老师在治学态度的严谨上以及对每一个学

生认真负责的态度又给我生动的上了一课，对我产生了深刻的影响，令我在学术研究和做

人方面受益匪浅，这些都会对我在今后的工作、学习产生深远影响。

感谢大学四年来默默奉献，给予我谆谆教诲的老师们。因为你们对我的鼓励和支持让

我在困难中坚定勇气前进，越挫越勇。

感谢我的同学们，因为是你们在我生活中遇到困难时不断给与帮助和支持，让我在学

习和生活上变得更加充实和自信。

感谢我的母校，除了优美的校园环境以外，浓厚的学术氛围，还有和蔼可亲的老师，

朝气蓬勃的学生，就读于青岛农业大学我深感荣幸。祝母校越办越好。

青岛农业大学机电工程学院本科毕业设计（论文）

附录

附录1 基于OpenCV的张正友标定算法程序（部分）

/********************************************************************/

/* granted through contract.

/* Restricted rights to use, duplicate or disclose this code are */

工程名称：

功能描述：对双目摄像机进行内外参数测定

// 开发环境： VC6.0

涉及的库： OPENCV

组成文件： caliberation_

// caliberation_

#include "cvut/cvut.h"

#include

using namespace cvut;

using namespace std;

#pragma comment(lib,"")

//#define REAL_TIME_MATCHING_SHOW

void main() {

ifstream fin1("");

ofstream fout1("caliberation_");

cout<<"开始提取角点………………"<

Matrix R1(3,3);

Matrix t1(3,1);

Matrix R2(3,3);

Matrix t2(3,1);

硬件连接：无

维护记录： 2014-03-14

v1.0

2014-03-28 v1.1

/********************************************************************/

Matrix R(3,3);

Matrix t(3,1);

void func(ifstream& fin,ofstream& fout,int

SelectImageNum,Matrix&

rotation_matrix,Matrix&

translation_vector);

func(fin1,fout1,5,R1,t1);

}

void func(ifstream& fin,ofstream& fout,int

ifstream fin2("");

ofstream fout2("caliberation_");

R=R1*invert(R2);

t=t1-invert(R2)*t2;

cout<

fout2<

func(fin2,fout2,5,R2,t2);

青岛农业大学机电工程学院本科毕业设计（论文）

SelectImageNum,Matrix&

rotation_matrix,Matrix&

translation_vector){

int image_count=0;CvSize image_size;

CvSize board_size = cvSize(4,6);

CvPoint2D32f* image_points_buf = new

Seq image_points_seq;

string filename;int count= -1 ;

while (getline(fin,filename))

{image_count++;

cout<<"image_count = "<

cout<<"-->count = "<

int count;

Image view(filename);

if (image_count == 1) {

image_ = ().width;

image_ = ().height;}

board_size,

();}}

int total = image_points_();

cout<<"total = "<

for (int ii=0 ; ii

{if (0 == ii%24)

{int i = -1;i = ii/24;int j=i+1;

image_points_buf, count, 1);

("calib");cvWaitKey(300);

CvPoint2D32f[board_*board_];

cout<<"--> 第 "< : "<

}

if (0 == ii%3) {cout<

else{(10);}

cout<<" -->"<

delete []image_points_buf;

cout<<"角点提取完成！n";

/*********摄像机定标*****************/

cout<<"开始定标………………";

CvSize square_size = cvSize(16,16);

Matrixobject_points(1,board_*b

oard_*image_count,3);

Matrix

image_points(1,image_points_->total,2);

Matrix point_counts(1,image_count,1);

Matrix intrinsic_matrix(3,3,1);

Matrix distortion_coeffs(1,4,1);

Matrix

int i,j,t;

for (t=0;t

for (i=0;i

for (j=0;j

object_points(0,t*board_*board_size

cout<<"image_=

cout<<"image_="<

endl;

if(0==cvFindChessboardCorners(e,boar

d_size,image_points_buf,&count,CV_CALIB_CB_

ADAPTIVE_THRESH ))

{cout<

eight<

/*****************************************/

cout<<"can not find chessboard corners!n";

exit(1);}else

{Image view_gray((),8,1);

rgb2gray(view,view_gray);

cvFindCornerSubPix(view_e,

image_points_buf,count, cvSize(11,11),cvSize(-1,-1),

cvTermCriteria( CV_TERMCRIT_EPS+CV_TERM

CRIT_ITER, 30, 0.1 ));

image_points__back(image_points_buf,cou

nt);

cvDrawChessboardCorners( e,

rotation_vectors(1,image_count,3);

translation_vectors(1,image_count,3);

.width+i*board_+j,0)

i*square_;

object_points(0,t*board_*board_

h+i*board_+j,1) = j*square_;

青岛农业大学机电工程学院本科毕业设计（论文）

object_points(0,t*board_*board_

h+i*board_+j,2) = 0;}}}

ht;

cvCalibrateCamera2(object_,image_poi

,point_,image_size,intrinsic_

,distortion_,rotation_vecto

,translation_,0);

cout<<"定标完成！n";

for (i=0;itotal;i++)

image_points(0,i,1) = image_points_seq[i].y;}

for (i=0;i

{image_points(0,i,0) = image_points_seq[i].x;

fout<<"t第"<

cout<<"t总体平均误差：

fout<<"总体平均误差：

point_counts(0,i)=board_*board_

cout<<"评价完成！n";

/*************保存定标结果************/

cout<<"开始保存定标结果………………";

Matrix rotation_vector(3,1);

fout<<"相机内参数矩阵：n";

fout<

fout<<"畸变系数：n";

fout<

fout<<"第"<

/***********对定标结果进行评价************/

),2);

cout<<"t每幅图像的定标误差：n";

fout<<"每幅图像的定标误差：n";

for (i=0;i

cvProjectPoints2(object__cols(i*point

cout<<"开始评价定标结果………………n";

double total_err = 0.0;

double err = 0.0;

旋转向量：n";

fout<

for(j=0;j<3;j++)

{rotation_vector(j,0,0)=rotation_vectors(0,SelectIma

geNum,j);}

cvRodrigues2(rotation_,rotation_

fout<<"第"<

);

Matriximage_points2(1,point_counts(0,0,0

_counts(0,0,0),(i+1)*point_counts(0,0,0)-1).cvmat,r

otation__col(i).cvmat,translation_vectors.

get_col(i).cvmat,intrinsic_,distortion_c

,image_,0,0,0,0);

err=cvNorm(image__cols(i*point_counts(

0,0,0),(i+1)*point_counts(0,0,0)-1).cvmat,image_poi

,CV_L1);

total_err += err/=point_counts(0,0,0);

cout<<"tt第"<

旋转矩阵：n";

fout<

fout<<"第"<

平移向量：n";

for (j=0;j<3;j++) {

translation_vector(j,0,0)=translation_vectors(0,

fout<

SelectImageNum,j);}

青岛农业大学机电工程学院本科毕业设计（论文）

附录2 DSP 的主程序和图像采集程序（部分）

/********************************************************************/

/* granted through contract.

工程名称： FVID.c

功能描述：通过DSP提供的FVID接口实现视频的进程采集

开发环境： CCStudio 3.1

硬件平台： TMS320DM642

涉及的库： math.h iic.h std.h stdio.h tsk.h sem.h gio.h等

组成文件： Main.c _sa7121h.c _tvp51xx.c colorbar.c process.c等

硬件连接：见DSP模块电路设计

维护记录： 2014-02-12

2014-03-02

……

v5.8

IntnumLinesCap=VMD642_

Stop1 - VMD642_rt1 + 1;

Int numLines = (numLinesDis > numLinesCap) ?

numLinesCap : numLinesDis;

FVID_Handle capChan[2];

Int

rt1 + 1;

Int

rt1 + 1;

Int

ld1;

/* Restricted rights to use, duplicate or disclose this code are */

v1.0

v1.1

2014-05-28

/********************************************************************/

void main()

{

int ui;

CSL_init();

EMIFA_config(&g_TMS320DM642ConfigA);

CACHE_clean(CACHE_L2ALL, 0, 0);

CACHE_enableCaching(CACHE_EMIFA_CE00);

CACHE_enableCaching(CACHE_EMIFA_CE01);

DAT_open(DAT_CHAANY,

DAT_OPEN_2D);

}

void tskVideoLoopback()

{Int i;

Int frames;

Int status;

FVID_Handle disChan;

FVID_Frame *disFrameBuf;

IntnumLinesDis=EVMTMS320DM642_vDisParam

zeFld1;

DAT_PRI_LOW,

numPixels=

CACHE_setL2Mode(CACHE_256KCACHE); VMD642_op1-VMD642_v

capLinePitch=

VMD642_op1-VMD642_v

disLinePitch=

EVMTMS320DM642_zeF

GIO_Attrs gioAttrs;

memset(&gioAttrs, 0, sizeof(gioAttrs));

ts = 2;

t = 2000;

frames = 0;

numLines *= 2;

青岛农业大学机电工程学院本科毕业设计（论文）

VMD642_

EXTERNALHEAP;

EVMTMS320DM642_

VMD642_2C =

= EXTERNALHEAP;

g_hI2C;

EVMTMS320DM642_vDisParamsSAA7121.h

I2C = g_hI2C;

capChan[0] =

= VPORT_CMD_START rn");}

disChan = FVID_create("/VP0DISPLAY/A/0",

IOM_OUTPUT,&status,

(Ptr)&EVMTMS320DM642_vDisParamsChan,

NULL);

if(NULL == disChan){

printf("FAILED rn");

return;}

VPORT_CMD_EDC_BASE + EDC_CONFIG,

(Ptr)&EVMTMS320DM642_vDisParamsSAA7121)

)

{printf("VP0DISPLAY :Error in Configuring

saa7121 rn");}

if(IOM_COMPLETED != FVID_control(disChan,

VPORT_CMD_START, NULL))

{printf("VP0DISPLAY:Error

VPORT_CMD_START rn");}

if(IOM_COMPLETED

FVID_alloc(capChan[0], &capFrameBuf[0]))

{printf("VP1CAPTURE: Error in FVID_alloc

rn");}

if(IOM_COMPLETED

in FVID_alloc(capChan[1], &capFrameBuf[1]))

{printf("VP2CAPTURE: Error in FVID_alloc

rn");}

if(IOM_COMPLETED != FVID_alloc(disChan,

&disFrameBuf))

{printf("VP0DISPLAY:Error in FVID_alloc

rn");}

// frames ++;

while(1){

if (frames >= 10){

curInput = (curInput == 0) ? 1 : 0;

frames = 0;}

//printf("curInput=%d ,frames=%dt",curInput,frame

for(i=intALines;i

printf(" Video Application Started rn"); if(IOM_COMPLETED != FVID_control(disChan,

FVID_create("/VP1CAPTURE/A/1",IOM_INPUT,

&status,(Ptr)&VMD642_vCapParamsChan,&gioAtt

rs);

if(NULL == capChan[0]){

printf("FAILED rn");

return;}

if(IOM_COMPLETED!=FVID_control(capChan[0],

VPORT_CMD_EDC_BASE + EDC_CONFIG,

(Ptr)&VMD642_vCapParamsTVP5150)){

printf("VP1CAPTURE:Error

TVP5150 rn");}

if(IOM_COMPLETED!=FVID_control(capChan[0],

VPORT_CMD_START, NULL))

{printf("VP1CAPTURE:Error

VPORT_CMD_START rn");}

capChan[1]=GIO_create("/VP2CAPTURE/A/2",IO

M_INPUT,&status,(Ptr)&VMD642_vCapParamsCh

an, &gioAttrs);

if(NULL == capChan[1])

{printf("FAILED rn");

return;}

if(IOM_COMPLETED!= FVID_control(capChan[1],

VPORT_CMD_EDC_BASE + EDC_CONFIG,

(Ptr)&VMD642_vCapParamsTVP5150))

{printf("VP2CAPTURE :Error in Configuring

TVP5150 rn");}

if(IOM_COMPLETED!= FVID_control(capChan[1],

VPORT_CMD_START, NULL))

{printf("VP2CAPTURE:Error

in Configuring

{DAT_copy(capFrameBuf[curInput]->.y

青岛农业大学机电工程学院本科毕业设计（论文）

1+i*capLinePitch+220,capFrameBuf[curInput]->fra

1 + i * disLinePitch+220,280);}

IMG_sobel((const

/* Input image data */

(unsigned

*)capFrameBuf[curInput]->1,

Output image data */

numPixels, numLines /*

dimensions */

);

CACHE_wbAllL2(CACHE_WAIT);

//MedianFilterEdge();

//DrawPoint(240,240,2);

//FindSinglePoint((const

// if (frames == 10)

unsigned char

*)capFrameBuf[curInput]->1);

{FindPoint((const

Image

char

unsigned char

*)capFrameBuf[curInput]->.y1,

// printf("temp=%d ",temp++);

// printf("FstPoint x=%d ",FstPoint[0]);

// printf("FstPoint y=%d n",FstPoint[1]);

// printf("SndPoint x=%d ",SndPoint[0]);

// printf("SndPoint y=%d n",SndPoint[1]);}

CalRoundPoint(FstPoint[0],FstPoint[1],SndPoint[0],

SndPoint[1],ThirdPoint[0],ThirdPoint[1]);

drawRectangle();

CACHE_wbAllL2(CACHE_WAIT);

for(i=intALines;i

{DAT_copy(capFrameBuf[curInput]->.c

b1+i*capLinePitch+220,disFrameBuf->.

y1 + i * disLinePitch+220,280);}

DAT_wait(DAT_XFRID_WAITALL);

FVID_exchange(capChan[curInput],

&capFrameBuf[curInput]);

FVID_exchange(disChan, &disFrameBuf);

frames ++;}

} *)capFrameBuf[curInput]->1);

青岛农业大学机电工程学院本科毕业设计（论文）

附录3 图像裁剪程序

/*画矩形边框函数的定义*/

void drawRectangle()

{

Int m,n;

for(m=intALines;m

{for(n=intAPixels;n

{*(capFrameBuf[curInput]->1 + n +

720 * m)=0xff;}}

for(m=intDLines-4;m

{for(n=intAPixels;n

{

*(capFrameBuf[curInput]->1 + n +

720 * m)=0xff;}}

for(m=intALines;m

{for(n=intAPixels;n

{*(capFrameBuf[curInput]->1 + n +

720 * m)=0xff;}}

for(m=intALines;m

{for(n=intDPixels-4;n

{*(capFrameBuf[curInput]->1 + n +

720 * m)=0xff;}}

}

青岛农业大学机电工程学院本科毕业设计（论文）

附录4 图像阈值分割程序

/*****进行阈值分割处理*********/

void threshold()

{

int i,j;

for(i=intALines;i

{

for(j=intAPixels;j

{

*(Uint8 *)(tempYbuffer +

}

for(i=numLines/2+intALines;i

{

for(j=intAPixels;j

{

*)(tempYbuffer + *(Uint8

DLines;i++)

i*numPixels + j) = *(Uint8 *)(tempYbuffer +

i*numPixels + j)

}

i*numPixels + j) = *(Uint8 *)(tempYbuffer +

i*numPixels + j)

}

青岛农业大学机电工程学院本科毕业设计（论文）

附录5 中值滤波程序

void MedianFilterEdge()

{int i,j,a,b;

Uint8 aValue[9],bTemp;

for(i=intALines;i

{for(j=intAPixels;j

{aValue[0]=(*(capFrameBuf[curInput]->

.cb1 + i*720 + (j-1)));

aValue[1]=(*(capFrameBuf[curInput]->.c

b1 + i*720 + j));

aValue[2]=(*(capFrameBuf[curInput]->.c

b1 + i*720 + (j+1)));

aValue[3]=(*(capFrameBuf[curInput]->.c

b1 + (i+1)*720 + (j-1)));

aValue[4]=(*(capFrameBuf[curInput]->.c

b1 + (i+1)*720 + j));

aValue[5]=(*(capFrameBuf[curInput]->.c

b1 + (i+1)*720 + (j+1)));

aValue[6]=(*(capFrameBuf[curInput]->.c

b1 + (i+2)*720 + (j-1)));

aValue[7]=(*(capFrameBuf[curInput]->.c

b1 + (i+2)*720 + j));

aValue[8]=(*(capFrameBuf[curInput]->.c

b1 + (i+2)*720 + (j+1)));

for(b=0;b<8;b++)

{for(a=0;a<8-b;a++)

{

if(aValue[a]>aValue[a+1])

{

bTemp = aValue[a];

aValue[a] = aValue[a+1];

bTemp = aValue[4];

aValue[a+1] = bTemp;} }}

*(capFrameBuf[curInput]->1+(i+1)*7

20 + j) = bTemp;}}}

青岛农业大学机电工程学院本科毕业设计（论文）

附录6 索贝尔边缘检测程序

Bool IMG_sobel

(const unsigned char *restrict in,

/* Image dimensions */);

Bool IMG_sobel(const unsigned char *restrict in,

/* Input image data */

unsigned char *restrict out,/* Output image data */

short cols, short rows /* Image dimensions */)

{

int H, O, V, i,j;

int i00, i01, i02;

int i10, i12;

int i20, i21, i22;

//for (i = 150*720+220; i < 720*(430-2) - 222; i++)

{

i00=in[i*720 + (j-2)];

i01=in[i*720 + j];

i02=in[i*720 + (j+2)];

i10=in[(i+1)*720 + (j-2)];

i12=in[(i+1)*720 + (j+2)];

for(i=intALines-1;i

{for(j=intAPixels;j

i20=in[(i+2)*720 + (j-2)];

i21=in[(i+2)*720 + j];

H = -i00 - 2*i01-i02+i20 + 2*i21 + i22;

V = - i00 + i02 - 2*i10 + 2*i12-i20+i22;

O = abs(H) + abs(V);

/*------------------------------------------------------ */

/* Clamp to 8-bit range. The output is always

positive due to */

/* the absolute value, so we only need to check for

overflow. */

/*---------------------------------------------------------- */

if (O > 255) O = 255;

/*---------------------------------------------------------- */

/* Store it. */

/*---------------------------------------------------------- */

out[(i+1)*720 + j] = O;

*(capFrameBuf[curInput]->1

(i+1)*720 + j) = O; }}

return TRUE;

}

unsigned char *restrict out,short cols, short rows i22=in[(i+2)*720 + (j+2)];

青岛农业大学机电工程学院本科毕业设计（论文）

附录7 番茄果实圆周上特征点获取的程序（部分）

Bool CalFstPoint(const unsigned char *restrict in,

int i ,int j )

{int p,q;

int i00, i01 ,i02;

int i10, i11 ,i12;

int i20, i21 ,i22;

int j1,j2;

i00=in[i*720 + (j-2) ];

i01=in[i*720 + j ];

i02=in[i*720 + j+2 ];

i10=in[(i+1)*720 + (j-2)];

i11=in[(i+1)*720 + j];

i12=in[(i+1)*720 + j+2];

i20=in[(i+2)*720 + (j-2)];

i21=in[(i+2)*720 + j];

i22=in[(i+2)*720 + j+2];

if( ((i00==255)&&(i01==255)&&(i02==255)

&&(i10==255)&&(i11==255)&&(i12==255)&&(i2

0==255)&&(i21==255)&&(i22==255))==1)

{p=1;

j1=i+1;

j2=j;

}

else

p=0;

if(p==1){

for(j=j+65; j<495; j++)

{

i00=in[i*720 + (j-2)];

i01=in[i*720 + j];

i02=in[i*720 + j+2];

i10=in[(i+1)*720 + (j-2)];

i11=in[(i+1)*720 + j];

i12=in[(i+1)*720 + j+2];

i20=in[(i+2)*720 + (j-2)];

i21=in[(i+2)*720 + j];

i22=in[(i+2)*720 + j+2];

if(((i00==255)&&(i01==255)&&(i02==255)&

&(i10==255)&&(i11==255)&&(i12==255)&&(i20

==255)&&(i21==255)&&(i22==255))==1 )

{

FstPoint[0]=j1;

FstPoint[1]=j2;

SndPoint[0]=i+1;

SndPoint[1]=j;

// DrawPoint(i,((j1+j2)/2),2);

DrawPoint(FstPoint[0],FstPoint[1],14);

DrawPoint(SndPoint[0],SndPoint[1],14);

j=495;

q=1;}

else

q=0;}}

else

q=0;

if(q==1)

return TRUE ;

else

return FALSE;}

Bool CalSecndPoint(const unsigned char *restrict in,

int i ,int j )

{int r,s;

int i00, i01 ,i02;

int i10, i11 ,i12;

int i20, i21 ,i22;

int j3,j4;

i00=in[i*720 + (j-2) ];

i01=in[i*720 + j ];

i02=in[i*720 + j+2 ];

i10=in[(i+1)*720 + (j-2)];

i11=in[(i+1)*720 + j];

i12=in[(i+1)*720 + j+2];

i20=in[(i+2)*720 + (j-2)];

i21=in[(i+2)*720 + j];

i22=in[(i+2)*720 + j+2];

if(((i00==255)&&(i01==255)&&(i02==255)&

&(i10==255)&&(i11==255)&&(i12==255)&&(i20

青岛农业大学机电工程学院本科毕业设计（论文）

==255)&&(i21==255)&&(i22==255))==1 )

{j3=i+1;

j4=j;

r=1;}

else

r=0;

if(r==1){

for(j=j+60; j<495; j++){

i00=in[i*720 + (j-2) ];

i01=in[i*720 + j ];

i02=in[i*720 + j+2 ];

i10=in[(i+1)*720 + (j-2)];

i11=in[(i+1)*720 + j];

i12=in[(i+1)*720 + j+2];

i20=in[(i+2)*720 + (j-2)];

i21=in[(i+2)*720 + j];

i22=in[(i+2)*720 + j+2];

if( ((i00==255)&&(i01==255)&&(i02==255)

&&(i10==255)&&(i11==255)&&(i12==255)&&(i2

0==255)&&(i21==255)&&(i22==255))==1 )

{ThirdPoint[0]=j3;

ThirdPoint[1]=j4;

FouthPoint[0]=i+1;

FouthPoint[1]=j;

DrawPoint(ThirdPoint[0],ThirdPoint[1],14);

DrawPoint(FouthPoint[0],FouthPoint[1],14);

// DrawPoint(i,((j3+j4)/2),2);

j=495;

s=1;}

else s=0;}}

else

s=0;

}

else ;

} ;break;

default : ;

}

return TRUE ;

char

*)capFrameBuf[curInput]->1,i,j)))

{ // j=j+100;

x++;

i=i+40;

j=intDPixels;}

else ;};

break;

case 1 : { if(TRUE==(CalSecndPoint((const

unsigned

{// i=i+50;

j=intDPixels; //stop j

//x++;

i=intDLines; // stop i

char

*)capFrameBuf[curInput]->1,i,j)))

if(s==1)return TRUE;

else return FALSE;}

Bool FindPoint(const unsigned char *restrict in)

{int i,j;

int x=0;

for(i=intALines+5;i

for(j=intAPixels+5;j

{switch (x)

{

case 0 : { if(TRUE==(CalFstPoint((const unsigned

青岛农业大学机电工程学院本科毕业设计（论文）

附录8 计算番茄果实的圆心和半径的算法（部分）

void CalRoundPoint(int x1,int y1,int x2,int y2,int YY1=YY1/10;

x3,int y3)

{int X1,Y1,X2,Y2;

float A1,B1,A2,B2;

float temp_x,temp_y;

float radius1,radius2;

int i;

x1=FstPoint[1];

y1=-FstPoint[0];

x2=SndPoint[1];

y2=-SndPoint[0];

x3=ThirdPoint[1];

y3=-ThirdPoint[0];

if(curInput==0)

{X1=(x1+x2)/2;

A1=(y3-y1)/(x3-x1);

B1=y1-A1*x1;

//Y1=A1*X1 + B1;

temp_x = (x1+x3)/2;

temp_y = (y1+y3)/2;

A2 = -(1/A1);

B2= temp_y -(temp_x*A2);

X2=X1;

Y2=A2*X1 + B2;

RoundPoint1[0]=-Y2;

RoundPoint1[1]=X2;

cout1_temp[cout1][0]=RoundPoint1[0];

cout1_temp[cout1][1]=RoundPoint1[1];

XX1=XX1+cout1_temp[cout1][0];

YY1=YY1+cout1_temp[cout1][1];

cout1++;

// printf("cout1=%dn",cout1);

if(cout1==10)

{

aa1=FstPoint[0];

bb1=FstPoint[1];

printf("1(%d,%d)",aa1,bb1);

XX1=XX1/10;

// printf("X1=%f Y1=%f n",XX1,YY1);

cout1=0;

// printf("1:(%d,%d) cout1=%dn",x1,y1,cout1);

}else

{;}

radius1=sqrt((X2-x1)*(X2-x1)+(-Y2+y1)*(-Y2+y1))

;

//printf("x1:%d,y1:%d; x2:%d,y2:%d;

x3:%d,y3:%dn",x1,y1,x2,y2,x3,y3);

//printf("X1:%d,Y1:%d; X2:%d;Y2:%d

n",X1,Y1,X2,Y2);

//printf("point1_x=%d,

point1_y=%dn",RoundPoint1[0],RoundPoint1[1]);

//printf("radius1=%f n",radius1);

//printf("X1=%d

Y1=%dn",RoundPoint1[0],RoundPoint1[1]);

DrawPoint(RoundPoint1[0],RoundPoint1[1],4);

}

else if(curInput==1)

{X1=(x1+x2)/2;

A1=(y3-y1)/(x3-x1);

B1=y1-A1*x1;

//Y1=A1*X1 + B1;

temp_x = (x1+x3)/2;

temp_y = (y1+y3)/2;

A2 = -(1/A1);

B2= temp_y -(temp_x*A2);

X2=X1;

Y2=A2*X1 + B2;

RoundPoint2[0]=-Y2;

RoundPoint2[1]=X2;

DrawPoint(RoundPoint2[0],RoundPoint2[1],4);

cout2_temp[cout2][0]=RoundPoint2[0];

cout2_temp[cout2][1]=RoundPoint2[1];

XX2=XX2+cout2_temp[cout2][0];

YY2=YY2+cout2_temp[cout2][1];

cout2++;

青岛农业大学机电工程学院本科毕业设计（论文）

// printf("cout2=%dn",cout2);

if(cout2==10)

{

aa2=FstPoint[0];

bb2=FstPoint[1];

printf("2(%d,%d)",aa2,bb2);

XX2=XX2/10;

YY2=YY2/10;

// printf("X2=%f Y2=%f n",XX2,YY2);

cout2=0;

// printf("2:(%d,%d)n",x1,y1);

Matrix_Cal();}

//printf("point2_x=%d,

point2_y=%dn",RoundPoint2[0],RoundPoint2[1]);

radius2

sqrt((X2-x1)*(X2-x1)+(-Y2+y1)*(-Y2+y1));

//printf("x1:%d,y1:%d;

//printf("X1:%d,Y1:%d;

n",X1,Y1,X2,Y2);

//printf("point1_x=%d,

point1_y=%dn",RoundPoint1[0],RoundPoint1[1]);

//printf("radius2=%f n",radius2);

//printf("X2=%d

Y2=%dn",RoundPoint2[0],RoundPoint2[1]);

}

// printf("A1=%f , B1=%f ",A1,B1);

// printf("A2=%f , B2=%f ",A2,B2);

//printf("temp_x=%f,temp_y=%fn",temp_x,temp_y)

;

}

x2:%d,y2:%d;

X2:%d;Y2:%d

x3:%d,y3:%dn",x1,y1,x2,y2,x3,y3);

青岛农业大学机电工程学院本科毕业设计（论文）

附录9 番茄果实的空间三维坐标定位的算法（部分）

/********************************************************************/

/* granted through contract.

工程名称： Main.c

功能描述：通过DSP的初步处理的数据，实现对果实的三维坐标计算

/* Restricted rights to use, duplicate or disclose this code are */

// 开发环境： CCStudio 3.1

涉及的库： math.h iic.h std.h stdio.h tsk.h sem.h gio.h等

组成文件： Main.c _sa7121h.c _tvp51xx.c colorbar.c process.c等

硬件连接：见DSP模块电路设计

维护记录： 2014-02-12

2014-03-02

……

v5.8

3610.375000},

{44.468365,979.871765,-345.328888,26307.972656

{0.196683,-0.094478,-0.975905,273.920258}};

loat u1=0;float v1=0;

float u2=0;float v2=0;

float A_trix[4][3];

float A_trix_T[3][4];

float B_trix[3][3]; // B_trix[3][3]=A' * A

float B_trix_value=0;

float B_trix_companion[3][3];

float B_trix_reverse[3][3];

float C_trix[3][4];

float b_trix[4][1];

float P_trix[3][1];

int ii,jj;

int mm=3;

int nn=4;

int kk=3;

int mm1=3;

int nn1=3;

v1.0

v1.1

2014-05-28

/********************************************************************/

void Matrix_Cal()

{ int m,n;

// float

M1[3][4]={{850.673218,111.298965,-323.157654,1

09535.554688},

//{16.517513,-825.572815,-504.847748,154946.625

//{-0.005411,0.280092,-0.959958,341.971649}};

// float

M2[3][4]={{684.152710,113.808945,-379.625061,4

8733.3125},

//{35.938122,-647.116943,-528.031067,127223.343

75},

//{0.017379,0.221989,-0.974894,268.562927}};

float

M1[3][4]={{827.169678,89.847015,-509.181824,40

468.425781},{-17.405102,-961.893188,-382.435974

,95255.5000},{-0.197770,0.105675,-0.974536,284.8

30719}};

float

M2[3][4]={{-822.958374,-59.660130,-564.898987,9

青岛农业大学机电工程学院本科毕业设计（论文）

int kk1=4;

int max[3]={0};

int i;

char x_start=0x78; //'x'

char x_end =0x58; //'X'

char y_start=0x79; //'y'

char y_end =0x59; //'Y'

char z_start=0x7A; //'z'

char z_end =0x5A; //'Z'

char start_flag = 0x73;

char end_flag = 0x65;

cc1=XX1;

dd1=YY1;

cc2=XX2;

dd2=YY2;

L1= sqrt ((cc1-aa1) * (cc1-aa1)+(dd1-bb1) *

(dd1-bb1) ) ;

L2=sqrt ((cc2-aa2) * (cc2-aa2)+(dd2-bb2) *

(dd2-bb2) ) ;

/***************************************/

//printf("(%d,%d)(%d,%d);",aa1,bb1,cc1,dd1);

//printf("(%d,%d)(%d,%d)",aa2,bb2,cc2,dd2);

a1=XX1;

b1=YY1;

a2=XX2;

b2=YY2;

u1=95;

v1=152;

u2=98;

v2=253;

//printf("X1=%f Y1=%f , X2=%f

Y2=%fn",u1,v1,u2,v2);

printf("3(%d,%d)4(%d,%d)",b1,a1,b2,a2);

printf("L1=(%d);L2=(%d)n",L1,L2);

A_trix[0][0]=M1[2][0]

u1-M1[0][0];A_trix[0][1]=M1[2][1]

A_trix[1][0]=M1[2][0]

v1-M1[1][0];A_trix[1][1]=M1[2][1]

v1-M1[1][1];A_trix[1][2]=M1[2][2] * v1-M1[1][2];

A_trix[2][0]=M2[2][0]

u2-M2[0][0];A_trix[2][1]=M2[2][1]

A_trix[3][0]=M2[2][0]

v2-M2[1][0];A_trix[3][1]=M2[2][1]

A_trix_T[0][0]=A_trix[0][0];

A_trix_T[0][1]=A_trix[1][0];

A_trix_T[0][2]=A_trix[2][0];

A_trix_T[0][3]=A_trix[3][0];

A_trix_T[1][0]=A_trix[0][1];

A_trix_T[1][1]=A_trix[1][1];

A_trix_T[1][2]=A_trix[2][1];

A_trix_T[1][3]=A_trix[3][1];

A_trix_T[2][0]=A_trix[0][2];

A_trix_T[2][1]=A_trix[1][2];

A_trix_T[2][2]=A_trix[2][2];

A_trix_T[2][3]=A_trix[3][2];

// printf("nA_trix=n");

// for(m=0;m<4;m++)

// for(n=0;n<3;n++)

{

}

// printf("nA_trix_T=n");

//for(m=0;m<3;m++)

// for(n=0;n<4;n++)

{

}

matrix_mpys(A_trix_T,A_trix,B_trix,mm,nn,kk,max

);

matrix_mpyc(A_trix_T,A_trix,B_trix,max);

//B_trix=A' * A

// printf("nB_trix=n");

printf("%f",A_trix_T[m][n]);

printf("n");

printf("%f",A_trix[m][n]);

printf("n");

u2-M2[0][1];A_trix[2][2]=M2[2][2] * u2-M2[0][2];

v2-M2[1][1];A_trix[3][2]=M2[2][2] * v2-M2[1][2];

u1-M1[0][1];A_trix[0][2]=M1[2][2] * u1-M1[0][2];

青岛农业大学机电工程学院本科毕业设计（论文）

// for(ii=0;ii

for(jj=0;jj

{

printf("%f ",B_trix[ii][jj]);

}

printf("nThe

B_trix_value

maximum number is

// printf("nB_trix_companion=n");

// for(ii=0;ii<3;ii++){

for(jj=0;jj<3;jj++)

{

printf("%f ",B_trix_companion[ii][jj]);

}

// printf("n");

B_trix_reverse[0][0]=(1/B_trix_value)*B_trix_com

panion[0][0];

B_trix_reverse[0][1]=(1/B_trix_value)*B_trix_com

panion[0][1];

B_trix_reverse[0][2]=(1/B_trix_value)*B_trix_com

panion[0][2];

B_trix_reverse[1][0]=(1/B_trix_value)*B_trix_com

panion[1][0];

B_trix_reverse[1][1]=(1/B_trix_value)*B_trix_com

panion[1][1];

B_trix_reverse[1][2]=(1/B_trix_value)*B_trix_com

panion[1][2];

B_trix_reverse[2][0]=(1/B_trix_value)*B_trix_com

panion[2][0];

B_trix_reverse[2][1]=(1/B_trix_value)*B_trix_com

panion[2][1];

B_trix_reverse[2][2]=(1/B_trix_value)*B_trix_com

panion[2][2];

// printf("nB_trix_reverse=n");

// for(ii=0;ii<3;ii++){

for(jj=0;jj<3;jj++)

{

printf("%f ",B_trix_reverse[ii][jj]);

}

B_trix[%d][%d]=%dn",max[1],max[2],max[0]);

B_trix[0][0]*B_trix[1][1]*B_trix[2][2]+B_trix[0][1]

*B_trix[1][2]*B_trix[2][0]

B_trix[0][2]*B_trix[1][0]*B_trix[2][1]-B_trix[0][0]

*B_trix[1][2]*B_trix[2][1]

B_trix[0][1]*B_trix[1][0]*B_trix[2][2]-B_trix[0][2]

*B_trix[1][1]*B_trix[2][0];

// printf("nB_trix_value=n");

// printf("%f ",B_trix_value);

// printf("n");

B_trix_companion[0][0]=(1 )*((B_trix[1][1]*B_trix[

2][2])-(B_trix[1][2]*B_trix[2][1])) ;

B_trix_companion[0][1]=(-1)*((B_trix[0][1]*B_trix

[2][2])-(B_trix[0][2]*B_trix[2][1])) ;

B_trix_companion[0][2]=(1 )*((B_trix[0][1]*B_trix[

1][2])-(B_trix[0][2]*B_trix[1][1])) ;

B_trix_companion[1][0]=(-1)*((B_trix[1][0]*B_trix

[2][2])-(B_trix[1][2]*B_trix[2][0])) ;

B_trix_companion[1][1]=(1 )*((B_trix[0][0]*B_trix[

2][2])-(B_trix[0][2]*B_trix[2][0])) ;

B_trix_companion[1][2]=(-1)*((B_trix[0][0]*B_trix

[1][2])-(B_trix[0][2]*B_trix[1][0])) ;

B_trix_companion[2][0]=(1 )*((B_trix[1][0]*B_trix[

2][1])-(B_trix[1][1]*B_trix[2][0])) ;

B_trix_companion[2][1]=(-1)*((B_trix[0][0]*B_trix

[2][1])-(B_trix[0][1]*B_trix[2][0])) ;

B_trix_companion[2][2]=(1 )*((B_trix[0][0]*B_trix[

1][1])-(B_trix[0][1]*B_trix[1][0])) ;

// printf("n");

matrix_mpyc1(B_trix_reverse,A_trix_T,C_trix,max);

//B_trix=A' * A

// printf("nC_trix=n");

// for(ii=0;ii<3;ii++)

// {

青岛农业大学机电工程学院本科毕业设计（论文）

for(jj=0;jj<4;jj++)

{

printf("%f ",C_trix[ii][jj]);

}

P_trix[2][0]=L2;

printf("P_trix= ");

for(ii=0;ii<3;ii++)

{printf("%f ",P_trix[ii][0]);}

printf("n");

Conv_float_to_char(bb1,aa1,L2);

P_trix[0][0]=bb1;

P_trix[1][0]=aa1;

P_trix[2][0]=L2;

Uart_SendFeature(start_flag);

Uart_SendFeature(x_start);

Uart_Send( x );

Uart_SendFeature(x_end);

// printf("n");

b_trix[0][0]=M1[0][3]-M1[2][3]*u1;

b_trix[1][0]=M1[1][3]-M1[2][3]*v1;

b_trix[2][0]=M2[0][3]-M2[2][3]*u2;

b_trix[3][0]=M2[1][3]-M2[2][3]*v2;

//printf("nb_trix=n");

// for(ii=0;ii<4;ii++)

// {

// printf("%f ",b_trix[ii][0]);

//}

matrix_mpyc2(C_trix,b_trix,P_trix,max);

//B_trix=A' * A

Conv_float_to_char(P_trix[0][0],P_trix[1][0],P_trix[

2][0]);

/****************************************/

P_trix[0][0]=bb1;

P_trix[1][0]=aa1;

Uart_SendFeature(y_start);

Uart_Send( y );

Uart_SendFeature(y_end);

Uart_SendFeature(z_start);

Uart_Send( z );

Uart_SendFeature(z_end);

Uart_SendFeature(end_flag);

Uart_SendFeature(0x0d);

}

青岛农业大学机电工程学院本科毕业设计（论文）

附录10 上位机TMS320DM642的串行通讯寄存器配置及串行通讯程序

Void

VMD642_UART_rset(VMD642_UART_Handle

hUart,Int16 regnum,Int16 regval)

{Int16 regindex, lcr;

/* Register index is determined by lower 3 bits

and the target UART */

regindex = regnum & 0x7;

if (hUart == 1) regindex += 8;

/* If regnum between 0x08 and 0x0F, set bit 7

of LCR to access register */

if ((regnum & 0x18) == 0x8)

{ lcr = VMD642_UART_rget(hUart,

VMD642_UART_LCR);

VMD642_waitusec(1);

VMD642_UART_rset(hUart,

VMD642_UART_LCR, lcr | 0x80);

VMD642_waitusec(1);

VMD642_rset(regindex, regval);

VMD642_waitusec(1);

VMD642_UART_rset(hUart,

VMD642_UART_LCR, lcr);

VMD642_waitusec(1);}

else{VMD642_rset(regindex, regval);

VMD642_waitusec(1);}}

/********************************/

/* ===VMD642_UART_rget =====*/

/*Get the value of a UART register*/

/********************************/

Int16

VMD642_UART_rget(VMD642_UART_Handle

hUart, Int16 regnum)

{Int16 regindex, returnval, lcr;

/* Register index is determined by lower 3 bits and

the target UART */

regindex = regnum & 0x7;

if(hUart == 1) regindex += 8;

/* If regnum between 0x08 and 0x0F, set bit 7 of

LCR to access register */

if ((regnum & 0x18) == 0x8)

{ lcr = VMD642_UART_rget(hUart,

VMD642_UART_LCR);

VMD642_UART_rset(hUart,VMD642_UART

_LCR, lcr | 0x80);

returnval = VMD642_rget(regindex);

VMD642_UART_rset(hUart,VMD642_UART

_LCR, lcr);}

else{

returnval = VMD642_rget(regindex);}

return returnval;}

/*******************************/

/*===VMD642_UART_open======*/

/* Initialize UART and return handle */

/********************************/

VMD642_UART_Handle

VMD642_UART_open(Int16

{VMD642_UART_Handle hUart;

Int16 dl = 0;

/* Assign handle */

hUart = devid;

/* Set registers based on config structure */

VMD642_UART_rset(hUart,

VMD642_UART_MCR,config -> regs[3]);

VMD642_UART_rset(hUart,VMD642_UART

_EFR, dl);

VMD642_UART_rset(hUart,

VMD642_UART_MCR, config -> regs[3]);

VMD642_UART_rset(hUart,

VMD642_UART_IER, config -> regs[0]);

VMD642_UART_rset(hUart,

VMD642_UART_FCR, config -> regs[1]);

VMD642_UART_rset(hUart,

VMD642_UART_LCR, config -> regs[2]);

dl=VMD642_UART_rget(hUart,VMD642_UA

RT_MCR);

if((dl & 0x80)==0x80)

devid,Int16

baudrate,VMD642_UART_Config *config)

青岛农业大学机电工程学院本科毕业设计（论文）

{baudrate = (baudrate/4);}

VMD642_UART_rset(hUart,VMD642_UART

_DLL, baudrate & 0xff);

dl=VMD642_UART_rget(hUart,VMD642_UA

RT_DLL);

VMD642_UART_rset(hUart,VMD642_UART

_DLH, (baudrate >> 8) & 0xff);

dl=VMD642_UART_rget(hUart,VMD642_UA

RT_DLH);

/* Clear any outstanding receive characters*/

VMD642_UART_rget(hUart,VMD642_UART

_RBR);

return hUart;}

/*****************************/

/*====VMD642_UART_getChar===*/

/* Get one character of data from the */

/*******************************/

Int16

VMD642_UART_getChar(VMD642_UART_Handl

e hUart)

{Int16 status;

while(1){status=VMD642_UART_rget(hUart,

VMD642_UART_LSR);

if ((status & 1) != 0) // DR

break;}

return VMD642_UART_rget(hUart,

VMD642_UART_RBR);}

/********************************/

/* ===VMD642_UART_putChar ===*/

/*Send one character of data to the UART */

/*******************************/

void

VMD642_UART_putChar(VMD642_UART_Handl

e hUart, Uint16 data)

{Int16 status;

while(1)

{status = VMD642_UART_rget(hUart,

VMD642_UART_LSR);

if ((status & 0x20) != 0) // THRE

break;}

VMD642_UART_rset(hUart,

VMD642_UART_THR, data);}

Uart_SendFeature(start_flag);

Uart_SendFeature(x_start);

Uart_Send( x );

Uart_SendFeature(x_end);

Uart_SendFeature(y_start);

Uart_Send( y );

Uart_SendFeature(y_end);

Uart_SendFeature(z_start);

Uart_Send( z );

Uart_SendFeature(z_end);

Uart_SendFeature(end_flag);

Uart_SendFeature(0x0d);

void Uart_Send(char x[])

{int ui,uj;

g_uartHandleA

W_VMD642_BAUD_9600,&g_uartConfig);

ui= VMD642_rget(VMD642_UART_LSR);

while (ui & 0x20)

{g_uartBuf =x[0];

VMD642_waitusec(400);

VMD642_UART_putChar(g_uartHandleA,(g_uartB

uf & 0xff));

VMD642_waitusec(400);

ui= VMD642_rget(VMD642_UART_LSR);

while (ui & 0x20)

{g_uartBuf =x[1];

VMD642_waitusec(400);

VMD642_UART_putChar(g_uartHandleA,

(g_uartBuf & 0xff));

VMD642_waitusec(4000);

ui= VMD642_rget(VMD642_UART_LSR);

while (ui & 0x20)

{g_uartBuf =x[2];

VMD642_waitusec(400);

VMD642_UART_putChar(g_uartHandleA,(g_uartB

VMD642_UART_open(VMD642_UARTA,UARTH

青岛农业大学机电工程学院本科毕业设计（论文）

uf & 0xff));

VMD642_waitusec(4000);

ui= VMD642_rget(VMD642_UART_LSR);

while (ui & 0x20)

{g_uartBuf =x[3];

VMD642_waitusec(400);

VMD642_UART_putChar(g_uartHandleA,

(g_uartBuf & 0xff));

VMD642_waitusec(4000);

ui= VMD642_rget(VMD642_UART_LSR);

while (ui & 0x20)

{g_uartBuf =x[4];

VMD642_waitusec(400);

VMD642_UART_putChar(g_uartHandleA,

(g_uartBuf & 0xff));

VMD642_waitusec(4000);

ui= VMD642_rget(VMD642_UART_LSR);

while (ui & 0x20)

{g_uartBuf =x[5];

VMD642_waitusec(400);

VMD642_UART_putChar(g_uartHandleA,

(g_uartBuf & 0xff));

VMD642_waitusec(4000);

ui= VMD642_rget(VMD642_UART_LSR);

while (ui & 0x20)

{g_uartBuf =x[6];

VMD642_waitusec(400);

VMD642_UART_putChar(g_uartHandleA,

(g_uartBuf & 0xff));

VMD642_waitusec(4000);

ui= VMD642_rget(VMD642_UART_LSR);

while (ui & 0x20)

{g_uartBuf =x[7];

VMD642_waitusec(400);

VMD642_UART_putChar(g_uartHandleA,

(g_uartBuf & 0xff));

VMD642_waitusec(4000);

ui=0x00;}}}

}}}}}}

void Uart_SendFeature(char x )

{int ui;

g_uartHandleA=VMD642_UART_open(VMD642_

UARTA,UARTHW_VMD642_BAUD_9600,&g_ua

rtConfig);

ui= VMD642_rget(VMD642_UART_LSR);

while (ui & 0x20)

{g_uartBuf =x;

VMD642_waitusec(400);

VMD642_UART_putChar(g_uartHandleA,

(g_uartBuf & 0xff));

VMD642_waitusec(400);

ui=0x00;}}

青岛农业大学机电工程学院本科毕业设计（论文）

附录11 下位机MSP430F149的串行通讯程序

/*****************************************

函数名称：InitUART

功能：初始化UART端口

参数：无

返回值：无

******************************************

**/

void InitUART(void)

{P3SEL|=0x30;

//Select P3.4 and P3.5 as UART

ME1|=UTXE0+URXE0; UCTL0|=CHAR;

UTCTL0 |=SSEL0;//UCLK=ACLK

UBR00=0x03;

UBR10=0x00;

//UMCTL0=0x4A;//Modulation

UCTL0&=~SWRST;

IE1|=URXIE0;

}

void uart0_send_char(unsigned char

*send_data,unsigned char n)

{while(n != 0)

{TXBUF0 = *(send_data);

send_data++;

while(!(U0IFG & UTXIFG0));n--;}

Delayus(10);}

/****************************************

函数名称：PutSting

功能：向PC机发送字符串

参数：无

返回值：无

*****************************************/

void PutString(uchar *ptr)

{while(*ptr != '0'){

while (!(IFG1 & UTXIFG0)); //

the IFG1 set when U0TXBUF is empty

TXBUF0 = *ptr++;}

while (!(IFG1 & UTXIFG0));

TXBUF0 = 'n';

}

/************************************

函数名称：UART0_RXISR

功能：UART0的接收中断服务函数，在这里

唤醒CPU，使它退出低功耗模式

参数：无

返回值：无

***************************************/

#pragma vector = UART0RX_VECTOR

__interrupt void UART0_RXISR(void)

{_EINT();

while (!(IFG1 & UTXIFG0));

{if(RXBUF0=='s')

{flag=1;}

if(flag==1)

{tempdata[j]=RXBUF0;

j++;

Write_Cmd(0x80);

Write_Data(tempdata[2]);

Write_Data(tempdata[3]);

Write_Data(tempdata[4]);

Write_Data(tempdata[5]);

Write_Cmd(0x82);

Write_Data(tempdata[6]);

Write_Data(tempdata[7]);

XX=((tempdata[2]-0x30)*100)+((tempdata[3]-0x30)

*10)+tempdata[4]-0x30;

x1=XX/100;

x2=(XX-x1*100)/10;

x3=XX-x1*100-x2*10;

x1=x1+0x30;

x2=x2+0x30;

x3=x3+0x30;

Write_Cmd(0x90);

Write_Data(tempdata[12]);

青岛农业大学机电工程学院本科毕业设计（论文）

Write_Data(tempdata[13]);

Write_Data(tempdata[14]);

Write_Data(tempdata[15]);

Write_Cmd(0x92);

Write_Data(tempdata[16]);

Write_Data(tempdata[17]);

YY=((tempdata[12]-0x30)*100)+((tempdata[13]-0x

Write_Data(tempdata[24]);

Write_Data(tempdata[25]);

Write_Cmd(0x8a);

Write_Data(tempdata[26]);

Write_Data(tempdata[27]);

if(tempdata[24]=='.')

ZZ=(tempdata[22]-0x30)*10+(tempdata[23]-0x30);

30)*10)+tempdata[14]-0x30;

y1=YY/100;

y2=(YY-y1*100)/10;

y3=YY-y1*100-y2*10;

y1=y1+0x30;

y2=y2+0x30;

y3=y3+0x30;

Write_Cmd(0x88);

Write_Data(tempdata[22]);

Write_Data(tempdata[23]);

else

ZZ=((tempdata[22]-0x30)*100)+((tempdata[23]-0x3

0)*10)+tempdata[24]-0x30;

z1=ZZ/100;

z2=(ZZ-z1*100)/10;

z3=ZZ-z1*100-z2*10;

z1=z1+0x30;

z2=z2+0x30;

z3=z3+0x30;}}

}

青岛农业大学机电工程学院本科毕业设计（论文）

附录12 下位机对番茄果实定位和采摘的算法（部分）

/********************************************************************/

/* granted through contract.

工程名称： Pitch.c

功能描述：接收DSP的串口数据，实现对三维滑台、机械手和传感器等的检测和控制

/* Restricted rights to use, duplicate or disclose this code are */

// 开发环境： IAR Embedded Workbench 6.0

涉及的库： math.h 等

组成文件： main.c subfuncs.c UART0.c Keypad.c

硬件连接：见MSP430F149模块电路设计

维护记录： 2014-03-16

2014-04-12

2014-04-15

……

v3.2

void Aut_Fir_you();

void Aut_Fir_zuo();

void Aut_Fir_Stop();

void Aut_Sec_shang();

void Aut_Sec_xia();

void Aut_Sec_Stop();

void Aut_Thir_qian();

void Aut_Thir_hou();

void Aut_Thir_Stop();

void Fir_you();

void Fir_zuo();

void Sec_shang();

void Sec_xia();

void Thir_qian();

void Thir_hou();

void Fir_Stop();

void Sec_Stop();

void Thir_Stop();

void All_Stop();

void Open();

v1.0

v1.1

v1.2

2014-05-26

/********************************************************************/

#include

#include "BoardConfig.h"

#include "subfuncs.h"

#include "dispdata.h"

#include "Keypad.h"

#include "gdata.h"

#include "uart0.h"

#include "math.h"

typedef int Bool;

#define false 0;

#define true 1;

//unsigned char decode_cmd(unsigned char

*buf,unsigned char *cmd,unsigned char

*para_num,unsigned char *parabuf);

void Delayus(unsigned int DelayTimes);

void init_timerA(void);

unsigned char rs232_send(unsigned char CMD);

void Ini_Lcddef(void);

void delay_n_s(int i);

void InitPWM();

青岛农业大学机电工程学院本科毕业设计（论文）

void Close();

void Get_Fruit();

void Xunhuanqianjin();

void Xunhuanhoutui();

void Open_chuankou();

void Close_chuankou();

void Check_zhogndian();

void Check_anjian();

void Check_jiexian();

void Go_Pitch11();void Go_Pitch21();

void Go_Pitch12();void Go_Pitch22();

void Go_Pitch13();void Go_Pitch23();

void Go_Pitch14();void Go_Pitch24();

void Go_Pitch15();void Go_Pitch25();

void Go_Pitch16();void Go_Pitch26();

void Go_Pitch17();void Go_Pitch27();

void Go_Pitch18();void Go_Pitch28();

void Go_Pitch19();void Go_Pitch29();

void Go_Pitch1A();void Go_Pitch2A();

void Go_Pitch1B();void Go_Pitch2B();

void Go_Pitch1C();void Go_Pitch2C();

void Go_Pitch1D();void Go_Pitch2D();

void Go_Pitch1E();void Go_Pitch2E();

void Go_Pitch1F();void Go_Pitch2F();

void Go_Pitch1G();void Go_Pitch2G();

void Go_Pitch1H();void Go_Pitch2H();

//void Go_Pitch1I();void Go_Pitch2I();

//void Go_Pitch1J();void Go_Pitch2J();

void panduanqujian();

void panduan11qu();void panduan21qu();

void panduan12qu();void panduan22qu();

void panduan13qu();void panduan23qu();

void panduan14qu();void panduan24qu();

void panduan15qu();void panduan25qu();

void panduan16qu();void panduan26qu();

void panduan17qu();void panduan27qu();

void panduan18qu();void panduan28qu();

void panduan19qu();void panduan29qu();

void panduan1Aqu();void panduan2Aqu();

void panduan1Bqu();void panduan2Bqu();

void panduan1Cqu();void panduan2Cqu();

void panduan1Dqu();void panduan2Dqu();

void panduan1Equ();void panduan2Equ();

void panduan1Fqu();void panduan2Fqu();

void panduan1Gqu();void panduan2Gqu();

void panduan1Hqu();void panduan2Hqu();

//void panduan1Iqu();void panduan2Iqu();

//void panduan1Jqu();void panduan2Jqu();

void yunxing_buchang();

Bool yanshi_panduan_shendu();

int you_buchang11=0;

int zuo_buchang11=0;

int shang_buchang11=0;

int xia_buchang11=0;

unsigned int Zong_X=0;

unsigned int Zong_Y=0;

unsigned int Zong_Z=0;

unsigned int Aut_Fir_zuo_flag=0;

unsigned int Aut_Fir_you_flag=0;

unsigned int Aut_Sec_shang_flag=0;

unsigned int Aut_Sec_xia_flag=0;

unsigned int Aut_Thir_qian_flag=0;

unsigned int Aut_Thir_hou_flag=0;

int zuo_flag=0;

int press_zuo_flag=0;

int you_flag=0;

int press_you_flag=0;

int shang_flag=0;

int press_shang_flag=0;

int xia_flag=0;

int press_xia_flag=0;

int qian_flag=0;

int press_qian_flag=0;

int hou_flag=0;

int press_hou_flag=0;

int jiajin_flag=0;

int press_jiajin_flag=0;

void PutString(uchar *ptr);

青岛农业大学机电工程学院本科毕业设计（论文）

uchar tempdata[32]={0};

int qian_yanshi=0;

uchar shuzi[] = {""};

uchar PP = 0;

uchar flag=0;

uchar cflag = 0;

uchar count=0;

uchar flag;

uchar disp_flag;

unsigned int XX=0;

unsigned int YY=0;

unsigned int ZZ=0;

unsigned int x1=0,x2=0,x3=0;

unsigned int y1=0,y2=0,y3=0;

unsigned int z1=0,z2=0,z3=0;

unsigned int X1=0,X2=0,X3=0;

unsigned int Y1=0,Y2=0,Y3=0;

unsigned int Z1=0,Z2=0,Z3=0;

unsigned int zuo=0,you=0,shang=0,xia=0;

uchar j=0;

unsigned int k=0;

/***************主函数*****************/

void main( )

{flag=0;

uint i;

WDTCTL=WDT_ADLY_1_9;//Set Watchdog Timer

interval to ~30ms

IE1|= WDTIE; // Enable WDT interrupt

/*------Select 8MHz Clock-------*/

BCSCTL1&=~XT2OFF;

{IFG1&=~OFIFG;

for (i = 0xFF; i > 0; i--);}

while ((IFG1 & OFIFG));

BCSCTL2|=SELM_2+SELS;

P5DIR=0xff;

P5OUT=0x00;

P3SEL|=0x0F;

P3DIR|=0x00;

P6DIR&=0x78;

Init_Keypad();

Ini_Lcd();

Disp_HZ(0x80,line1,8);

Disp_HZ(0x90,line2,8);

Disp_HZ(0x98,line3,8);

delay_n_s(1);

Disp_HZ(0x98,line4,8);

InitUART( );

delay_n_s(1);

Disp_HZ(0x98,line5,8);

InitPWM( );

delay_n_s(1);

Ini_Lcd();

disp_flag=1;

_EINT();

while(1){}}

void Open()

{PutString("#1P1050#2P1200T1000r");

delay1500();}

void Close()

{PutString("#1P1500#2P1200T1000r");

delay1500();}

void Get_Fruit()

{PutString("#1P1500#2P1200T1000r");

delay1500();

PutString("#1P1500#2P800T1000r");

delay1500();

PutString("#1P1500#2P1600T1000r");

delay1500();

PutString("#1P1500#2P800T1000r");

delay1500();

PutString("#1P1500#2P1600T1000r");

delay1500();

PutString("#1P1500#2P1200T1000r");

delay1500();

PutString("#1P1050#2P1200T1000r");

delay1500( ); }

void Delayus(unsigned int DelayTimes)

青岛农业大学机电工程学院本科毕业设计（论文）

{unsigned int i;

for(i=0;i

{_NOP();}

}

void InitPWM( )

{

P4DIR|= 0x7e; // P4.1 - P4.6 output

P4SEL |= 0x7e; // P4.1 - P4.6 TBx options

TBCCR0 = 32-1; // PWM Period

TBCCTL1=OUTMOD_7; // CCR1 reset/set

TBCCR1 = 16; // CCR1 PWM duty cycle

TBCCTL2 = OUTMOD_7;

TBCCR2 = 16;

TBCCTL3 = OUTMOD_7;

TBCCR3 = 16;

TBCCTL4 = OUTMOD_7;

TBCCR4 = 16;

TBCCTL5 = OUTMOD_7;

TBCCR5 = 16;

TBCCTL6 = OUTMOD_7;

TBCCR6 = 16;

TBCTL = TBSSEL_1 + MC_1; // ACLK, up mode

}

/**************************************/

void Fir_Stop( )

{

P5OUT &=0XFC; //1111 1100

}

/***************************************/

void Sec_Stop()

{

P5OUT &=0XCF; //1100 1111

}

/*****************************************/

void Thir_Stop()

{

P5OUT &=0XF3; //1111 0011

}

/*****************************************/

void All_Stop()

{

P5OUT&=0x00; // P5 0000 0000

}

/*****************************************/

void Fir_you()

{you=1;zuo=0;shang=0;xia=0;

P5OUT |= 0x03; // P5 0000 0011

}

/****************************************/

void Fir_zuo()

{

zuo=1;you=0;shang=0;xia=0;

P5OUT&=0xFD; //1111 1101

P5OUT|= 0x01; // P5 0000 0001

}

/*****************************************/

void Sec_shang()

{

shang=1;xia=0;zuo=0;you=0;

P5OUT |= 0x30; // P5 0011 0000

}

/****************************************/

void Sec_xia()

{

xia=1;shang=0;zuo=0;you=0;

P5OUT &= 0xDF;

P5OUT |= 0x10; // P5 0001 0000

}

/***************************************/

void Thir_qian()

{

P5OUT |= 0x0c; // P5 0000 1100

}

/*****************************************/

void Thir_hou()

{

P5OUT &= 0xF7;

P5OUT |= 0x04; // P5 0000 0100

青岛农业大学机电工程学院本科毕业设计（论文）

}

/*****************************************/

void Aut_Fir_you()

{

P5OUT |= 0x03; // P5 0000 0011

Aut_Fir_you_flag=1;

Aut_Fir_zuo_flag=0;

}

/***************************************/

void Aut_Fir_zuo()

{

P5OUT&=0xFD; //1111 1101

P5OUT|= 0x01; // P5 0000 0001

Aut_Fir_zuo_flag=1;

Aut_Fir_you_flag=0;

}

/*****************************************/

void Aut_Sec_shang()

{

P5OUT |= 0x30; // P5 0011 0000

Aut_Sec_shang_flag=1;

Aut_Sec_xia_flag=0;

}

/****************************************/

void Aut_Sec_xia()

{

P5OUT &= 0xDF;

P5OUT |= 0x10; // P5 0001 0000

Aut_Sec_xia_flag=1;

Aut_Sec_shang_flag=0;

}

/***************************************/

void Aut_Thir_qian()

{

P5OUT |= 0x0c; // P5 0000 1100

Aut_Thir_qian_flag=1;

Aut_Thir_hou_flag=0;

}

/****************************************/

void Aut_Thir_hou()

{

P5OUT &= 0xF7;

P5OUT |= 0x04; // P5 0000 0100

Aut_Thir_hou_flag=1;

Aut_Thir_qian_flag=0;

}

/****************************************/

void Aut_Fir_Stop( )

{

P5OUT &=0XFC; //1111 1100

Aut_Fir_zuo_flag=0;

Aut_Fir_you_flag=0;

}

/*****************************************/

void Aut_Sec_Stop()

{

P5OUT &=0XCF; //1100 1111

Aut_Sec_xia_flag=0;

Aut_Sec_shang_flag=0;

}

/****************************************/

void Aut_Thir_Stop()

{

P5OUT &=0XF3; //1111 0011

Aut_Thir_qian_flag=0;

Aut_Thir_hou_flag=0;

}

void delay1000()

{

uint tmp;

uint tmp_1;

for(tmp_1 = 200;tmp_1 > 0;tmp_1--)

{for(tmp = 6000;tmp > 0;tmp--);}

}

void delay_n_s(int i)

{uint tmp;

uint tmp_1;

while(i--)

青岛农业大学机电工程学院本科毕业设计（论文）

{ for(tmp_1 = 300;tmp_1 > 0;tmp_1--)

{ ;for(tmp = 6000;tmp > 0;tmp--);} }}

/***********判断程序代码*****************/

/**********判断（1,1）区***************/

void panduan11qu()

{

if((XX>=240)&&(XX<260)&&(YY>=170)&&(YY

<180)&&(ZZ>90)&&(ZZ<120))

{Write_Cmd(0x84);Write_Data('1');Write_Data('1');

Write_Cmd(0x86);Write_Data(x1);Write_Data(x2);

Write_Cmd(0x87);Write_Data(x3);Write_Data('R');

Write_Cmd(0x96);Write_Data(y1);Write_Data(y2);

Write_Cmd(0x97);Write_Data(y3);Write_Data('R');

Write_Cmd(0x8E);Write_Data(z1);Write_Data(z2);

Write_Cmd(0x8F);Write_Data(z3);Write_Data('R');

//_DINT();

IE1 &= ~URXIE0; // 使能USART0的

接收中断

Go_Pitch11();

// _EINT();

IE1 |= URXIE0;

Write_Cmd(0x84);Write_Data(' ');Write_Data('

');

}

else

{

Write_Cmd(0x86);Write_Data(x1);Write_Data(x2);

Write_Cmd(0x87);Write_Data(x3);Write_Data('W');

Write_Cmd(0x96);Write_Data(y1);Write_Data(y2);

Write_Cmd(0x97);Write_Data(y3);Write_Data('W');

Write_Cmd(0x8E);Write_Data(z1);Write_Data(z2);

Write_Cmd(0x8F);Write_Data(z3);Write_Data('W');

}

/***************判断（2,1）区***************/

void panduan21qu()

{if( (XX>=260)&&(XX<280)&&(YY>=170)&&(Y

Y<180)&&(ZZ>90)&&(ZZ<120))

// x:(260-280) y:(170-180)

z:(90-120)

{

Write_Cmd(0x84);Write_Data('2');Write_Data('1');

Write_Cmd(0x86);Write_Data(x1);Write_Data(x2);

Write_Cmd(0x87);Write_Data(x3);Write_Data('R');

Write_Cmd(0x96);Write_Data(y1);Write_Data(y2);

Write_Cmd(0x97);Write_Data(y3);Write_Data('R');

Write_Cmd(0x8E);Write_Data(z1);Write_Data(z2);

Write_Cmd(0x8F);Write_Data(z3);Write_Data('R');

IE1 &= ~URXIE0;

Go_Pitch21();

IE1 |= URXIE0;

Write_Cmd(0x84);Write_Data('

');Write_Data(' '); }

else

{Write_Cmd(0x86);Write_Data(x1);Write_Data(x2);

Write_Cmd(0x87);Write_Data(x3);Write_Data('W');

Write_Cmd(0x96);Write_Data(y1);Write_Data(y2);

Write_Cmd(0x97);Write_Data(y3);Write_Data('W');

Write_Cmd(0x8E);Write_Data(z1);Write_Data(z2);

Write_Cmd(0x8F);Write_Data(z3);Write_Data('W');

}

……

（此处省略若干行程序）

……

/**************判断（1,H）区****************/

void panduan1Hqu( )

{if((XX>=240)&&(XX<260)&&(YY>=330)&&(Y

Y<340)&&(ZZ>95)&&(ZZ<126))

{Write_Cmd(0x84);Write_Data('1');Write_Data('H');

青岛农业大学机电工程学院本科毕业设计（论文）

Write_Cmd(0x86);Write_Data(x1);Write_Data(x2);

Write_Cmd(0x87);Write_Data(x3);Write_Data('R');

Write_Cmd(0x96);Write_Data(y1);Write_Data(y2);

Write_Cmd(0x97);Write_Data(y3);Write_Data('R');

Write_Cmd(0x8E);Write_Data(z1);Write_Data(z2);

Write_Cmd(0x8F);Write_Data(z3);Write_Data('R');

IE1 &= ~URXIE0;

Go_Pitch1H();

IE1 |= URXIE0;

Write_Cmd(0x84);Write_Data(' ');Write_Data(' '); }

else

{Write_Cmd(0x86);Write_Data(x1);Write_Data(x2);

Write_Cmd(0x87);Write_Data(x3);Write_Data('W');

Write_Cmd(0x96);Write_Data(y1);Write_Data(y2);

Write_Cmd(0x97);Write_Data(y3);Write_Data('W');

Write_Cmd(0x8E);Write_Data(z1);Write_Data(z2);

Write_Cmd(0x8F);Write_Data(z3);Write_Data('W');

}}

/**************判断（2,H）区****************/

void panduan2Hqu()

{if((XX>=260)&&(XX<280)&&(YY>=330)&&(Y

Y<340)&&(ZZ>98)&&(ZZ<125))

Write_Cmd(0x8F);Write_Data(z3);Write_Data('W');

}}

/**************采摘程序代码***************/

/***************采摘（1,1）区***************/

void Go_Pitch11()

{if(yanshi_panduan_shendu()==1)

{yunxing_buchang();

qian_yanshi=108-ZZ;

zuo_buchang11=(XX-230)/8;

shang_buchang11=(YY-170)/2;

if(XX<240)

{Aut_Fir_zuo();delay_n_s(zuo_buchang11);Zong_X

=Zong_X+zuo_buchang11;Aut_Fir_Stop( ); }

else

{ Aut_Fir_you();delay_n_s(zuo_buchang11+1);Zong

_X=Zong_X-zuo_buchang11;Aut_Fir_Stop( ); }

if(YY<=175){ Aut_Sec_shang( );delay_n_s(33+sha

ng_buchang11);Zong_Y=Zong_Y+33+shang_bucha

ng11;Aut_Sec_Stop( );}

else

{ Aut_Sec_shang( );delay_n_s(37-shang_buchang11

);Zong_Y=Zong_Y+37-shang_buchang11;Aut_Sec_

{Write_Cmd(0x84);Write_Data('2');Write_Data('H');

Stop( );}

Write_Cmd(0x86);Write_Data(x1);Write_Data(x2);

Write_Cmd(0x87);Write_Data(x3);Write_Data('R');

Write_Cmd(0x96);Write_Data(y1);Write_Data(y2);

Write_Cmd(0x97);Write_Data(y3);Write_Data('R');

Write_Cmd(0x8E);Write_Data(z1);Write_Data(z2);

Write_Cmd(0x8F);Write_Data(z3);Write_Data('R');

IE1 &= ~URXIE0;

Go_Pitch2H();

IE1 |= URXIE0;

Write_Cmd(0x84);Write_Data(' ');Write_Data(' '); }

else

{Write_Cmd(0x86);Write_Data(x1);Write_Data(x2);

Write_Cmd(0x87);Write_Data(x3);Write_Data('W');

Write_Cmd(0x96);Write_Data(y1);Write_Data(y2);

Write_Cmd(0x97);Write_Data(y3);Write_Data('W');

Write_Cmd(0x8E);Write_Data(z1);Write_Data(z2);

//Sec_shang( );delay_n_s(32);Sec_Stop( );

Open( );

Aut_Thir_qian( );delay_n_s(qian_yanshi);Zong_Z=

Zong_Z+qian_yanshi; Aut_Thir_Stop( );

Get_Fruit( );

Aut_Sec_xia();delay_n_s(20);Zong_Y=Zong_Y-20;

Aut_Sec_Stop();

Aut_Thir_hou( );delay_n_s(qian_yanshi+1);Zong_Z

=Zong_Z-qian_yanshi; Aut_Thir_Stop( );

Aut_Sec_xia(

);

Open( );

Aut_Sec_shang(

if(XX<240)

);

delay_n_s(2);Zong_Y=Zong_Y+2;Aut_Sec_Stop( );

);

delay_n_s(13);Zong_Y=Zong_Y-13;Aut_Sec_Stop(

青岛农业大学机电工程学院本科毕业设计（论文）

{ Aut_Fir_you( );delay_n_s(zuo_buchang11);Zong_

X=Zong_X-zuo_buchang11;Aut_Fir_Stop( ); }

else

{Aut_Fir_zuo( );delay_n_s(zuo_buchang11);Zong_

X=Zong_X+zuo_buchang11;Aut_Fir_Stop( ); }}

else ;}

/**************采摘（2,1）区****************/

void Go_Pitch21()

{ if(yanshi_panduan_shendu()==1)

{yunxing_buchang();

qian_yanshi=112-ZZ;

zuo_buchang11=(XX-240)/8;

shang_buchang11=(YY-170)/2;

if(XX<260)

{Fir_zuo();delay_n_s(zuo_buchang11);Fir_Stop( );

}

else

{ Fir_you();delay_n_s(zuo_buchang11+1);Fir_Stop(

); }

if(YY<=175)

{ Sec_shang( );delay_n_s(33+shang_buchang11);Se

c_Stop( ); }

else

{ Sec_shang( );delay_n_s(39-shang_buchang11);Sec

_Stop( ); }

//Sec_shang( );delay_n_s(32);Sec_Stop( );

Open( );

Thir_qian( );delay_n_s(qian_yanshi); Thir_Stop( );

Get_Fruit( );

Sec_xia( ); delay_n_s(20);Sec_Stop( );

Thir_hou( );delay_n_s(qian_yanshi+1); Thir_Stop( );

//退回到原始位置

Sec_xia( ); delay_n_s(12);Sec_Stop( );

Open( );//释放果实

//回到采摘前的的位置

Aut_Sec_xia(

if(XX<240)

{ Aut_Fir_you( );delay_n_s(zuo_buchang11);Zong_

X=Zong_X-zuo_buchang11;Aut_Fir_Stop( ); }//右

补偿

else

{ Aut_Fir_zuo( );delay_n_s(zuo_buchang11);Zong_

X=Zong_X+zuo_buchang11;Aut_Fir_Stop( ); }//

右补偿

}

else ;

}

……

（此处省略若干行程序）

……

/************************采摘（1,G）区

*************************/

void Go_Pitch1G()

{

if(yanshi_panduan_shendu()==1)

{

yunxing_buchang();

qian_yanshi=abs(115-ZZ);

zuo_buchang11=(XX-239)/7;

shang_buchang11=(YY-319)/4;

if(XX<240)

{Aut_Fir_zuo();delay_n_s(zuo_buchang11) ;Aut_

Fir_Stop( ); }//右补偿

else

{Aut_Fir_you();delay_n_s(zuo_buchang11+1);Aut_

Fir_Stop( ); }//右补偿

if(YY<330)

{Aut_Sec_shang();delay_n_s(23-shang_buchang11);

Aut_Sec_Stop( );}

);

delay_n_s(1);Zong_Y=Zong_Y+2;Aut_Sec_Stop( );

青岛农业大学机电工程学院本科毕业设计（论文）

else

{Aut_Sec_shang();delay_n_s(30-shang_buchang11);

Aut_Sec_Stop( );}

//Sec_shang( );delay_n_s(32);Sec_Stop( );

Open( );

Aut_Thir_qian(

Aut_Thir_Stop( );

Get_Fruit( );

Aut_Sec_xia( ); delay_n_s(20);Aut_Sec_Stop( );

Aut_Thir_hou( );delay_n_s(qian_yanshi+1);

Aut_Thir_Stop( ); //退回到原始位置

Aut_Sec_xia( ); delay_n_s(14);Aut_Sec_Stop( );

Open( );//释放果实

//回到采摘前的的位置

Aut_Sec_shang( ); delay_n_s(7);Aut_Sec_Stop( );

if(XX<240){Aut_Fir_you();delay_n_s(zuo_buchang

11);Aut_Fir_Stop( ); }//右补偿

else

{Aut_Fir_zuo();delay_n_s(zuo_buchang11+2);Aut_

Fir_Stop();}}

else ;

}

/*************采摘（2,G）区*****************/

void Go_Pitch2G()

{

if(yanshi_panduan_shendu()==1)

{

yunxing_buchang();

qian_yanshi=abs(116-ZZ);

zuo_buchang11=(XX-260)/6;

shang_buchang11=(YY-319)/4;

if(XX<259)

{Aut_Fir_zuo();delay_n_s(zuo_buchang11);Aut_Fir

_Stop( );}

else

{Aut_Fir_you();delay_n_s(zuo_buchang11+3);Aut_

Fir_Stop( );}

if(YY<330)

{ Aut_Sec_shang( );delay_n_s(23-shang_buchang11

);Aut_Sec_Stop( );}

else

{ Aut_Sec_shang( );delay_n_s(31-shang_buchang11

);Aut_Sec_Stop( );}

);delay_n_s(qian_yanshi); //Sec_shang( );delay_n_s(32);Sec_Stop( );

Open( );

if(ZZ<=116)

{Aut_Thir_qian( );delay_n_s(qian_yanshi);}

else

{ Aut_Thir_hou( );delay_n_s(qian_yanshi-1); }

Aut_Thir_Stop();

Get_Fruit( );

Aut_Sec_xia( ); delay_n_s(20);Aut_Sec_Stop( );

Aut_Thir_hou(

Aut_Thir_Stop();

Aut_Sec_xia( ); delay_n_s(15);Aut_Sec_Stop( );

Open( );

Aut_Sec_shang( ); delay_n_s(6);Aut_Sec_Stop( );

if(XX<240)

{ Aut_Fir_you();delay_n_s(zuo_buchang11);Aut_Fir

_Stop( );}

else{ Aut_Fir_zuo();delay_n_s(zuo_buchang11+3);

Aut_Fir_Stop( ); }

}else ;}

/***************采摘（1,H）区***************/

void Go_Pitch1H()

{if(yanshi_panduan_shendu()==1)

{

yunxing_buchang();

qian_yanshi=abs(116-ZZ);

zuo_buchang11=(XX-239)/7;

shang_buchang11=(YY-329)/4;

if(XX<240){Aut_Fir_zuo();delay_n_s(zuo_buchang

11) ;Aut_Fir_Stop( );}

else

{Aut_Fir_you();delay_n_s(zuo_buchang11+2);Aut_

Fir_Stop( );}

if(YY<340)

);delay_n_s(qian_yanshi+1);

青岛农业大学机电工程学院本科毕业设计（论文）

{Aut_Sec_shang();delay_n_s(23-shang_buchang11);

Aut_Sec_Stop( );}

else

{Aut_Sec_shang();delay_n_s(30-shang_buchang11);

Aut_Sec_Stop( );}

//Sec_shang( );delay_n_s(32);Sec_Stop( );

Open( );

Aut_Thir_qian();delay_n_s(qian_yanshi);

Aut_Thir_Stop( );

Get_Fruit( );

Aut_Sec_xia( ); delay_n_s(20);Aut_Sec_Stop( );

Aut_Thir_hou();delay_n_s(qian_yanshi+1);

Aut_Thir_Stop( );

Aut_Sec_xia( ); delay_n_s(14);Aut_Sec_Stop( );

Open( );

Aut_Sec_shang( ); delay_n_s(7);Aut_Sec_Stop( );

if(XX<240){Aut_Fir_you();delay_n_s(zuo_buchang

11);Aut_Fir_Stop( );}

else

{Aut_Fir_zuo();delay_n_s(zuo_buchang11+2);Aut_

Fir_Stop( );}}

else;

}

/*************采摘（2,H）区****************/

void Go_Pitch2H()

{ if(yanshi_panduan_shendu()==1)

{yunxing_buchang();

qian_yanshi=abs(118-ZZ);

zuo_buchang11=(XX-260)/6;

shang_buchang11=(YY-329)/4;

if(XX<259)

{Aut_Fir_zuo();delay_n_s(zuo_buchang11);Aut_Fir

_Stop( );}

else

{Aut_Fir_you();delay_n_s(zuo_buchang11+3);Aut_

Fir_Stop( );}

if(YY<340)

{ Aut_Sec_shang( );delay_n_s(22-shang_buchang11

);Aut_Sec_Stop( );}

else

{ Aut_Sec_shang( );delay_n_s(31-shang_buchang11

);Aut_Sec_Stop( );}

//Sec_shang( );delay_n_s(32);Sec_Stop( );

Open( );

if(ZZ<=116)

{Aut_Thir_qian( );delay_n_s(qian_yanshi);}

else {Aut_Thir_hou( );delay_n_s(qian_yanshi-1); }

Aut_Thir_Stop();

Get_Fruit( );

Aut_Sec_xia( ); delay_n_s(20);Aut_Sec_Stop( );

Aut_Thir_hou(

Aut_Thir_Stop();

Aut_Sec_xia( ); delay_n_s(15);Aut_Sec_Stop( );

Open( );

Aut_Sec_shang( ); delay_n_s(6);Aut_Sec_Stop( );

if(XX<240)

{ Aut_Fir_you();delay_n_s(zuo_buchang11);Aut_Fir

_Stop( ); }

else

{Aut_Fir_zuo();delay_n_s(zuo_buchang11+3);Aut_

Fir_Stop( );}}else ;}

/***********判断果实位于哪个区间**********/

void panduanqujian()

{

panduan11qu(); panduan21qu();

panduan12qu(); panduan22qu();

panduan13qu(); panduan23qu();

panduan14qu(); panduan24qu();

panduan15qu(); panduan25qu();

panduan16qu(); panduan26qu();

panduan17qu(); panduan27qu();

panduan18qu(); panduan28qu();

panduan19qu(); panduan29qu();

panduan1Aqu(); panduan2Aqu();

panduan1Bqu(); panduan2Bqu();

panduan1Cqu(); panduan2Cqu();

panduan1Dqu(); panduan2Dqu();

panduan1Equ(); panduan2Equ();

);delay_n_s(qian_yanshi+1);

青岛农业大学机电工程学院本科毕业设计（论文）

panduan1Fqu(); panduan2Fqu();

panduan1Gqu(); panduan2Gqu();

panduan1Hqu(); panduan2Hqu();

Sec_Stop();}

void Open_chuankou()

{//_EINT();

}

void Xunhuanqianjin()

{

Fir_zuo();

delay_n_s(6);

Zong_X=Zong_X+6;

Fir_Stop();

Sec_shang();

delay_n_s(2);

Zong_Y=Zong_Y+2;

Sec_Stop();

Fir_you();

delay_n_s(6);

Zong_X=Zong_X-6;

Fir_Stop();

Sec_shang();

delay_n_s(2);

Zong_Y=Zong_Y+2;

Sec_Stop();}

void Xunhuanhoutui()

{

Fir_you();

delay_n_s(6);

Zong_X=Zong_X-6;

Fir_Stop();

Sec_xia();

delay_n_s(2);

Zong_Y=Zong_Y-2;

Sec_Stop();

Fir_zuo();

delay_n_s(6);

Zong_X=Zong_X+6;

Fir_Stop();

Sec_xia();

delay_n_s(2);

Zong_Y=Zong_Y-2;

IE1 |= URXIE0; }

void Close_chuankou()

{//_DINT();

IE1 &= ~URXIE0; }

/**************************************/

void Check_jiexian()

{

if((P3IN & 0x01)==0x00)

{

Fir_Stop();

zuo_flag=1;}

else zuo_flag=0;

if((P3IN & 0x02)==0x00)

{Fir_Stop();

you_flag=1;

Zong_X=0;}

else you_flag=0;

if((P3IN & 0x04)==0x00)

{Sec_Stop();

shang_flag=1;}

else shang_flag=0;

if((P3IN & 0x08)==0x00)

{Sec_Stop();

xia_flag=1;

Zong_Y=0;}

else xia_flag=0;

if((P6IN & 0x02)==0x00)

{Thir_Stop();

qian_flag=1;}

else qian_flag=0;

if((P6IN & 0x04)==0x00)

{Thir_Stop();

hou_flag=1;

Zong_Z=0;}

else hou_flag=0;

if((P6IN & 0x80)==0x00)

青岛农业大学机电工程学院本科毕业设计（论文）

{jiajin_flag=1;}

else jiajin_flag=0;}

// Watchdog Timer interrupt service routine

#pragma vector=WDT_VECTOR

__interrupt void watchdog_timer(void)

{ _EINT();

Check_jiexian();

Check_anjian();

Check_zhogndian();

/*X1=0x30+(Zong_X/100);

X2=0x30+((Zong_X-X1*100)/10);

X3=0x30+(Zong_X-X1*100-X2*10);

Y1=0x30+(Zong_Y/100);

Y2=0x30+((Zong_Y-Y1*100)/10);

Y3=0x30+(Zong_Y-Y1*100-Y2*10);

Z1=0x30+(Zong_Z/100);

Z2=0x30+((Zong_Z-Z1*100)/10);

Z3=0x30+(Zong_Z-Z1*100-Z2*10);

if(j==30)

{j=0;

flag=0;

panduanqujian(); }

}

/**********检测终点，然后出来*************/

void Check_zhogndian()

{if(((you_flag==1)&&(press_zuo_flag==1))==1)

Fir_zuo();

else

if(((zuo_flag==1)&&(press_you_flag==1))==1)

Fir_you();

else

if(((shang_flag==1)&&(press_xia_flag==1))==1)

Sec_xia();

else

if(((xia_flag==1)&&(press_shang_flag==1))==1)

Sec_shang();

else

if(((qian_flag==1)&&(press_hou_flag==1))==1)

Thir_hou();

else

if(((hou_flag==1)&&(press_qian_flag==1))==1)

Thir_qian();

else ;

if(((zuo_flag==1)&&(Aut_Fir_you_flag==1))==1)

Aut_Fir_you();

else

if(((you_flag==1)&&(Aut_Fir_zuo_flag==1))==1)

Aut_Fir_zuo();

else

if(((hou_flag==1)&&(Aut_Thir_qian_flag==1))==1

)

Aut_Thir_qian();

else

if(((qian_flag==1)&&(Aut_Thir_hou_flag==1))==1

)

Aut_Thir_hou();

else

if(((shang_flag==1)&&(Aut_Sec_xia_flag==1))==1

)

Aut_Sec_xia();

else

if(((xia_flag==1)&&(Aut_Sec_shang_flag==1))==1

)

Aut_Sec_shang();

else ;

if(((zuo_flag!=1)&&(Aut_Fir_zuo_flag==1))==1)

Aut_Fir_zuo();

else

if(((you_flag!=1)&&(Aut_Fir_you_flag==1))==1)

Aut_Fir_you();

else

if(((shang_flag!=1)&&(Aut_Sec_shang_flag==1))=

=1)

Aut_Sec_shang();

else

if(((xia_flag!=1)&&(Aut_Sec_xia_flag==1))==1)

Aut_Sec_xia();

青岛农业大学机电工程学院本科毕业设计（论文）

else

if(((qian_flag!=1)&&(Aut_Thir_qian_flag==1))==1

)

Aut_Thir_qian();

else

if(((hou_flag!=1)&&(Aut_Thir_hou_flag==1))==1)

Aut_Thir_hou();

else ;

{press_shang_flag=1;

Write_Cmd(0x9F);

Write_Data('4');

Sec_shang(); }

else

press_shang_flag=0;

if(key_val==0x0a) //向下运动

{press_xia_flag=1;

}

/***********检测哪个按键按下**************/

void Check_anjian()

{Key_Event();

if(key_Flag == 1)

{

key_Flag = 0;

if(key_val==0x0c)

{Write_Cmd(0x9F);

Write_Data('0');

Xunhuanhoutui();}

if(key_val==0x0d)

you_flag=1;

{press_zuo_flag=1;

Write_Cmd(0x9F);

Write_Data('1');

Fir_zuo();}

else

press_zuo_flag=0;

if(key_val==0x09)

{press_you_flag=1;

Write_Cmd(0x9F);

Write_Data('2');

Fir_you();}

else

press_you_flag=0;

if(key_val==0x05)

{Write_Cmd(0x9F);

Write_Data('3');

Fir_Stop();}

if(key_val==0x0e)

Write_Cmd(0x9F);

Write_Data('5');

Sec_xia();}

else

press_xia_flag=0;

if(key_val==0x06){

Write_Cmd(0x9F);

Write_Data('6');

Sec_Stop();}

if(key_val==0x0f)

{press_qian_flag=1;

Write_Cmd(0x9F);

Write_Data('7');

Thir_qian(); }

else

press_qian_flag=0;

if(key_val==0x0b)

{press_hou_flag=1;

Write_Cmd(0x9F);

Write_Data('8');

Thir_hou(); }

else

press_hou_flag=0;

if(key_val==0x07){

Write_Cmd(0x9F);

Write_Data('9');

Thir_Stop();}

if(key_val==0x10){

Write_Cmd(0x9F);

Write_Data('*');

Xunhuanqianjin();}

青岛农业大学机电工程学院本科毕业设计（论文）

if(key_val==0x08){

Write_Cmd(0x9F);

Write_Data('#');

Close_chuankou();}

if(key_val==0x01)

{Write_Cmd(0x9F);

Write_Data('A');

Get_Fruit();}

if(key_val==0x02)

{Write_Cmd(0x9F);

Write_Data('B');

Open();}

if(key_val==0x03)

{Write_Cmd(0x9F);

Write_Data('C');

Close();}

if(key_val==0x04)

{Write_Cmd(0x9F);

Write_Data('D');

Open_chuankou();}}}

/****************运行补偿*****************/

void yunxing_buchang()

{

if(zuo==1)

{Aut_Fir_you();

delay_n_s(1); Aut_Fir_Stop();}

else if(you==1)

{

Aut_Fir_zuo();

delay_n_s(1); Aut_Fir_Stop();}

else if(shang==1)

{Aut_Sec_xia();

delay_n_s(1); Aut_Sec_Stop();}

else if(xia==1)

{Aut_Sec_shang();

delay_n_s(1); Aut_Sec_Stop();}

else ;

}

/***********延时判断深度信息************/

Bool yanshi_panduan_shendu()

{All_Stop();

delay_n_s(2);

if(((ZZ>90)&&(ZZ<120))==1)

return 1;

else return 0;}