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

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

1.2 修改大臂的舵机模式

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

其中，大臂的舵机线如下图所示。

第二步：在串口调试助手里，选择端口号、波特率并打开串口。

第三步：依次点击下边描红的命令，就可完成大臂模式的设置，同时可观察到大臂转动。

1.3 修改小臂的舵机模式

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

第二步：在串口调试助手里，选择端口号、波特率并打开串口。

第三步：依次点击下边描红的命令，就可完成小臂模式的设置，同时可观察到小臂转动。

这样就完成了总线舵机模式的修改工作。

2.1分析-串联的运动学

      串联的运动学有两种：一种是正运动学，一种是逆运动学。

      串联的正运动学，简单来说是指确定每个关节舵机转动的角度，从而确定端点位置。这种方法在调试时对于少量自由度的机械臂比较实用，但是当自由度增加时，调试复杂程度也会随之增加。比如下方这个3自由度（不含执行器），我们只需要确定其3个关节上的舵机转动角度α，θ，β，即可确定执行端的位置（暂时不考虑臂长的因素）。

     串联的逆运动学，简单来说是指确定端点的位置，然后通过算法计算出各关节需要转动的角度，自动调整到合适的位置。结合本项目中的3轴机械臂（见下图），我们将采用逆运动学算法进行设计，控制运动。

则通过三角函数可得出以下关系：

公式1：

d为OB1，可通过两点距离公式得出如下关系：

公式2：

将“公式1”与“公式2”结合，可得转台旋转角度α与末端坐标的关系如下：

公式3：

      ③ 计算大臂和小臂夹角与坐标之间的关系。作O点和B点之间的辅助线OB（图中两点距离为e），则可知∠OAB（即图中β）为大臂小臂的夹角，如下所示：

从图示关系中，利用三角函数可得以下关系：

公式4：

上式中注意a，b分别为大臂和小臂的长度，可通过实际的进行测量，为已知量；

其中e可根据空间两点距离公式可得：

如图可知：

其中θ为大臂角度，β关系满足

图中θ为大臂转动角度，图中进行了一个简单的几何关系变化。

最后得到简化模型的运动算法：

转台：

大臂：

小臂：

其中θ为大臂角度，β关系满足

      注意：a为大臂长度，b为小臂长度，可通过利用配套游标卡尺测量得出；x，y，z为末端需要达到的实际坐标值。

      第四步：接下来计算AC段在运动过程中对各关节角度的影响，这里采用一种利用坐标变化的补偿计算。

这里我们先来说下补偿计算的作用。在第二步中我们说到第三步的算法是将AC段压缩为0进行的计算，那么我们在第三步中利用真实的坐标点来计算各关节角度将会有一定的误差。补偿算法就是将实际的坐标转化为与第三步中所得算法一致的坐标，得出变换后的坐标之后，在带入到第三步的算法中进行计算，这样就可以获得准确的各个关节转动的角度。接下来我们详细了解一下具体的计算：

      ① 我们先在xy平面进行投影，如下所示：

图中0A为大臂，AC为大臂末端与小臂旋转中心的间距，CB为小臂；末端点B坐标为（x，y）。

② 为方便计算，作OA延长线AB1，且作B点在该延长线的垂线BB1，相交于点B1，设B1坐标为（x1，y1）；

则从图中可知，转台转动角度为γ（这里默认OX为转台初始位置，因为我们这里通过坐标点求角度，所以γ为未知量）；

③ 如下图所示作辅助线，B1点、B点作x轴平行线，且B1点作与B点平行线的垂线，如下所示：

通过平行线定理和余角定理可得：

公式11：∠B1=γ；

图中c为大臂末端和小臂旋转中心间距，可通过配套游标卡尺测量，为已知量；

从图中可得：

公式12：

依余弦定理可得：

公式13：

在△BOB1中，依余弦定理可得：

公式14：

其中OB依两点距离公式可得：

公式15：

合并“公式14”、“公式15”，可得：

公式16：

合并“公式12”、“公式13”、公式16，可计算出γ角度：

公式17：

⑤ 接下来如下图所示，在△HBB1中，利用三角函数计算出B1（x1，y1）：

3. 机械臂气动搬运

实现思路：利用串联机械臂的逆运动学算法，实现机械臂的气动搬运。实验场景如下图所示，其中两个物块代表所需要搬运的物品。

操作步骤：

① 下载文末资料内的参考程序color_experiment_ws\src\Pneumatic_manipulator_handling\Pneumatic_manipulator_handling.ino：

/*------------------------------------------------------------------------------------
  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
           Distributed under MIT license.See file LICENSE for detail or copy at
           https://opensource.org/licenses/MIT
           by 机器谱 2023-08-10 https://www.robotway.com/
  ------------------------------*/
/*
-----------------------------------------------------------------------------------------------------------------
实验功能：实现气动搬运物体（抓取两个物体，并放置两个物体）。

实验思路：通过三自由度机械臂算法求出机械臂的逆运动算法（通过坐标点计算出机械臂转动角度），接着
                结合气动吸盘，规划机械臂运动路径（即添加坐标点），将物块抓取并搬运到指定目标点。搬运
                结束后，在打开的窗口中可自行输入机械臂要要运行的轨迹点坐标。

实验接线：（主要接线）
          机械臂通信口-------------（机械臂）电控箱
          气动机械臂---------------（执行器）电控箱
          其他线路按照教材图片连接。
Created 2020.7.16     By：Boris.yuan
-----------------------------------------------------------------------------------------------------------------         
*/

#include <Arduino.h>
#include <stdio.h>
#include <string.h>

#define CTL_BAUDRATE 115200 //总线舵机波特率
#define SERIAL_PRINTLN Serial1
#define SerialBaudrate 115200
#define Bus_servo_Angle_inits 1500

String receive_string = "";
enum{CATCH=1,REALIZE};
int catch_red_numbers=0;
int catch_blue_numbers=0;
unsigned long old_time = millis();
unsigned long new_time = millis();
unsigned long old_time_blue = millis();
unsigned long new_time_blue = millis();

int value_move[3]={1500,1500,1500};
int last_value[3]={0,0,0};
int poit_sqrt = 0;
int catch_number = 0;
void setup() {
  // put your setup code here, to run once:
  delay(1555);
  Serial.begin(SerialBaudrate);delay(1000);
  SERIAL_PRINTLN.begin(CTL_BAUDRATE);delay(1000);
  Bus_servo_angle_init();delay(3000);
  Pneumatic_raw_init();delay(1000);
}


void loop() {
  raw_translation();
  delay(8000);
}


void raw_translation()
{
  //catch the first sth
  calculate_postion(60,130,40);delay(3000);
  catch_and_realize_something(0);delay(3000);//catch
  calculate_postion(60,110,80);delay(3000);
  calculate_postion(-120,110,80);delay(3000);
  calculate_postion(-120,130,2);delay(3000);
  catch_and_realize_something(1);delay(3000);//relize
  calculate_postion(-120,130,70);delay(3000);
  calculate_postion(-100,90,70);delay(3000);
 
  //catch the second sth
  calculate_postion(60,100,70);delay(3000);
  calculate_postion(60,130,10);delay(3000);//down
  catch_and_realize_something(0);delay(3000);//catch
  calculate_postion(60,110,80);delay(3000);
  calculate_postion(-120,110,80);delay(3000);
  calculate_postion(-120,130,37);delay(3000);
  catch_and_realize_something(1);delay(3000);//relize
  calculate_postion(-120,130,70);delay(3000);
  calculate_postion(-100,90,70);delay(3000);
  Bus_servo_angle_init();delay(2000);
  delay(1000);  
}

      ② 程序下载完成后，观察气动搬运效果。正常情况下机械臂会先搬运上边的物块，放置在一侧；接着再搬运下边的物块，并把其放置在刚才的第一个物块上。

4. 机械臂手爪搬运

      实现思路：利用串联机械臂的逆运动学算法，实现机械手爪搬运两个物块。实验场景如下图所示，其中两个物块代表所需要搬运的物品。

操作步骤：

① 下载文末资料内的参考程序color_experiment_ws\src\Manipulator_handling\Manipulator_handling.ino：

/*------------------------------------------------------------------------------------
   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
           Distributed under MIT license.See file LICENSE for detail or copy at
           https://opensource.org/licenses/MIT
           by 机器谱 2023-08-10 https://www.robotway.com/
   ------------------------------*/
/*
-----------------------------------------------------------------------------------------------------------------
实验功能：实现气动搬运物体（抓取两个物体，并放置两个物体）。

实验思路：通过三自由度机械臂算法求出机械臂的逆运动算法（通过坐标点计算出机械臂转动角度），接着
                结合气动吸盘，规划机械臂运动路径（即添加坐标点），将物块抓取并搬运到指定目标点。搬运
                结束后，在打开的窗口中可自行输入机械臂要要运行的轨迹点坐标。

实验接线：（主要接线）
          机械臂通信口-------------（机械臂）电控箱
          气动机械臂---------------（执行器）电控箱
          其他线路按照教材图片连接。
Created 2020.7.16     By：Boris.yuan
-----------------------------------------------------------------------------------------------------------------         
*/

#include <Arduino.h>
#include <stdio.h>
#include <string.h>

#define CTL_BAUDRATE 115200 //总线舵机波特率
#define SERIAL_PRINTLN Serial1
#define SerialBaudrate 115200
#define Bus_servo_Angle_inits 1500

String receive_string = "";
enum{CATCH=1,REALIZE};
int catch_red_numbers=0;
int catch_blue_numbers=0;
unsigned long old_time = millis();
unsigned long new_time = millis();
unsigned long old_time_blue = millis();
unsigned long new_time_blue = millis();

int value_move[3]={1500,1500,1500};
int last_value[3]={0,0,0};
int poit_sqrt = 0;
int catch_number = 0;
void setup() {
   // put your setup code here, to run once:
   delay(1555);
   Serial.begin(SerialBaudrate);delay(1000);
   SERIAL_PRINTLN.begin(CTL_BAUDRATE);delay(1000);
   Bus_servo_angle_init();delay(3000);
   Pneumatic_raw_init();delay(1000);
}


void loop() {
   raw_translation();
   delay(8000);
}


void raw_translation()
{
   //catch the first sth
   calculate_postion(60,130,40);delay(3000);
   catch_and_realize_something(0);delay(3000);//catch
   calculate_postion(60,110,80);delay(3000);
   calculate_postion(-120,110,80);delay(3000);
   calculate_postion(-120,130,2);delay(3000);
   catch_and_realize_something(1);delay(3000);//relize
   calculate_postion(-120,130,70);delay(3000);
   calculate_postion(-100,90,70);delay(3000);
   
   //catch the second sth
   calculate_postion(60,100,70);delay(3000);
   calculate_postion(60,130,10);delay(3000);//down
   catch_and_realize_something(0);delay(3000);//catch
   calculate_postion(60,110,80);delay(3000);
   calculate_postion(-120,110,80);delay(3000);
   calculate_postion(-120,130,37);delay(3000);
   catch_and_realize_something(1);delay(3000);//relize
   calculate_postion(-120,130,70);delay(3000);
   calculate_postion(-100,90,70);delay(3000);
   Bus_servo_angle_init();delay(2000);
   delay(1000);   
}

② 程序下载完成后，观察搬运效果。

5. 机械臂位置反馈

      实现思路：让机械臂做3组动作，然后返回其相应的舵机的pwm值。其中机械臂包括转台、大臂、小臂三部分，实验中将分别返回控制其三部分的舵机Pwm值。

操作步骤：

① 下载文末资料内的参考程序color_experiment_ws\src\Get_BusServo_Current_Angle\Get_BusServo_Current_Angle.ino：

/*------------------------------------------------------------------------------------
  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
           Distributed under MIT license.See file LICENSE for detail or copy at
           https://opensource.org/licenses/MIT
           by 机器谱 2023-08-10 https://www.robotway.com/
  ------------------------------*/
/*
-----------------------------------------------------------------------------------------------------------------
实验功能：在串口监视器中查看舵机当前转动的角度。

实验思路：首先编写驱动总线舵机驱动程序，之后，编写接收串口监视器数据的程序，接着将
        串口监视器接收的数据进行简单的处理，最后，将处理后的数据与机械臂各部位结合。
       
实验操作：将该例程下载到arduino开发版，接着按照实验教程连接线路，完成后，打开arduino
        的串口监视器，结合总线舵机使用教程，观察串口监视器中的数据是否与总线舵机教程相符。             
        例如串口监视器输出的：总线舵机000:#000P1114!表示的意思是：
                              地址为000的总线舵机（即转台）当前的pwm值为1114（即55度）。
                              度数angle与pwm的关系式为： angle = map(pwm,500,2500,0,180)
实验接线：（主要接线）
          机械臂通信口-------------（机械臂）电控箱
          其他线路按照教材图片连接。
Created 2020.7.16     By：Boris.yuan
-----------------------------------------------------------------------------------------------------------------         
*/


#include <stdio.h>
#include <string.h>

#define CTL_BAUDRATE 115200 //总线舵机波特率
#define SERIAL_PRINTLN Serial1
#define SerialBaudrate 115200
#define Bus_servo_Angle_inits 1500

String receive_string="";
int value_move[3]={1500,1500,1500};
int last_value[3]={0,0,0};

void setup() {
  // put your setup code here, to run once:
  delay(1100);
  Serial.begin(SerialBaudrate);
  SERIAL_PRINTLN.begin(CTL_BAUDRATE);
  Serial.println("Start!");
  Bus_servo_angle_init();delay(2000);
  int ss = map(1114,500,2500,0,180);
  Serial.println(ss);
}



void loop() {

  for(int i=0;i<3;i++)
  {
    arm_action_test(i);
    for(int j=0;j<3;j++){
      Read_get_busservo_angle_current(j);
      delay(1000);
    }
    delay(5000);
  }
}

② 打开串口，查看返回的结果值（如下图所示）。

程序源代码及总线舵机ID及模式更改资料下载地址详见 桌面级机械臂-运动控制