机器人制作开源方案 | 桌面级机械臂--本体说明+驱动及控制

机器谱

一、本体说明
1. 机械臂整体描述
       该桌面级机械臂为模块化设计，包含主机模块1个、转台模块1个、二级摆动模块1个、可编程示教盒1个、2种末端执行器、高清摄像头，以及适配器、组装工具、备用零件等。可将模块快速组合为一个带被动关节的串联3自由度机械臂，亦可进一步加装、更换执行器、传感器来完成各类控制实验。

1.1 机械臂清单介绍

序号	名称	备注
1	本体	包括转台、小臂、大臂、执行机构
2	执行机构	如：手爪执行器、气动执行器
3	主机	包括树莓派、Arduino 主控板等相关的配置
4	显示屏
5	电子部件	如：摄像头、遥控模块

机械臂的主要配置清单

1.2 产品配置
机械臂的主要硬件图如下所示：

主要硬件图

2. 机械臂结构说明
该机械臂结构是一个带被动关节的3自由度，包含一个1自由度的转台和带被动关节的2自由度关节。

       其中2自由度关节模块分为大臂和小臂：大臂通过1.76减速比的齿轮箱进行传动，小臂通过二级带传动实现传动和被动关节设计。被动关节可以保证执行器末端运动时指向保持一个方向，其中一级带传动控制小臂转动，另一级带传动使末端方向保持一致。

如下所示是装有手爪、摄像头的实物图：

装有手爪和摄像头的实物图

其中的执行机构部分（即现在装手爪执行器的地方）还可以灵活的替换为气动部件（见下图）。

装有气动部件的实物图

3. 机械臂的电路连接
       在进行电路连接前，先来了解一下主机的布局及引脚图。下图是主机的布局实物图，本实验中主要用到总线、PWM（D4）、TX2/RX2。

主机正面图  

主机电路引脚图（从右向左与主机对应）

按下图所示进行机械臂的电路连接：

机械臂与主机外设的接线图

显示屏、鼠标、键盘与主机进行连接

开机注意事项：
       机械臂的状态主要包含非复位状态、复位状态（其中复位状态指的是：大臂转动到垂直状态、小臂转动到水平状态）。只有开机看到机械臂能够从非复位状态转到复位状态，后续实验才能正常进行；否则可能会出现烧录程序，但机械臂不按预期运动的现象。

机械臂非复位状态

机械臂的复位状态

4. 软硬件环境配置及使用
4.1软硬件环境介绍
机器人可利用视觉识别技术，进行识别、追踪目标、定位等功能。我们可以利用上位机来控制机器人的大脑，用下位机控制设备。   
上位机是指：
       人可以直接发出操控命令的计算机，一般是PC或者计算机的屏幕上显示各种信号变化（液压，水位，温度等），本实验将利用树莓派屏幕上显示视觉识别的结果。

树莓派及Ros系统

下位机是指：
       直接控制设备获取设备状况的的计算机，一般是PLC/单片机之类的，本实验将利用Arduino开源主控板充当下位机进行开发。

Arduino主控板及编程IDE

       上位机发出的命令首先传给下位机，下位机再根据此命令解释成相应时序信号，直接控制相应设备。下位机读取设备状态数据（一般模拟量），转化成数字信号反馈给上位机。选择合适的平台及编程环境、函数库，就可以进行机器人结构、运动控制、视觉开发等多方面的应用，控制机器人的行为。

4.2在树莓派上安装Ros及功能包
       为了便于使用，本实验中已把树莓派和Arduino板子进行了组合，设计出主控盒子（又叫主机），方便使用。主控盒子已预装好操作系统、编程环境以及需要用到的功能包。

主控盒子（主机）

       对树莓派上的环境配置主要包括：安装Raspberry Pi OS (32-bit) with desktop操作系统、ROS、配置ROS环境、Arduino IDE、功能包。其中本实验需要的功能包包括视觉识别库（OpenCV2.4.9）、二维码识别库（zbar）。为了方便使用，本实验已做好了树莓派镜像文件，已经预安装好上述开发环境，电路连接好后可直接使用。当把主机与显示屏连接后，找到文末资料下载中的color_experiment_ws文件夹即可。

5. 资料下载
资料内容：
①样机3D文件
②程序源代码
资料下载地址：桌面级机械臂-本体说明 https://www.robotway.com/h-col-259.html
二、驱动及控制
1. 总线舵机模式的设置
       机械臂驱动采用总线式电机驱动，该总线式电机具有多种模式，如360°圆周模式、270°逆时针模式、270°顺时针模式等。总线舵机采用单总线通信方式，与传统舵机相比，最大特点就是舵机之间可串联，最多可级联255个舵机。
       打开文末资料内的“总线舵机ID及模式更改资料”，为总线舵机设置模式，包括转台、大臂、小臂三部分，需要分别为总线舵机设置ID、工作模式。关于总线舵机ID设置如下图所示：

机械臂的各舵机ID图

接下来开始为机械臂的各部分设置相应的舵机模式：

各部分	总线舵机ID	计划设置舵机模式	代表含义
转台	000	#000PMOD2!	舵机模式，角度最大范围270 度，方向逆时针
大臂	001	#001PMOD2!	同上
小臂	002	#002PMOD2!	同上

器材准备：PC机或笔记本电脑（windows操作系统、Arduino IDE）、以及下图所示的器材。

操作过程：分别为机械臂的转台、大臂、小臂设置舵机模式，并能设置其初始角度。

1.1设置转台的舵机模式
第一步：按下图所示连接电路。

第二步：打开文末资料内的”总线舵机ID及模式更改资料\串口调试助手\sscom5.13.1.exe”（如下图所示）。

第三步：选择端口号、波特率，并打开串口。
先在菜单栏：串口设置-打开串口设置，选择端口号、波特率，点击“ok”即可。

接着点击“打开串口”，并点击“扩展”。

这里已经把命令设置好了，只需要依次点击描红的命令（如下图所示），就可以完成转台模式的设置，即可观察到转台转动到指定角度。

1.2 设置大臂的舵机模式
第一步：按下图所示把大臂的舵机线与小模块进行电路连接（注意：请先把小模块断电）。

第二步：在串口调试助手里，选择端口号、波特率并打开串口。
第三步：依次点击下边描红的命令，就可完成大臂模式的设置，同时可观察到大臂转动。

1.3设置小臂的舵机模式
第一步：按下图所示把小臂的舵机线与小模块进行电路连接（注意：请先把小模块断电）。

第二步：在串口调试助手里，选择端口号、波特率并打开串口。
第三步：依次点击下边描红的命令，就可完成小臂模式的设置，同时可观察到小臂转动。

这样就完成了总线舵机模式的设置工作，接下来的实验都是基于这种舵机模式来完成的。

2. 机械臂的单关节控制
实现思路：串口发送命令，控制单关节控制（提示：需要先设置好舵机模式）
器材准备：机械臂本体、主控盒、显示屏、键盘、鼠标（如下图所示）

装有手爪和摄像头的实物图

操作步骤：
① 下载文末资料内的参考程序color_experiment_ws\src\Contril_Single_Joint\arm_driver\arm_driver.ino：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-08-02 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. /*
    
14. 
    
15. -----------------------------------------------------------------------------------------------------------------
    
16. 
    
17. 实验功能：实现单关节控制机械臂控制。
    
18. 
    
19. 实验思路：通过串口发送命令，控制机械臂单关节控制。首先程序实现读取串口发送的数据，接着处理串口发送的数据，
    
20. 
    
21.         最后程序将处理好的数据通过Serial1发送给机械臂达到控制机械臂的效果。
    
22. 
    
23.         注意：串口发送命令格式为［机械臂部件pwm］（pwm范围是［500～2500］）。
    
24. 
    
25.         例如：rm 1500 底盘转到1500
    
26. 
    
27.               um 1500 大臂转到1500
    
28. 
    
29.               lm 1500 小臂转到1500
    
30. 
    
31.                   
    
32. 
    
33. 实验接线：（主要接线）
    
34. 
    
35.           机械臂通信口-------------（机械臂）电控箱
    
36. 
    
37.           机械爪（舵机）---------------（执行器）电控箱
    
38. 
    
39.           其他线路按照教材图片连接。
    
40. 
    
41. Created 2020.7.16     By：Boris.yuan
    
42. 
    
43. -----------------------------------------------------------------------------------------------------------------
    
44. 
    
45. */
    
46. 
    
47. #include <Servo.h>
    
48. 
    
49. #define CTL_BAUDRATE 115200 //总线舵机波特率
    
50. 
    
51. #define SERIAL_PRINTLN Serial1
    
52. 
    
53. #define SerialBaudrate 115200
    
54. 
    
55. #define RM "rm"
    
56. 
    
57. #define UM "um"
    
58. 
    
59. #define LM "lm"
    
60. 
    
61. String receive_string = "";
    
62. 
    
63. int catch_red_numbers=0;
    
64. 
    
65. int catch_blue_numbers=0;
    
66. 
    
67. int current_angle=0;
    
68. 
    
69. void setup() {
    
70. 
    
71.   // put your setup code here, to run once:
    
72. 
    
73.   delay(1100);
    
74. 
    
75.   Serial.begin(SerialBaudrate);
    
76. 
    
77.   SERIAL_PRINTLN.begin(CTL_BAUDRATE);
    
78. 
    
79.   Bus_servo_angle_init();delay(2000);
    
80. 
    
81.   Serial.println("Please input your commond......");
    
82. 
    
83. }
    
84. 
    
85. void loop() {
    
86. 
    
87.   while(Serial.available()>0)
    
88. 
    
89.   {
    
90. 
    
91.     String commond = "";
    
92. 
    
93.     long n_step = 0;
    
94. 
    
95.     int index = 0;
    
96. 
    
97.     commond = Serial.readStringUntil('\n');
    
98. 
    
99.     index = commond.indexOf(' ');
    
100. 
     
101.     n_step = commond.substring(index+1).toInt();
     
102. 
     
103.     commond = commond.substring(0,index);
     
104. 
     
105.     Serial.print(commond);Serial.print('\t');
     
106. 
     
107.     Serial.println(n_step);
     
108. 
     
109.     if(commond==RM){
     
110. 
     
111.         if( (n_step<500) || (n_step>2500) ){
     
112. 
     
113.           Serial.println("Sorry,the angle is out of range!");
     
114. 
     
115.         }
     
116. 
     
117.         else{
     
118. 
     
119.           ArmSingleServoTo(0,n_step);
     
120. 
     
121.         }
     
122. 
     
123.         n_step=0;commond="";
     
124. 
     
125.     }
     
126. 
     
127.     if(commond==UM){
     
128. 
     
129.         if( (n_step<500) || (n_step>2500) ){
     
130. 
     
131.           Serial.println("Sorry,the angle is out of range!");
     
132. 
     
133.         }
     
134. 
     
135.         else{     
     
136. 
     
137.           ArmSingleServoTo(1,n_step);
     
138. 
     
139.         }
     
140. 
     
141.         n_step=0;commond="";
     
142. 
     
143.     }
     
144. 
     
145.     if(commond==LM){
     
146. 
     
147.         if( (n_step<500) || (n_step>2500) ){
     
148. 
     
149.           Serial.println("Sorry,the angle is out of range!");
     
150. 
     
151.         }
     
152. 
     
153.         else{     
     
154. 
     
155.           ArmSingleServoTo(2,n_step);
     
156. 
     
157.         }
     
158. 
     
159.         n_step=0;commond="";
     
160. 
     
161.     }
     
162. 
     
163.     Serial.println();
     
164. 
     
165.     Serial.println("------------------------------------");
     
166. 
     
167.   }
     
168. 
     
169. }

复制代码

② 打开串口，查看返回的结果值。
注意：串口发送命令格式为［部件 pwm］（pwm范围是［500～2500］）。

串口发送命令例子	代表含义
rm 1500	底盘转到1500
um 1500	大臂转到1500
lm 1500	小臂转到1500

首先，设置波特率并等待程序初始化，界面如下图所示：

接下来，可以发送控制转台运动的例子，来观看结果。
输入格式如下图所示，就可以看见机械臂的转台运动：

同时也可看见串口显示数据。

下面是本实验中分别针对转台、大臂、小臂转动位置进行测试的结果（如下图所示）。

3. 操作杆方案
实现思路：通过操作杆控制气动盒的开、关； 当气动盒打开后，实现可以吸物块的功能。
实验器材及接线：如下图所示。

操作步骤：
第一步：将 Basra主控板 、Bigfish扩展板 与  Birdmen手柄扩展板 堆叠在一起。

Birdmen手柄扩展板引脚图如下图所示：

第二步：下载文末资料内的参考程序color_experiment_ws\src\Rocker_Control_Sucker\rocker_contril_sucker\rocker_contril_sucker.ino：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-08-03 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. /*
    
14. 
    
15. 实验功能：摇杆模块控制吸盘开关。
    
16. 
    
17. 实验思路：读取摇杆模块数值，判断该数据，进而根据数据选取区间，编写不同区间，吸盘状态。
    
18. 
    
19. 实验操作：将该例程下载到arduino开发版，按照实验教程接线，完成后，给模块上电，试着
    
20. 
    
21.         滑动摇杆模块，控制吸盘开关状态。
    
22. 
    
23. 实验接线：气动箱信号脚连接到Barsa的D12引脚
    
24. 
    
25. Create 2020.7.17   By:Boris.yuan
    
26. 
    
27. */
    
28. 
    
29. #define Sucker_Pin 12
    
30. 
    
31. #define Rocker_Pin A1
    
32. 
    
33. void setup() {
    
34. 
    
35.   // put your setup code here, to run once:
    
36. 
    
37.   delay(1000);
    
38. 
    
39.   Serial.begin(9600);
    
40. 
    
41.   pinMode(Sucker_Pin,OUTPUT);
    
42. 
    
43.   pinMode(Rocker_Pin,INPUT);
    
44. 
    
45. }
    
46. 
    
47. void loop() {
    
48. 
    
49.   // put your main code here, to run repeatedly:
    
50. 
    
51.   int receive_Rocker_data = analogRead(Rocker_Pin);
    
52. 
    
53.   Serial.println(receive_Rocker_data);
    
54. 
    
55.   if( (receive_Rocker_data<200) )
    
56. 
    
57.   {
    
58. 
    
59.     digitalWrite(Sucker_Pin,LOW);
    
60. 
    
61.   }
    
62. 
    
63.   else if( (receive_Rocker_data>800) )
    
64. 
    
65.   {
    
66. 
    
67.     digitalWrite(Sucker_Pin,HIGH);
    
68. 
    
69.   }
    
70. 
    
71.   delay(30);
    
72. 
    
73. }

复制代码

第三步：左侧的摇杆，当向右按下时，气动盒开关打开，可以吸取物品（如下图所示）；当向左按下时，气动盒开关关闭。

4. 机械臂的无线遥控
本实验将通过 蓝牙 结合一个可编程示教器进行无线遥控的学习，包含蓝牙通信设定、无线控制。

4.1 两个蓝牙通信设定
实验目的：测试蓝牙主从模块数据发收。
连接在Birdmen手柄扩展板上的蓝牙模块为主模块；连接在带开关通信扩展坞上的蓝牙模块为从模块。

实现思路：当蓝牙主模块发送数据（如123）时，蓝牙从模块能收到该数据（123），代表通信成功。
实验器材：如下图所示。

蓝牙通信模块及配套线

操作步骤：
       第一步：先给Basra主控板下载蓝牙主模块程序。
       ① 先按下图所示连接好电路（注意：蓝牙通信扩展坞的开关需拨到左侧）。

② 然后下载文末资料内的参考程序color_experiment_ws\src\Bluetooth_communication\Master\Master.ino：

1. /*------------------------------------------------------------------------------------
   
2.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
3.            Distributed under MIT license.See file LICENSE for detail or copy at
   
4.            https://opensource.org/licenses/MIT
   
5.            by 机器谱 2023-08-03 https://www.robotway.com/
   
6.   ------------------------------*/
   
7. #define Baud 9600
   
8. #define SERIAL Serial2
   
9. 
   
10. void setup() {
    
11.   // put your setup code here, to run once:
    
12. delay(1000);
    
13. Serial.begin(Baud);
    
14. SERIAL.begin(Baud);
    
15. }
    
16. 
    
17. void loop() {
    
18.   // put your main code here, to run repeatedly:
    
19.   while(SERIAL.available()>0)
    
20.   {
    
21.     String receive_string = SERIAL.readStringUntil('\n');
    
22.     Serial.println(receive_string);
    
23.     receive_string = " ";
    
24.   }
    
25. }

复制代码

③ 最后拔掉此主控板的USB线，将蓝牙主模块插在Bigfish扩展板的扩展坞上（如下图所示）。打开电源后，若看到蓝牙上的灯一闪一闪的，代表正在等待连接中。

第二步：给主机里的Arduino mega2560控制板下载蓝牙从模块程序。
① 把蓝牙通信模块与主机的Tx2/RX2接口进行连接（如下图所示）。

② 然后下载文末资料内的参考程序color_experiment_ws\src\Bluetooth_communication\Salve\Salve.ino：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-08-03 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. #define Baud 9600
    
14. 
    
15. #define SERIAL Serial2
    
16. 
    
17. void setup() {
    
18. 
    
19.   // put your setup code here, to run once:
    
20. 
    
21. delay(1000);
    
22. 
    
23. Serial.begin(Baud);
    
24. 
    
25. SERIAL.begin(Baud);
    
26. 
    
27. }
    
28. 
    
29. void loop() {
    
30. 
    
31.   // put your main code here, to run repeatedly:
    
32. 
    
33.   while(SERIAL.available()>0)
    
34. 
    
35.   {
    
36. 
    
37.     String receive_string = SERIAL.readStringUntil('\n');
    
38. 
    
39.     Serial.println(receive_string);
    
40. 
    
41.     receive_string = " ";
    
42. 
    
43.   }
    
44. 
    
45. }

复制代码

③ 把蓝牙从模块连接在底座上（如下图所示），然后接在主机的TX2/RX2上，可以看见蓝牙从模块的灯亮，且一闪一闪的。

第三步：观察蓝牙主从模块灯，如果看到蓝牙主从模块上的灯亮着但不闪烁，则表示蓝牙主从模块连接成功。

第四步：打开蓝牙（从）模块的Arduino串口监视器，观察是否接受到数据“123”，如果接收到相应的数据（如下图所示），则表示蓝牙主从模块通信成功。

4.2 蓝牙遥控
实验目的：通过控制手柄上的两个摇杆，来实现机械臂气动搬运物块的功能。
实现思路：当按下手柄摇杆的不同方向时，可以控制机械臂的转台、大臂、小臂朝不同方向运动（默认规定手柄左边为左侧摇杆，右边为右侧摇杆）。

当左侧摇杆按下后，控制的吸盘、小臂的情况如下图所示：

当右侧摇杆按下后，控制的转台、大臂的情况如下图所示：

实验器材：BLE4.0蓝牙模块 ×2、连接线、Basra控制板×1、与蓝牙连接的底座、机械臂本体、气动盒、连接线、主机、显示屏、鼠标、键盘
操作步骤：
第一步：先给带有摇杆模块的主控板（接蓝牙主模块）下载程序。
① 按下图所示连接电路：

② 然后下载文末资料内的参考程序color_experiment_ws\src\Wireness_Contril\Program_With_Rocker\controller\controller.ino：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-08-03 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. void setup() {
    
14. 
    
15.   // put your setup code here, to run once:
    
16. 
    
17.   Serial.begin(9600); //蓝牙串口波特率
    
18. 
    
19.   pinMode(A0, INPUT); //左侧上下摇杆引脚
    
20. 
    
21.   pinMode(A1, INPUT); //左侧上下摇杆引脚
    
22. 
    
23.   pinMode(A2, INPUT); //右侧上下摇杆引脚
    
24. 
    
25.   pinMode(A3, INPUT); //右侧上下摇杆引脚
    
26. 
    
27. }
    
28. 
    
29. void loop() {
    
30. 
    
31.   // put your main code here, to run repeatedly:
    
32. 
    
33.   if(analogRead(A0) > 1000)
    
34. 
    
35.   {
    
36. 
    
37.     Serial.print('u');
    
38. 
    
39.   }
    
40. 
    
41.   if(analogRead(A0) < 25)
    
42. 
    
43.   {
    
44. 
    
45.     Serial.print('d');
    
46. 
    
47.   }
    
48. 
    
49.   if(analogRead(A1) > 1000)
    
50. 
    
51.   {
    
52. 
    
53.     Serial.print('c');
    
54. 
    
55.   }
    
56. 
    
57.   if(analogRead(A1) < 25)
    
58. 
    
59.   {
    
60. 
    
61.     Serial.print('s');
    
62. 
    
63.   }
    
64. 
    
65.   if(analogRead(A2) > 1000)
    
66. 
    
67.   {
    
68. 
    
69.     Serial.print('f');
    
70. 
    
71.   }
    
72. 
    
73.   if(analogRead(A2) < 25)
    
74. 
    
75.   {
    
76. 
    
77.     Serial.print('b');
    
78. 
    
79.   }
    
80. 
    
81.   if(analogRead(A3) > 1000)
    
82. 
    
83.   {
    
84. 
    
85.     Serial.print('r');
    
86. 
    
87.   }
    
88. 
    
89.   if(analogRead(A3) < 25)
    
90. 
    
91.   {
    
92. 
    
93.     Serial.print('l');
    
94. 
    
95.   }
    
96. 
    
97.   delay(10);//发送执行周期20ms，可更改，越小机械臂运动越快，但是可能导致机械臂卡顿
    
98. 
    
99. }

复制代码

③ 将蓝牙主模块按下图所示插在手柄的扩展坞上。

第二步：给主机里的Arduino mega2560控制板（与蓝牙从模块连接）下载程序。
① 先把蓝牙从模块连接在扩展底座上；并把另一端与主机的TX2/RX2进行连接。

再把气动盒的三芯线与主机的PWM进行连接（如下图所示）。

② 然后下载文末资料内的参考程序color_experiment_ws\src\Wireness_Contril\Program_With_Chassis\Black_Handle_Robot_Arm_New\Black_Handle_Robot_Arm_New.ino：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-08-03 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. //#include <SoftwareSerial.h>
    
14. 
    
15. #include <stdio.h>
    
16. 
    
17. #include <string.h>
    
18. 
    
19. //#define mySerial Serial2 //总线舵机串口,TX2,GND,5V
    
20. 
    
21. //#define mySerial2 Serial1 //蓝牙遥控串口TX1,RX1,GND,3.3V
    
22. 
    
23. #define CTL_BAUDRATE 115200 //总线舵机波特率
    
24. 
    
25. #define NOZZLE_PIN 4 //气动引脚，注意气动供电6v
    
26. 
    
27. #define INIT_X 1 //假设大臂长度单位为1
    
28. 
    
29. #define INIT_Y 1 //小臂长度与大臂长一致单位为1
    
30. 
    
31. #define INIT_R 1500 //底座总线舵机中间位置波特率为1500
    
32. 
    
33. #define DX 0.01 //每次X方向长的移动距离单位
    
34. 
    
35. #define DY 0.01 //每次Y方向臂长的移动距离单位
    
36. 
    
37. #define DR 5 //每次底座转动的单位
    
38. 
    
39. //SoftwareSerial mySerial(51, 4);
    
40. 
    
41. //SoftwareSerial mySerial2(10, 11);
    
42. 
    
43. #define mySerial2 Serial2
    
44. 
    
45. #define mySerial Serial1
    
46. 
    
47. float cur_x = INIT_X, cur_y = INIT_Y;
    
48. 
    
49. int cur_r = INIT_R;
    
50. 
    
51. int alphaToUpwm(float alpha)
    
52. 
    
53. {
    
54. 
    
55.   return 2732.0-(alpha/M_PI)*2464;
    
56. 
    
57.   /*大臂转动角度的计算公式。2464是大臂摆动180°总线舵机计算后的pwm长度。
    
58. 
    
59.   大臂的减速比是44/25=1.76，测试的motor转动180°需要的pwm长度是1400，1.76×1400=2464；
    
60. 
    
61.   2732是转动90°之后竖直向上对应的实际pwm值，2732=（2464/2）+1500,其中1500是大臂初始化竖直状态是电机的PWM值*/
    
62. 
    
63. }
    
64. 
    
65. int betaToLpwm(float beta)
    
66. 
    
67. {
    
68. 
    
69.   return 1500 + (beta/M_PI)*1400; //小臂无减速比，由总线舵机直接驱动，小臂转动角度的计算公式，1400是小臂摆动180°总线舵机需要的PWM长度，1500是小臂保持水平状态的舵机PWM值
    
70. 
    
71. }
    
72. 
    
73. int armMoveTo(float x, float y)
    
74. 
    
75. {
    
76. 
    
77.   float alpha, beta;
    
78. 
    
79.   if (!cartesianToAlphaBeta(x, y, alpha, beta))
    
80. 
    
81.   {
    
82. 
    
83.     armServoTo(alphaToUpwm(alpha),betaToLpwm(beta));
    
84. 
    
85.     return 0;
    
86. 
    
87.   }
    
88. 
    
89.   return 1;
    
90. 
    
91. }
    
92. 
    
93. void setup() {
    
94. 
    
95.   // put your setup code here, to run once:
    
96. 
    
97.   mySerial.begin(CTL_BAUDRATE);
    
98. 
    
99.   pinMode(NOZZLE_PIN, OUTPUT); //气动引脚定义
    
100. 
     
101.   mySerial2.begin(9600); //蓝牙串口波特率设置
     
102. 
     
103.   nozzle_off(); //初始化气动关闭
     
104. 
     
105.   delay(1000);
     
106. 
     
107.   Serial.begin(9600);
     
108. 
     
109.   armMoveTo(cur_x, cur_y); //初始化机械臂大臂小臂位置
     
110. 
     
111.   cur_r = rotServoTo(cur_r); //初始化机械臂底座位置
     
112. 
     
113. }
     
114. 
     
115. void loop() {
     
116. 
     
117. //   Serial.print(Serial1.parseInt());
     
118. 
     
119.   // put your main code here, to run repeatedly:
     
120. 
     
121. if (mySerial2.available())
     
122. 
     
123. {
     
124. 
     
125.     char c = mySerial2.read();
     
126. 
     
127.     float tx, ty;
     
128. 
     
129.     switch(c)
     
130. 
     
131.     {
     
132. 
     
133.       case 'u': // up
     
134. 
     
135.         ty = cur_y + DY;
     
136. 
     
137.         if (armMoveTo(cur_x, ty) == 0) // success
     
138. 
     
139.         {
     
140. 
     
141.           cur_y = ty;
     
142. 
     
143.         }
     
144. 
     
145.         break;
     
146. 
     
147.       case 'd': // down
     
148. 
     
149.         ty = cur_y - DY;
     
150. 
     
151.         if (armMoveTo(cur_x, ty) == 0) // success
     
152. 
     
153.         {
     
154. 
     
155.           cur_y = ty;
     
156. 
     
157.         }
     
158. 
     
159.         break;
     
160. 
     
161.       case 'f': // forward
     
162. 
     
163.         tx = cur_x + DX;
     
164. 
     
165.         if (armMoveTo(tx, cur_y) == 0) // success
     
166. 
     
167.         {
     
168. 
     
169.           cur_x = tx;
     
170. 
     
171.         }
     
172. 
     
173.         break;
     
174. 
     
175.       case 'b': // backward
     
176. 
     
177.         tx = cur_x - DX;
     
178. 
     
179.         if (armMoveTo(tx, cur_y) == 0) // success
     
180. 
     
181.         {
     
182. 
     
183.           cur_x = tx;
     
184. 
     
185.         }
     
186. 
     
187.         break;
     
188. 
     
189.       case 'l': // left
     
190. 
     
191.         cur_r = rotServoTo(cur_r - DR);
     
192. 
     
193.         break;
     
194. 
     
195.       case 'r': // right
     
196. 
     
197.         cur_r = rotServoTo(cur_r + DR);
     
198. 
     
199.         break;
     
200. 
     
201.       case 'c': // catch
     
202. 
     
203.         //Serial.println('c');
     
204. 
     
205.         nozzle_on();
     
206. 
     
207.         break;
     
208. 
     
209.       case 's': // release
     
210. 
     
211.         //Serial.println('s');
     
212. 
     
213.         nozzle_off();
     
214. 
     
215.         break;
     
216. 
     
217.     }
     
218. 
     
219.   }
     
220. 
     
221. }

复制代码

第三步：通过控制手柄上的两个摇杆，来实现机械臂气动搬运物块的功能。

5. 资料下载
资料内容：
①程序源代码
②总线舵机ID及模式更改资料
资料下载地址：桌面级机械臂-驱动及控制 https://www.robotway.com/h-col-259.html