串联式机械臂小车

一、串联式机械臂概括

1.应用领域总结分析

近年来，随着工业自动化水平的飞速发展，工业机器人在现代机械制造中发挥着越来越重要的作用，而串联机械臂作为工业制造领域的关键载体对航天、汽车等制造领域正起着越来越重要的作用，尤其在复杂和环境恶劣的作业条件下，更扮演着不可替代的角色。串联机械臂作为一种多学科高度综合的产品，集成了机械、计算机电子、自动控制理论等于一体，其技术水准直接反映了国家工业制造水平，目前广泛应用于汽车及汽车零部件制造业、机械加工行业、木材与家居制造业等领域，提高了各加工制造业的作业效率。比如焊接机器人在机械加工领域中代替传统的人工焊接，不仅提高了焊接效率和焊接精度，而且解决了工人在焊接恶劣条件的安全问题。

由于具有较高的灵活性和多自由度，串联机械臂在许多领域都有广泛应用：

（1）自动化生产线：串联机器人在汽车制造、电子产品组装、食品加工等自动化生产线上发挥重要作用。它们可以完成精细的装配、焊接、喷涂等工作，提高生产效率和产品质量。

（2）医疗和健康：串联机器人在手术、康复辅助和医疗器械制造等领域得到应用。它们可以进行精确的手术操作、康复训练和辅助活动，提高手术精度和患者疗效。

（3）仓储和物流：串联机器人在仓储和物流领域可以完成货物的搬运、分拣和装载等任务。

（4）精密加工：串联机器人在精密加工领域扮演着重要角色，如机械零件加工、精密雕刻和模具制造等。其高精度和灵活性使其能够进行微小尺寸和复杂形状的加工操作。

（5）实验室研究：串联机器人在科学研究和实验室应用中具有广泛的应用。例如，在化学实验、材料测试和生物医学研究中，串联机器人可以进行精确的液体分注、样品处理和实验操作。

（6）危险环境：串联机器人可以在危险环境中代替人工进行操作，如核能厂、爆炸物处理和深海勘探等。它们能够承担高温、高压、有毒或放射性环境下的任务，保护人类的安全。

（7）娱乐和艺术：串联机器人还在娱乐和艺术领域展现出其创造力和表演能力。例如，在舞台表演、电影特效和艺术创作中，串联机器人可以呈现出流畅的舞蹈动作、惊人的表演和艺术装置。

2.机械臂技术研发需求

随着中国工业机器人市场需求的不断增加，开发完全自主的工业机器人控制系统具有不可替代的意义。目前衡量工业机器人性能的重要标准是运行高速以及加工高精度，而这些也是市场非常看重的。其中工业机器人的运动精度和平稳性的基础和保障是平滑的位姿运动，同时速度规划算法的选择决定了机器人运动精度和平滑性，因此研究机器人的前瞻算法和轨迹规划控制算法对于提高机器人的控制精度和运行效率具有重要意义。

（1）机器人轨迹光顺技术

轨迹光顺是机器人轨迹规划中一个不可或缺的部分，通过各种曲线对运行路径进行近似，在满足轨迹弓高误差的情况下，生成新的运动轨迹，消除曲率和切向的不连续性，满足轨迹连续性要求，目前在三轴和五轴机床上应用非常广泛。

（2）机器人奇异位形规避技术

奇异位形是指机构在运动过程中机构的运动学、动力学性能发生瞬间突变，机构处于死点或者自由度减少，使得机构运动能力失常。串联机器人的运动奇异性由其串联机构所决定的，无法消除，产生的不良影响主要表现在两个方面：①自由度减少；②某些关节角速度趋向无穷大，引起机器人失控。

（3）机器人同步前瞻技术

速度前瞻是数控加工技术中的重要一环，基本思想是通过预读一段待运行路径，判断该路径上的高曲率约束点和危险点，提前进行速度规划，保证机床末端运行至危险点之前能够降速至合适速度，平稳过渡危险点后在加速至正常速度。

（4）机器人速度规划和插补技术

速度规划和插补是机器人运动控制中不可缺少的一部分，已知当前段始末点速度和位移，通过速度规划计算出当前段的运动时间，然后插补出每一循环周期的位移。常见的速度规划算法有直线加减速，也被称为梯形加减速，Ｓ曲线加减速，修正梯形加减速，指数型加减速，三角函数加减速等。

二、串联机械臂机构分析

1.机械臂结构分析

通过搭建组装机械臂平台，分析其基本结构与传动方式，并对部件具体尺寸进行测量，可以得到如下的机构简图，并建立对应的工具坐标：

其中，经过多次测量取平均值，测得如下物理结构参数（图中已标注)：

表1：机械臂物理参数

	数值（mm）
L1	152
L2	105
L3	98
L4	182

2.机械臂正运动学模型

基于上述坐标系，进一步分析其传动关系与结构，得出如下D-H参数表：

表2：D-H参数表

参数\关节	0	1	2	3	4	5	6
a_i	0	0	L2	L3	0	0
d_i		L1	0	0	0	0	L4
α_i	0	-90°	0	0	-90°	0
θ_i		θ1	θ2	θ3	θ4-90°	θ5	0

通过如下变化通式，可以在各连杆的关节坐标系之间进行变换：

代入D-H表中对应的参数值即可计算得出对应的位姿矩阵，表中的θ_i（i=1,2,3,4,5）即表示舵机转角（最后一个舵机仅控制夹子夹紧，不影响总体位姿，默认为夹紧状态），在机械臂自身物理参数已经确定的情况下，一旦给定各舵机转角，机械臂末端在全局坐标系中的坐标也可立即得到。在本项目的六轴机械臂模型中，共可建立六个坐标系，得到六个相邻坐标系之间的位姿矩阵T；将六个位姿矩阵按顺序依次右乘，即可得到机械臂末端相对于0坐标系（即全局坐标系）的位姿矩阵:

该矩阵中前三行的最后一个元素分别代表了机械臂末端在全局坐标系中的x,y和z坐标。依据这样的原理，通过Matlab编程，可以实现正运动学的计算（附件1），例如设定五个舵机转角分别为0°、-45°、45°、45°、0°，可以计算得出如下的位姿矩阵：
进一步可以绘制出此时各机械臂关节的位姿图，与实际控制结果进行比照：

3.机械臂逆运动学求解

通过观察与分析机械臂结构可以发现，由于θ5只影响末端夹子的转动，θ6只影响末端夹子夹紧的程度，即在固定θ5=0°、θ6=90°（夹子夹紧）时，机械臂的位姿与末端坐标只受θ1、θ2、θ3、θ4的控制，故只需求解该四者即可。

（1）求解θ1

观察连杆简图（上左图），可以发现: θ1为机械臂在x-y平面中的投影与x轴的夹角，即只与机械臂末端的x与y坐标有关：θ1=arctan(yP*/x*P)。

（2）求解θ2、θ3、θ4

进一步观察机械臂所在平面（上右图）：将原坐标系中的z轴作为纵坐标（y’ = z），将原x轴与y轴映射到机械臂所在平面内作为横坐标（x’ = sqrt(x^2+y^2))；利用几何法，建立方程组对θ2、θ3、θ4分别进行求解。

在求解公式推导的过程中，默认末端机械臂与水平面的夹角α已知，在给定机械臂末端坐标（x,y,z）的情况下，可解得如下结果（必须求出解析解，若将方程组输入Matlab调用求解器求解，可能会出现计算无解但实际有解的情况）：

其中：

事实上，这样求解得到的结果往往不唯一，为使得机械臂在各位姿转换的过程中更加直接迅速且满足机械结构约束，还需满足如下约束条件：

【1】求解位置不能低于底座：

【2】三角形ABC存在的几何约束

【3】三角形ACC’存在的几何约束

【4】运动连续性约束：

【5】角度范围约束：

实际编写Matlab程序进行求解时，末端机械臂与水平面的夹角α在不同位姿下会发生改变，因此需要在编程时对其在一定范围（-45°到90°之间）内进行遍历，当有合适的满足约束条件的解时输出对应的解并停止循环。

在Matlab程序（附件2）中，将目标机械臂末端坐标[x,y,z]设置为以IN_theta=[0,-45,45,45,0]作为输入的正解结果[300.9396, 0, 97.5528]，得到的逆解结果与IN_theta完全一致，正确性得到验证。

三、串联机械臂控制技术分析

1.电机特性分析

从本质上来说，电机属于一种能量转换装置，其目的是希望将电能转换为动能（机械能），但在实际的工作过程中这样的转换效率永远不会是100%，而是由于电机线圈的欧姆加热，会有相当部分的能量以热量形式散失；但总而言之，其工作过程本身仍然满足能量守恒定律，即其将输入的电能P_elec转换为输出的机械能P_mech与散失的热能P_heat：

图6：电机理论模型

将其改写成电气和机械量：

其中v_m为电机端子两端的电压，i是流过电机的电流，τ是电机产生的扭矩，ω是其角速度。

事实上，除此之外，电机内的线圈除具有电阻R之外，还具有电感L。加入电感项后，再将两边同时对电流i求导，可以得到电机的定义方程：

其中v_emf为反电动势，与电机转速n成正比，即（其中常数k_v取决于电机设计）:

电机产生的转矩与通过线圈的电流成正比，即：

但实际的输出扭矩还需要在此基础上减去负载扭矩与电机轴摩擦扭矩（与电机轴转速成正比，比例系数为电机自身特性)。

基于上述这些公式即可对于一个电机的特性与性能进行相对完整描述，通过对于上述公式进行变形和转换还可以得到电机的速度-扭矩曲线，从而得到失速扭矩与空载速度，为我们更好地把握电机的大致性能提供参考。

为了后续更精准地对电机进行控制（如调节PID参数等），可以在Simulink中对电机的基本物理特性进行仿真建模：

该模型的输入值为电机电压和负载扭矩，输出值为电机转角、电流与转速，能够很好地描述一个电机的物理特性，可以将其封装成单独的电机模块，并利用PID等控制模块实现对于该电机模块的控制。

不同的电机由于其结构、材料等的不同，性能也会有所差异，具体体现在不同的结构和材料对于上述公式中所涉及到的比例系数都有不同程度的影响，因而能够影响电机的速度-扭矩特性曲线。在对电机进行选型时，厂家往往不会直接给出这些系数，而是给出空载速度等参数以供参考，因此更需要清楚了解这些特性参数所代表的物理意义，从而更好地针对需求进行合适的选型。

2.电机控制策略与PID特性分析

PID控制器是工业过程控制中广泛采用的一种控制算法，其特点是结构简单灵活、技术成熟、适应性强。在有一定精度和灵敏度要求的电机控制过程中，往往也偏向于采用PID控制器实现电机控制。

P、I、D分别为比例（Proportion）、积分（Integral）、微分（Differential)的简写；将偏差的比例、积分和微分通过线性组合构成控制量，用该控制量对受控对象进行控制，称为PID算法，本质上是对于偏差的控制算法。PID通过对输入的偏差进行比例积分微分运算，将其叠加结果用于控制执行机构。在工程实践中，一般P是必须的，衍生出有PI、PD、PID等多种PID控制器。

连续状态下，PID控制器的计算公式如下：

（1）比例部分

作用是对系统瞬间产生的偏差进行快速修正，只有P时会有静差。偏差一旦产生，控制器立即产生控制作用，使控制量向减少偏差的方向变化。比例系数K_p越大，控制作用越强，则过渡过程越快，控制过程的静态偏差也就越小；但是越大，也越容易产生震荡，破坏系统的稳定性。因此，比例系数必须选择恰当，才能达到过渡时间少，静差小且稳定的效果。

（2）积分部分

作用是消除系统偏差，补偿系统的静态误差，只要存在偏差，则其控制作用就不断增加；只有在无偏差时，其积分值才为常数，控制作用才是一个不会增加的常数。积分环节的调节作用虽然会消除静态误差，但也会降低系统的响应速度，增加系统的超调量。积分常数越大，积分的积累作用越弱，这时系统在过渡时才不会产生震荡；但增大积分常数会减慢静态误差的消除过程，消除偏差所需的时间较长，但可以减少超调量，提高系统的稳定性。

（3）微分部分

作用是加快调节过程，阻止偏差的变化。偏差变化的越快，微分控制器的输出就越大，并能在偏差值变大之前进行修正。微分作用的引入，将有助于减少超调量，克服震荡，使系统趋于稳定，特别对高阶系统非常有利，它加快了系统的跟踪速度。但微分的作用对输入信号的噪声很敏感，对那些噪声较大的系统一般不用微分，或在微分起作用之前先对输入信号进行滤波。

总体而言，在PID控制器参数选择的过程中，一般遵循如下步骤顺序：

【1】确定比例系数Kp

【2】确定积分时间常数Ti

【3】确定微分时间常数Td

【4】系统空载、带载联调

3.嵌入式控制系统总结分析

嵌入式系统是以应用为中心，以现代计算机技术为基础，能够根据用户需求(功能、可靠性、成本、体积、功耗、环境等)灵活裁剪软硬件模块的专用计算机系统。嵌入式系统的硬件和软件必须根据具体的应用任务，以功耗、成本、体积、可靠性、处理能力等为指标来进行选择。嵌入式系统的核心是系统软件和应用软件，由于存储空间有限，因而要求软件代码紧凑、可靠，且对实时性有严格要求。目前常见的嵌入式控制开发板有C51，STM32，Arduino，树莓派等。

本项目中使用的嵌入式控制系统基于Arduino Uno开发板搭建。作为一款便捷灵活、方便上手的开源硬件产品，其具有丰富的接口，有数字I/O口，模拟I/O口，同时支持SPI,IIC,UART串口通信，能通过各种各样的传感器来感知环境，通过控制灯光、马达和其他装置来反馈、影响环境。它没有复杂的单片机底层代码，没有难懂的汇编，只是简单而实用的函数。而且具有简便的编程环境IDE，极大的自由度，可拓展性能非常高，同时其标准化的接口模式也为它的可持续发展奠定了坚实的基础，但其低廉的价格也意味着性能并不尽如人意。

4.传感系统总结分析

传感器是能感受到被测量的信息，并能将感受到的信息，按一定规律变换成为数字/模拟信号或其他所需形式的信息输出，以满足信息的传输、处理、存储、显示、记录和控制等要求的检测装置。传感器具有微型化、数字化、智能化、多功能化、系统化、网络化等特点，它是实现自动检测和自动控制的首要环节。

传感器一般由敏感元件、转换元件、变换电路和辅助电源四部分组成：敏感元件直接感受被测量，并输出与被测量有确定关系的物理量信号；转换元件将敏感元件输出的物理量信号转换为电信号；变换电路负责对转换元件输出的电信号进行放大调制；转换元件和变换电路一般还需要辅助电源供电。

按照其测量的物理量，传感器可大致分为如下种类：

【1】力学量传感器：包括压力传感器、力传感器、位移传感器、加速度传感器、速度传感器、转速传感器等，用于测量力、压力、运动等力学参数。

【2】热学量传感器：热敏电阻/热电偶/红外温度传感器等，用于测量温度。

【3】光学量传感器：如光电二极管、CCD/CMOS图像传感器等，用于检测光强度、颜色、图像信息。

【4】声学量传感器：麦克风，用于声音、噪声的检测。

【5】磁学量传感器：如霍尔效应传感器、磁敏电阻，用于测量磁场强度、磁通量。

【6】化学量传感器：如电化学传感器、气体传感器，用于检测特定气体浓度、pH值、离子浓度等。

本项目中用到的传感器主要有三类：

（1）红外光学传感器：不断发出红外线并通过检测返回的红外线强弱返回不同的电信号，可用于区分亮处/暗处，通过与对应的控制算法相配合可以使小车实现沿黑线完成循迹任务；

（2）超声波测距传感器：不断向前方发出超声波并接收反射回的超声波，通过检测发送与接收同一超声波信号的时间间隔，计算得出前方物体与自身的距离，进行距离的判断，可配合相应控制算法实现对应功能；

（3）视觉传感器：采用Openmv视觉摄像头模块，对摄像头采集的图像进行实时的处理，需要配合编写对应的识别算法并烧录进去，并将识别结果以数字信号的形式反馈到指定的引脚上，与主控制板实现通信，从而根据不同的识别结果执行对应命令。

四、综合实践环节报告

1. 任务目标与任务分解

任务目标：

任务分解：

总体而言，可以将整个项目分解成如下模块：

（1）利用红外传感器模块，实现对于黑线道路的循迹；

（2）利用超声波测距模块与视觉模块，在对应的任务点停下，并根据视觉识别结果执行对应的任务；

（3）在A点处，根据识别到的物块颜色判断应进行左转还是右转；在B点出，根据识别到的物块大小，控制机械臂沿不同的指定轨迹完成运动（写字）。

简而言之，任务主要分为循迹、视觉识别与机械臂控制运动三个模块。

2. 本人负责部分的实现过程

（1）任务描述

我主要负责的任务板块是最终B点处的写字任务，即需要在接收到Openmv模块识别反馈的物块大/小对应的电平信号后，执行对应的机械臂运动，最终达成在墙上写数字”1”或”2”的效果；由于该实现过程中大量涉及到对于Arduino编写C++程序并烧录以实现舵机控制的内容，同时还需要与Openmv视觉模块进行交互，因此我也一并负责了整个测试小车行进业务逻辑的编写与整合工作，将各个模块整合在一起。

（2）技术路径与策略

在之前的机械臂控制（正/逆解）过程中，由于不涉及到写字任务这一具体情境，只是对于实验平台基本的物理参数进行了测量；但在写字任务中，机械臂末端的夹子需要夹取白板笔，用白板笔的尖端在墙壁上完成写字任务，因此需要对于机械臂的L4参数进行重新测量，端点由夹子末端更改至白板笔的末端，测得新的L4为250mm。

针对写”1”和”2”两个不同的字符，有不同的定位方式：如写”1”只需定位上端点，中间点与下端点即可，而写”2”则根据字形需要定位6个点才能完成任务（为简化控制流程，将各控制点之间的距离设置的较小从而不需要在控制点之间再进行直线插补，目标是仅需要完成字形而并不追求大小）。附件3中的综合控制代码中给出了这些控制点的坐标，在此不详细赘述。事实上，这些点坐标的确定还与小车在停下识别时与墙壁间的距离（即超声测距决定停下识别时的临界距离）有关，实际在反复多次的测试中磨合得出。

为使得机械臂控制流程更加流畅和自然，在基于Matlab实现机械臂正逆解算的代码后，将该算法移植至C++中，并以类的形式封装（名为Arm）,一个Arm对象即对应一个位姿，其构造时需要输入机械臂的结构参数（L1,L2,L3,L4）与希望机械臂末端到达的点坐标，当再次调用其setup()方法时，会自动根据该目标坐标进行逆解算，并控制舵机按照解算出的角度转动，实现控制效果。在舵机控制时，综合了测量出的转动误差，对于各舵机的转动角度都有对应的不同的比例系数实现误差补偿。

（3）核心程序逻辑

最终完整项目的代码详见附件3，在此仅简要阐述其逻辑。

小车的默认状态为以循迹方式前进，即执行follow()函数；在行进过程中，始终构造一个Arm对象（在armset()函数中），控制机械臂保持一个相对合适的姿态（坐标280，0，320）；当超声波测距返回值小于19时，说明到达了A点，需要进行第一次识别，此时会反馈给Openmv一个高电平，告诉视觉模块需要开始识别颜色；视觉模块在识别完成后通过另一个引脚将识别结果以高/低电平形式反馈给主控Arduino开发板，主板在接收到对应信号后会对应做出向左/向右转的行动（执行turningleft()或turningright()）函数，并在随后的过程中继续完成循迹；在T字路口处，由于我们组并未采取视觉辅助循迹的解决方案，因此只能采取”背板”的方式，在A点处转弯后开始计时，当继续循迹达到30s左右时，根据前面判断执行的左转/右转指令对应再次执行左转/右转；但实际操作过程中”背板”往往无法在路口精准转弯，因此又补充了新的逻辑，在时间差不多的时候提前大角度拐弯并一直沿直线行驶，直到再次回到黑线上就继续循迹，这样就可以绕过T字路口；理想状态下，通过T字路口后再行驶短暂的一段距离后，应该就到达了需要写字的B点，此时与A点处处理类似，当超声测距返回值小于17时停下，通过Openmv模块识别红色物块的大小，并执行对应的机械臂运动操作（写”1”或”2”）。

（4）实现的实际效果

上述操作逻辑仅仅在理论上可行，而实际测试时，由于没有采用视觉循迹方案，在T字路口屡败屡战，屡战屡败，最终在多次尝试后仍然没有进入T字弯后的B点写字区域；在第二次尝试过程中，由于个人的失误，在补充了绕过T字弯的逻辑之后忘记补充循迹逻辑，导致在到达A点后失去循迹功能，最终宣告失败。但总体而言，除了T字弯之外，其他所有功能的实现都非常良好，不论是前段的循迹，把速度稍微压下来一点但是比较稳不会跑出去，还是A点的颜色判断也非常准确而迅速。在私下练习测试时，物块大小识别与写字功能也非常流畅，正确率极高，只是可惜最后没有办法展示出来了。

4. 心得体会

其实整个课程与项目过程中的心路历程和工作内容在上面已经基本讲完了，因为我没有打过机器人相关的竞赛，相比于其他同学而言少了很多这方面的经验，不论是这次课程中涉及到的机械臂正逆解算与控制也好，还是视觉模块的引入也好，甚至是超声波测距模块其实对我来说都是初次接触，都是一个迅速的从陌生到熟悉的过程，这个成长过程很突然但也很迅速，效果是拔群的。在后半学期的实践阶段，我一直在努力去主导我们整个组进度的推进，之前课上一些小的checkpoint也都顺利通过了；最终测试中也基本是我在主导整个组进度的推进，以及各模块任务的分配与整体业务逻辑的串联，而且由于我一直在跟随堂测试的机械臂控制部分同时也想充分利用我Matlab代码编写与数据处理计算的优势，我在最终测试中还主要负责了机械臂的控制部分（写字），巧妙利用C++面向对象的编程思想，大大简化了控制流程。但可惜由于我们组经验和技术水平比较有限，没有及时认识到视觉循迹的必要性，导致最终在T字路口功亏一篑，甚至使得最早调完的写字部分前功尽弃，最后一次测试本来调整了逻辑企图通过”歪门邪道”绕过T字弯（纯背板真的太难卡准了），但是由于个人失误导致在A点就提前结束了战斗，也是留下了一些遗憾吧。但总体来说，整个过程给我带来的成长和提升是显而易见的，我开始对于机器人、对于硬件慢慢地从心理上去接受这个事情并且甚至一度乐在其中，也学到了机械臂控制、电机控制等方面的一些知识，课上第一个把电机Simulink模型搭对确实给我增长了很多信心。我们组在编程的过程中除了直接移植部分模块外几乎没有用过Mixly图形化编程，而是直接使用Arduino IDE（虽然这好像也是最后逻辑不清晰留下遗憾的原因之一），这有效地提高了我们的开发效率。这一次从装车开始，到一步步熟悉起小车和开发板的每一个部分，看着程序从一行两行到最后近千行，整体功能不断扩充从一开始只有机械臂位姿控制，到加上循迹，加上与Openmv模块的通信逻辑，以及最后面对T字路口的垂死挣扎，其实在准备过程中也尝试去做视觉辅助循迹，但由于场地各方面条件限制（场地太拥挤，调试的人太多导致环境光源不稳定；摄像头安装位置不合适等），最终还是放弃了视觉循迹方案，选择背板赌一把，很可惜没有成功。

希望在以后的学习实践中，能够充分吸取本次项目的经验，利用好这次学到的理论知识，同时也在做事的过程中汲取这次的经验教训，我相信努力的过程与收获比结果更重要，当然如果能有好的结果也再好不过。感谢这门课一路上三位任课老师与助教学长的辛勤付出，是你们的倾情指导加速了我们成长的速度。

千言万语诉不尽收获与感激。最后，感谢一直努力的自己，以及一直陪伴的老师和朋友。

附件1：Matlab正运动学解算代码

clear;clc;

%赋予输入值
L1=152;
L2=105;
L3=98;
L4=182;
IN_theta=[0,-45,45,45,0];

%建立D-H参数表
C_a=[0,0,L2,L3,0,0];
C_d=[0,L1,0,0,0,0,L4];
C_alpha=[0,-90,0,0,-90,0];
C_theta=[0,IN_theta(1),IN_theta(2),IN_theta(3),IN_theta(4)-90,IN_theta(5),0];

i=1;%i-1=0
T0_1=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);
    cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));
    sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));
    0,0,0,1];
i=2;
T1_2=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];
i=3;
T2_3=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];
i=4;
T3_4=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];
i=5;
T4_5=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];
i=6;
T5_6=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];

x0=[0;0;0];
x1=T0_1(1:3,4);
T0_2=T0_1*T1_2;
x2=T0_2(1:3,4);
T0_3=T0_1*T1_2*T2_3;
x3=T0_3(1:3,4);
T0_4=T0_1*T1_2*T2_3*T3_4;
x4=T0_4(1:3,4);
T0_5=T0_1*T1_2*T2_3*T3_4*T4_5;
x5=T0_5(1:3,4);
T0_6=T0_1*T1_2*T2_3*T3_4*T4_5*T5_6;
x6=T0_6(1:3,4);
% 将点的坐标分别存储在 x, y, z 向量中
x = [x0(1), x1(1), x2(1), x3(1), x4(1), x5(1), x6(1)];
y = [x0(2), x1(2), x2(2), x3(2), x4(2), x5(2), x6(2)];
z = [x0(3),x1(3), x2(3), x3(3), x4(3), x5(3), x6(3)];

% 绘制三维线
figure; % 创建一个新的图窗
plot3(x, y, z, '-o'); % 使用 '-o' 绘制带有圆圈标记的线
grid on; % 打开网格
xlabel('X-axis');
ylabel('Y-axis');
zlabel('Z-axis');
title('3D Line Plot Connecting Points');

T0_6

附件2：Matlab逆运动学解算代码

clear;clc;
% 初始化标志变量
solution_found = false;
x = 300.9396;  
y = 0;
z = 97.5528;
theta1=rad2deg(atan(y/x));
disp(theta1);
L1 = 152;  L2 = 105;  L3 = 98;   L4 = 182;  
% 遍历 alpha 的值，从 -45 到 90
for alpha = -45:90
    t = sqrt(x^2 + y^2);
    m=z-L1-L4*sind(alpha);
    n=t-L4*cosd(alpha);
    theta3=acosd((m^2+n^2-L2^2-L3^2)/(2*L2*L3));
    A=L3*sind(theta3);
    B=L2+L3*cosd(theta3);
    theta2=asind(n/sqrt(A^2+B^2))-acosd(A/sqrt(A^2+B^2));
    if theta2<0
        theta2=180-asind(n/sqrt(A^2+B^2))-acosd(A/sqrt(A^2+B^2));
    end
    theta4=theta2-theta3-alpha;
    % 检查是否找到解
    if L4*sind(alpha)<z && L3*cosd(theta2-theta3)>0 && theta2>0 && theta2<90 && theta3>0 && theta3<90 && theta4>0 && theta4<90 
        % 记录找到解的标志
        solution_found = true;
        break;
    end
end
if solution_found
    % 显示结果
    disp('Solution found:');
    disp(['Alpha: ', num2str(alpha)]);
    disp('Theta1:');    disp(theta1);
    disp('Theta2:');    disp(-theta2);
    disp('Theta3:');    disp(theta3);
    disp('Theta4:');    disp(theta4);
else
    disp('No solution found in the given alpha range.');
end

附件3：项目测试完整Arduino控制代码

#include <math.h>
#include <stdio.h>
#include <Servo.h>

Servo servo_7;
Servo servo_3;
Servo servo_5;
Servo servo_6;
Servo servo_9;

const double L1 = 152;
const double L2 = 105;
const double L3 = 98;
const double L4 = 250;

volatile int item;

int flag_cp1=0;
int flag_cp2=0;

unsigned long currentTime;
unsigned long elapsedTime;
unsigned long startTime;

char cmd_return_tmp[64];

int color;
int size;

struct Angles 
{
    double theta1;
    double theta2;
    double theta3;
    double theta4;
};

struct Points 
{
    double x1;
    double y1;
    double z1;
};

class Arm 
{
  public:
      Arm(double L1, double L2, double L3, double L4, Points terminal) 
      {
          this->L1 = L1;
          this->L2 = L2;
          this->L3 = L3;
          this->L4 = L4;
          this->x0 = terminal.x1;
          this->y0 = terminal.y1;
          this->z0 = terminal.z1;
          Angles angles=inverseKinematics(this->x0,this->y0,this->z0);
          this->theta1=angles.theta1;
          this->theta2=angles.theta2;
          this->theta3=angles.theta3;
          this->theta4=angles.theta4;
          Points point=forwardKinematics(this->theta1,this->theta2,this->theta3,this->theta4);
          this->x1=point.x1;
          this->y1=point.y1;
          this->z1=point.z1;
      }

      void run() 
      {
          double oc1=(140.25+1.12*this->theta1)*2/3;
          double oc2=(180+1.13*this->theta2)*2/3;
          double oc3=(142.5+1.07*this->theta3)*2/3;
          double oc4=(132+1.12*this->theta4)*2/3;
          servo_7.write(oc1);
          servo_3.write(oc2);
          servo_5.write(oc3);
          servo_6.write(oc4);
          servo_9.write(96);
          //servo_8.write(270);
          Serial.print("x1: "); Serial.println(this->x1);
          Serial.print("y1: "); Serial.println(this->y1);
          Serial.print("z1: "); Serial.println(this->z1);
          Serial.print("terminalx1: "); Serial.println(this->x0);
          Serial.print("terminaly1: "); Serial.println(this->y0);
          Serial.print("terminalz1: "); Serial.println(this->z0);
      }

  private:
      double L1;
      double L2;
      double L3;
      double L4;
      double x0;
      double y0;
      double z0;
      double theta1;
      double theta2;
      double theta3;
      double theta4;
      double x1;
      double y1;
      double z1;
      bool solution_found = false;
      Angles inverseKinematics(double x0, double y0, double z0) 
      {
          Angles angles;
          double theta1 = atan2(y0,x0) * 180.0 / PI;
          angles.theta1=theta1;

          for (int alpha = -45; alpha <= 90; alpha++) 
          {
              double t0 = sqrt(x0 * x0 + y0 * y0);
              double m = z0 - this->L1 - this->L4 * sin(alpha * PI / 180.0);
              double n = t0 - this->L4 * cos(alpha * PI / 180.0);

              double cos_theta3 = (m * m + n * n - this->L2 * this->L2 - this->L3 * this->L3) / (2 * this->L2 * this->L3);
              if (cos_theta3 < -1.0 || cos_theta3 > 1.0) 
                  continue;
              double theta3 = acos(cos_theta3) * 180.0 / PI;
              double A = L3 * sin(theta3 * PI / 180.0);
              double B = L2 + L3 * cos(theta3 * PI / 180.0);
              double sin_theta2_part = n / sqrt(A * A + B * B);
              double cos_theta2_part = A / sqrt(A * A + B * B);

              if (sin_theta2_part < -1.0 || sin_theta2_part > 1.0 || cos_theta2_part < -1.0 || cos_theta2_part > 1.0) 
                  continue;

              double theta2 = asin(sin_theta2_part) * 180.0 / PI - acos(cos_theta2_part) * 180.0 / PI;
              if (theta2 < 0) 
                  theta2 = 180.0 - asin(sin_theta2_part) * 180.0 / PI - acos(cos_theta2_part) * 180.0 / PI;
              double theta4 = theta2 - theta3 - alpha;

              if (this->L4 * sin(alpha * PI / 180.0) < z0 && this->L3 * cos((theta2 - theta3) * PI / 180.0) > 0 && theta2 >= 0 && theta2 <= 90 && theta3 >= 0 && theta3 <= 90 && theta4 >= 0 && theta4 <= 90) 
              {
                  solution_found = true;
                  angles.theta2=-theta2;
                  angles.theta3=theta3;
                  angles.theta4=theta4;
                  break;
              }
          }
          if (solution_found)
            return angles;
      }

      Points forwardKinematics(double theta1, double theta2, double theta3, double theta4) 
      {
          Points point;
          double IN_theta[] = {theta1, theta2, theta3, theta4, 0};

          // 定义DH参数
          double C_a[] = {0, 0, this->L2, this->L3, 0, 0};
          double C_d[] = {0, this->L1, 0, 0, 0, 0, this->L4};
          double C_alpha[] = {0, -90, 0, 0, -90, 0};
          double C_theta[] = {0, IN_theta[0], IN_theta[1], IN_theta[2], IN_theta[3] - 90, IN_theta[4], 0};

          // 计算变换矩阵
          double T0_1[4][4], T1_2[4][4], T2_3[4][4], T3_4[4][4], T4_5[4][4], T5_6[4][4];
          double T0_2[4][4], T0_3[4][4], T0_4[4][4], T0_5[4][4], T0_6[4][4];

          dhMatrix(T0_1, C_theta[1], C_d[1], C_a[0], C_alpha[0]);
          dhMatrix(T1_2, C_theta[2], C_d[2], C_a[1], C_alpha[1]);
          dhMatrix(T2_3, C_theta[3], C_d[3], C_a[2], C_alpha[2]);
          dhMatrix(T3_4, C_theta[4], C_d[4], C_a[3], C_alpha[3]);
          dhMatrix(T4_5, C_theta[5], C_d[5], C_a[4], C_alpha[4]);
          dhMatrix(T5_6, C_theta[6], C_d[6], C_a[5], C_alpha[5]);

          // Initialize T0_6 as identity matrix
          for (int i = 0; i < 4; i++) 
              for (int j = 0; j < 4; j++) 
                  T0_6[i][j] = (i == j) ? 1 : 0;

          // 计算最终的变换矩阵
          multiplyMatrix(T0_1, T1_2, T0_2);
          multiplyMatrix(T0_2, T2_3, T0_3);
          multiplyMatrix(T0_3, T3_4, T0_4);
          multiplyMatrix(T0_4, T4_5, T0_5);
          multiplyMatrix(T0_5, T5_6, T0_6);

          point.x1=T0_6[0][3];
          point.y1=T0_6[1][3];
          point.z1=T0_6[2][3];

          return point;
      }

      void dhMatrix(double T[4][4], double theta, double d, double a, double alpha) 
      {
          double radTheta = theta * PI / 180.0;
          double radAlpha = alpha * PI / 180.0;
          T[0][0] = cos(radTheta); T[0][1] = -sin(radTheta); T[0][2] = 0; T[0][3] = a;
          T[1][0] = cos(radAlpha) * sin(radTheta); T[1][1] = cos(radTheta) * cos(radAlpha); T[1][2] = -sin(radAlpha); T[1][3] = -d * sin(radAlpha);
          T[2][0] = sin(radAlpha) * sin(radTheta); T[2][1] = sin(radAlpha) * cos(radTheta); T[2][2] = cos(radAlpha); T[2][3] = d * cos(radAlpha);
          T[3][0] = 0; T[3][1] = 0; T[3][2] = 0; T[3][3] = 1;
      }

      void multiplyMatrix(double A[4][4], double B[4][4], double C[4][4]) 
      {
          for (int i = 0; i < 4; i++) 
              for (int j = 0; j < 4; j++) 
              {
                  C[i][j] = 0;
                  for (int k = 0; k < 4; k++)
                      C[i][j] += A[i][k] * B[k][j];
              }
      }
};

void turnL() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+400,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+320,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+320,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+320,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(10);
}

void turnR() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+(-400),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(10);
}

void forward() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(40);
}

void stop() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+0,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+0,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+0,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+0,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
}

void turningleft() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500-400,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500-600,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500-400,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500-600,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(700);
}

void turningright() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+(+600),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(+400),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+(+600),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(+400),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(800);
}

void leftturningleft() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500-000,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500-800,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500-000,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500-800,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(750);
}

void leftforward() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(-360),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(-360),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(40);
}

void rightturningright() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+800,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500-000,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+800,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500-000,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(750);
}

void rightforward() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(-360),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(-360),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(40);
}

float checkdistance_A1_A2() 
{
  digitalWrite(A1, LOW);
  delayMicroseconds(2);
  digitalWrite(A1, HIGH);
  delayMicroseconds(10);
  digitalWrite(A1, LOW);
  float distance = pulseIn(A2, HIGH) / 58.00;
  //delay(10);
  return distance;
}

void follow()
{
  if (digitalRead(A4) == 0 && digitalRead(A5) == 1) 
  {
    item = 0;
    forward();
  } 
  else if (digitalRead(A4) == 1 && digitalRead(A5) == 0) 
  {
    item = 1;
    forward();
  } 
  else if (digitalRead(A4) == 0 && digitalRead(A5) == 0) 
    forward();
  else if (item == 0 && (digitalRead(A4) == 1 && digitalRead(A5) == 1)) 
    turnL();
  else if (item == 1 && (digitalRead(A4) == 1 && digitalRead(A5) == 1))
    turnR();
}

void writeone()
{
  Points p1;
  Points p2;
  Points p3;
  p1.x1=345;
  p1.y1=0;
  p1.z1=400;
  p2.x1=345;
  p2.y1=0;
  p2.z1=360;
  p3.x1=345;
  p3.y1=0;
  p3.z1=320;
  Arm arm1(L1,L2,L3,L4,p1);
  arm1.run();
  delay(1000);
  Arm arm2(L1,L2,L3,L4,p2);
  arm2.run();
  delay(1000);
  Arm arm3(L1,L2,L3,L4,p3);
  arm3.run();
  delay(1000);
  flag_cp2 = 3;
}

void writetwo()
{
  Points p1;
  Points p2;
  Points p3;
  Points p4;
  Points p5;
  Points p6;
  p1.x1=345;
  p1.y1=40;
  p1.z1=400;
  p2.x1=345;
  p2.y1=-40;
  p2.z1=400;
  p3.x1=345;
  p3.y1=-40;
  p3.z1=360;
  p4.x1=345;
  p4.y1=40;
  p4.z1=360;
  p5.x1=345;
  p5.y1=40;
  p5.z1=320;
  p6.x1=345;
  p6.y1=-40;
  p6.z1=320;
  Arm arm1(L1,L2,L3,L4,p1);
  arm1.run();
  delay(1000);
  Arm arm2(L1,L2,L3,L4,p2);
  arm2.run();
  delay(1000);
  Arm arm3(L1,L2,L3,L4,p3);
  arm3.run();
  delay(1000);
  Arm arm4(L1,L2,L3,L4,p4);
  arm4.run();
  delay(1000);
  Arm arm5(L1,L2,L3,L4,p5);
  arm5.run();
  delay(1000);
  Arm arm6(L1,L2,L3,L4,p6);
  arm6.run();
  delay(1000);
  flag_cp2 = 3;
}

void armset()
{
  Points p0;
  p0.x1=280;
  p0.y1=0;
  p0.z1=320;
  Arm arm0(L1,L2,L3,L4,p0);
  arm0.run();
}

void setup()
{
  servo_7.attach(7);
  servo_3.attach(3);
  servo_5.attach(5);
  servo_6.attach(6);
  servo_9.attach(9);
  //servo_8.attach(8);
  Serial.begin(115200);
  delay(400);
  item = 2;
  pinMode(A1, OUTPUT);
  pinMode(A2, INPUT_PULLUP);
  pinMode(A5, INPUT_PULLUP);
  pinMode(A4, INPUT_PULLUP);
  pinMode(A0, OUTPUT);
  pinMode(A3, INPUT_PULLUP);
}

int leftflag=0;
int rightflag=0;

void loop()
{
  if (flag_cp2 == 0)
    armset();
  else if (flag_cp2 == 1)
    writeone();
  else if (flag_cp2 == 2)
    writetwo();
  float distance = checkdistance_A1_A2();
  if (distance > 19 && flag_cp1==0)
    follow();
  else if (distance <=19 && flag_cp1==0)
  {
    stop();
    digitalWrite(A0, HIGH);
    delay(3000);
    color = digitalRead(A3);//0-green,1-blue
    if(color == 1)
      turningleft();
    else if (color == 0)
      turningright();
    digitalWrite(A0, LOW);
    flag_cp1=1;
    startTime = millis();
  }
  else if (distance >17 && flag_cp1==1)
  {
    currentTime = millis();
    elapsedTime = currentTime - startTime;
    if(color == 1)
    {
      if (elapsedTime<=33000)
        follow();
      else if (elapsedTime>33000) 
      {
        if(leftflag == 0)
        {
          leftturningleft(); 
          leftforward();
          leftflag=1;
        }
        if (digitalRead(A4) == 0 && digitalRead(A5) == 0)
          leftforward();
        else
          follow();
      }
    }
    else if (color == 0)
    {
      if (elapsedTime<=34000)
        follow();
      else if (elapsedTime>34000) 
      {
        if(rightflag == 0)
        {
          rightturningright(); 
          rightforward();
          rightflag=1;
        }
        if (digitalRead(A4) == 0 && digitalRead(A5) == 0)
          rightforward();
        else
          follow();
      }
    }
  }
  else if (distance <=17 && flag_cp1==1)
  {
    stop();
    digitalWrite(A0, HIGH);
    delay(5000);
    size=digitalRead(A3);//0-small,1-big
    digitalWrite(A0, LOW);
    if (size==0 && flag_cp2!=3)
      flag_cp2=1;
    else if (size==1 && flag_cp2!=3)
      flag_cp2=2;
  }
}