基于ESP32 Arduino控制海豚机械臂制作分享

写在前面：本文是基于小智学长的b站教程零基础Arduino ESP32入门到项目的个人用学习进度记录帖，其中海豚机械臂模型来自b站阿奇，博客中以海豚机械臂项目进行学习，博客不作为商业用途或其他盈利目的，仅作为个人学习分享使用。

最新更新时间：2024/5/30

---

 学习资源如下：

小智学长教程视频及配套文档

AiPG（光打机三合一）项目指南 持续更新ING - 飞书云文档 (feishu.cn)

【小智学长】零基础Arduino ESP32教程入门到项目 手把手教你 光随屏动、Mini打印机、机械数字孪生 嵌入式 单片机 实战项目教程 50节课手把手编程_哔哩哔哩_bilibili

【全开源】机械臂控制、数字孪生、正逆解项目教程，代码 结构 APP Unity工程全开源！！！_哔哩哔哩_bilibili

---

1、 物料准备

1.1 机械臂构成

机械臂外观如下，机械臂上有四个舵机，分别控制旋转、A轴、B轴、和夹爪的运动，这四个舵机控制线经过机械臂本体后，从下方引出，引出的四组线接到ESP32板卡上，然后就可以通过板卡实现多个关节的控制。

1.2 3D打印资料配件

机械臂的支架及部分构件是需要3D打印制作的，如果感兴趣的话可以到b站阿奇那里学习从solidwork上绘制所有零件，不过这边也是有制作好的打印文件的

Docs

一共24件

博主是借用了实验室的3D打印机来进行打印的，如果是自己用3D打印机来进行打印的话务必要准备好锉刀或者虎口钳等强力工具以及砂纸打磨，否则很难拆除支撑的

然后这是博主的打印机使用经验分享，有同款打印机的可以借鉴一下

拓竹A1 3D打印机使用经验分享-CSDN博客

另外一种选择就是到淘宝上找商家打印，不过打印价格也是有点小贵的，价格从80到300多不等，打印完之后零件如下

1.3 机械臂控制板（面包板及杜邦线）

博主使用的是ESP32CP2102开发板进行开发，也可以用其他支持Arduino的板子来控制，如果使用ESP公板要再买面包板，连接舵机也需要不少的杜邦线最好公对公、公对母的线都买一些

重点！！

开发板需要安装好驱动并且配置好环境工程才行，如果没有配置好需要先看我的基础知识文章

Arduino从入门到项目学习进度记录帖-CSDN博客

还有一个就是供电模块，实测用ESP32公板连接电脑供电3.3v是足以控制四个舵机运动的，不过通过搭载电源模块就可以脱离电脑控制了，这个看情况购买吧，因为不知什么原因，舵机供电线必须和板子的地线相连，这个后续会提及

1.4 其他零配件

材料购置可在文档内链接找到淘宝商家，其实不太建议直接买阿奇工具包，里边的零件都是数量刚刚好的，如果有丢失就得重买了

海豚机械臂资料 |文件下载|BOM表|安装教程|工具表 By阿奇设计分享 - 飞书云文档 (feishu.cn)

1.5 工具

那么当我们的物料都准备好后，在开始安装机械臂之前，得先进行舵机角度调整

2、 舵机控制

2.1 舵机介绍

舵机是一种位置伺服的电机，与马达不同，我们需要马达提供的是旋转，控制的是转速和方向。而舵机不需要整圈的旋转，需要的是旋转角度并维持住。一般舵机旋转的角度范围是0 度到180 度。舵机引线为3线，分别用棕、红、橙三种颜色进行区分，舵机品牌和生产厂家不同，会有些许差异，使用之前需查看资料。我们使用的是最常见的舵机，棕、红、橙分别对应“电源负极，电源正极，控制信号”。

2.2 舵机控制原理介绍

通过向舵机的信号线发送电平时序信号来控制舵机的转动位置

MG996R与MG90S:

0.5ms--------------0度； 2.5% （0.5ms高电平+19.5低电平）

1.0ms------------45度； 5.0% （1.0ms高电平+19.0低电平）

1.5ms------------90度； 7.5% （1.5ms高电平+18.5低电平）

2.0ms-----------135度；10% （2.0ms高电平+18.0低电平）

2.5ms-----------180度；12.5%（2.5ms高电平+17.5低电平）

舵机的控制周期通常为20毫秒、50Hz的频率，但是许多舵机在40至200 Hz的范围内都能正常工作。

下面，我们尝试实现一个舵机的角度控制。

2.3 单舵机转动指定角度

首先拿出一个MG996R舵机观察连接线发现有棕红黄三种颜色

这三种从左到右依次是信号线，电源线，地线

那么要连接板卡的话，舵机红色线接3V3，黄色线接控制IO，棕色线接GND

而这个黄色连接的IO口是可以通过代码定义是连接板上的哪一个引脚的，这里我们将以IO23进行单舵机示例

首先先进行连线，将开发板插在面包板上同时用两条公对公的杜邦线将电源组装好（这里以开发板供电为例，独立电源模块后续介绍）注意接正极那一列的线接3V3，负极那一列接GND，面包板排线原理有不懂的可以百度一下

之后拿三条公对公的线连接舵机以及面包板上，注意分辨各条线舵机红色线接3V3，黄色线接D23，棕色线接GND

连接完成之后就是通过代码控制舵机角度转动了

在vscode上左栏点击小蚂蚁标识，在主页中找到libraries搜索esp32servo

点击第一个选择添加项目，将库文件添加到你的项目中

检查一下库文件是否引用

在项目文件中找到platformio.ini检查是否有这段代码，没有就得重新添加库

添加完库之后编写main.cpp文件实现舵机控制

先来编写一个简单的角度控制

#include <Arduino.h>
#include <ESP32Servo.h>

#define servo1Pin 23 //定义引脚

Servo servo1;

void setup() {
    servo1.setPeriodHertz(50); // Standard 50hz servo，设置周期赫兹
    servo1.attach(servo1Pin, 500, 2500); //MG996R的范围是500-2500us
    //将Servo变量附加到引脚,语法servo.attach(pin)servo.attach(pin, min, max)
    //参数说明servo，一个类型为servo的变量pin，连接至舵机的引脚编号
    //min(可选)，舵机为最小角度（0度）时的脉冲宽度，单位为微秒，默认为544
    //max(可选)，舵机为最大角度（180度时）的脉冲宽度，单位为微秒，默认为2400
    servo1.write(180);
    //向舵机写入一个数值，来直接控制舵机的轴。在一个标准的舵机中，这将设定齿轮的角度.
    //将齿轮转到对应的位置。在一个连续旋转的舵机中，这将设置一个舵机的角度
    //0作为全速向一边，180为全速向另一边，在90附近的值为停止。
}
void loop() {
    
}

代码写完之后进行编译烧录和运行

如果舵机转动起来即代表成功连接了，如果没有动静或者转动无规律抖动等就得自行debug了，可能是供电出问题或者是输入端接触不良等

仅仅让舵机动起来还不够，我们让它实现从0°到180°再到0°的循环过程吧

#include <Arduino.h>
#include <ESP32Servo.h>

Servo servo1;
int minUs = 500;
int maxUs = 2500; //MG996R的范围是500-2500us

int servo1Pin = 23;//定义引脚

ESP32PWM pwm;
void setup() {
    Serial.begin(9600);
    servo1.setPeriodHertz(50); // Standard 50hz servo，设置周期赫兹
    servo1.attach(servo1Pin, minUs, maxUs);
    //将Servo变量附加到引脚,语法servo.attach(pin)servo.attach(pin, min, max)
    //参数说明servo，一个类型为servo的变量pin，连接至舵机的引脚编号
    //min(可选)，舵机为最小角度（0度）时的脉冲宽度，单位为微秒，默认为544
    //max(可选)，舵机为最大角度（180度时）的脉冲宽度，单位为微秒，默认为2400
    servo1.write(0);//向舵机写入一个数值，来直接控制舵机的轴。在一个标准的舵机中，这将设定齿轮的角度.
    //将齿轮转到对应的位置。在一个连续旋转的舵机中，这将设置一个舵机的角度
    //0作为全速向一边，180为全速向另一边，在90附近的值为停止。
    delay(3000);

}

void loop() {
    int pos = 0;
    for (pos = 0; pos <= 180; pos += 1) { // sweep from 0 degrees to 180 degrees
        // in steps of 1 degree
        servo1.write(pos);
        delay(20); // waits 20ms for the servo to reach the position,调速作用
    }
    for (pos = 180; pos >= 0; pos -= 1) { // sweep from 180 degrees to 0 degrees
        servo1.write(pos);
        delay(20);
    }
    delay(3000);
}
//完成舵机从0-180°，在到0°的循环编程。

那么有了这些理论基础后，我们来看下机械臂和舵机的关系。

机械臂上有四个舵机，分别控制旋转、A轴、B轴、和夹爪的运动，所以相比于原来控制一个舵机的代码，我们先来实现控制对多个舵机的控制。

2.4 多舵机控制转动角度

这里我们使用控制板控制四个舵机，首先是接线

四个舵机的红色线一起接到3V3，四个舵机的棕色线一起接到GND，四个舵机的信号线分别接4个IO口。

IO23号引脚-后续接机械臂夹爪

IO22号引脚-后续接机械臂C轴

IO21号引脚-后续接机械臂B轴

IO19号引脚-后续接机械臂旋转底A轴

接线方式还是和单舵机差不多，借由面包板我们能够用多个舵机的线连接电源

如果想用单独供电模块可以接公对母的线到正负极上，这样就可以实现脱离电脑供电了

最好还是给每个舵机做个记号，方便调整原点角度

那么连接好之后我们就可以编写代码进行控制了

速度控制

1、初始化舵机控制结构体，把5个舵机控制变量放入结构体内

2、编写定时器函数，每15ms执行一轮，通过当前角度与控制角度检测舵机是否需要运动

3、编写设置舵机控制角度函数，后续可以通过该函数控制运动角度

4、编写舵机回原点控制函数，后续可通过该函数控制所有舵机回原点

#include <Arduino.h>
#include <ESP32Servo.h>
#include <Ticker.h>//可以设置以固定时间间隔反复执行某个任务

#define SERVO_NUM 4

#define RESET_ANGLE 0

#define PIN_SERVOA 19
#define PIN_SERVOB 21
#define PIN_SERVOC 22
#define PIN_SERVOG 23

//初始化舵机控制结构体，把多个舵机控制变量放入结构体内
typedef struct 
{
    Servo servo[SERVO_NUM];
    uint8_t last_angle[SERVO_NUM];//上一角度
    uint8_t set_angle[SERVO_NUM];//设置角度
}t_servo_list;//初始化舵机控制结构体，把5个舵机控制变量放入结构体内

t_servo_list list;

Ticker read_state_timer;//初始化一个定时器
//判断舵机是否运动到所需角度的函数
static uint8_t em_motor_speed_ctl_run(uint8_t id,uint8_t set_angle,uint8_t now_angle){
    if(set_angle > now_angle){
        now_angle++;
        list.servo[id].write(now_angle);
    }else if(set_angle < now_angle){
        now_angle--;
        list.servo[id].write(now_angle);
    }
    return now_angle;
}

//舵机定时器回调函数，用于更新舵机转动角度
static void motor_timer_callbackfun(){
    for(int index = 0;index < SERVO_NUM;index ++){
        list.last_angle[index] = em_motor_speed_ctl_run(index, list.set_angle[index] ,
        list.last_angle[index]);
    }
}

//检查舵机转动角度是否在0-180°之间
static bool check_angle(uint8_t *angle)
{
    for(int index = 0;index < SERVO_NUM ;index ++){
        if (angle[index] < 0 || angle[index] > 180)
            return false;
    }
    return true;
}

// 通过数组设置舵机转动角度
void em_motor_run(uint8_t *angle)
{
    if (check_angle(angle) == false)
        return;
    for(int index = 0;index < SERVO_NUM;index ++){
        list.set_angle[index] = angle[index];
    }
}

// 分别设置每个舵机的转动角度
void em_motor_run_by_angle(uint8_t angle1,uint8_t angle2,uint8_t angle3,uint8_t angle4){
    list.set_angle[0] = angle1;
    list.set_angle[1] = angle2;
    list.set_angle[2] = angle3;
    list.set_angle[3] = angle4;
}

// 复位所有舵机角度到初始值
void em_motor_reset()
{
    for(int index = 0;index < SERVO_NUM;index ++){
        list.set_angle[index] = RESET_ANGLE;
    }
    // Serial.printf("执行复位\n")，清空上次设置角度，以进行下一次角度转动;
}

// 初始化所有舵机，设置初始角度，并初始化定时器
void em_motor_init()
{
    // 例如，如果范围是500us到2000us， 500us等于0的角，1500us等于90度，2500us等于1800 度。
    list.servo[0].attach(PIN_SERVOA, 500, 2500);
    list.servo[1].attach(PIN_SERVOB, 500, 2500);
    list.servo[2].attach(PIN_SERVOC, 500, 2500);
    list.servo[3].attach(PIN_SERVOG, 500, 2500);
    // timer init 15ms转1°
    read_state_timer.attach_ms(15,motor_timer_callbackfun);//每15ms，执行一次角度判断函数
    //初始化位置
    for(int index = 0;index < SERVO_NUM;index ++){
        list.set_angle[index] = RESET_ANGLE;
        //设置当前角度，因为舵机上电时无法知道自身角度，所以这里假设角度为原点+1
        //这就要求我们上电前把机械臂调整到安装的原点角度，也就是每个轴都在0°附近
        //否则会导致机械臂第一次控制运动非常快
        list.last_angle[index] = RESET_ANGLE +1;
    }
}

void setup() {
  em_motor_init();
}

void loop() {
  em_motor_run_by_angle(90,0,180,0);
//   delay(5000);
//   em_motor_run_by_angle(90,90,90,90);
//   delay(5000);
//   em_motor_run_by_angle(180,180,180,180);
//   delay(5000); 
//修改此处代码能控制舵机角度变化，这边是将周期运动注释掉了，用来连接舵机进行原点控制，可以看到，舵机ABCG的角度分别设置为90，0，180，0，这样我们组装完机械臂之后机械臂启动时就会自动调回原点不会抽搐
}

这里需要特别注意的点是，舵机部分在安装前需要做调原点操作，因为这两款舵机是没有角度反馈的，如果我们不设置原点安装，就会导致控制姿态出现不确定性，也因为这是180°舵机，也会导致有的角度到达不了。

所以我们在安装舵机前，应该通过软件设置好舵机装配原点。

原点：

舵机A：90°

舵机B：0°

舵机C：180°

舵机G：0°

那么代码编写完编译烧录运行之后就可以控制舵机运动或者是调原点了，调完原点之后就可以开始组装机械臂啦

3、 组装机械臂

组装机械臂这边文档给的挺详细了的，这边可能就说几点吧

1、自己打印的材料支撑最好清干净一些，因为很多孔都是刚刚好甚至有些会小一点的

2、各配件最好用零件盒装好分清楚，要不然会很麻烦的

3、如果要喷油漆不要在宿舍里喷味道很大，博主这边也是没有去用油漆了

4、螺丝刀各种型号的最好都备齐一些，一字、十字、六角都得要

5、拧关于轴承转动的螺丝尽量拧紧但不要拧死，没拧紧转动的时候容易掉

6、文档内bc连杆文档图片跟实物不太一样，在胶水粘贴那部分按大致位置粘贴即可

7、很多时候会发现螺丝螺母轴承等难以塞入，切记一定不可以差不多就行，像轴承那些一定得完全塞进去才行不然尺寸不对，记住：大力出奇迹

8、机械臂很坚固也很脆弱，组装好后不要大力过猛扭动，否则会出现胶水粘结处脱落，舵机损坏等情况（被乱动导致差点要重新组装的痛）

9、一定要调好舵机原点才组装机械臂！！

其他应该也没啥了，跟着文档一步步来就行，组装过程会碰到很多困难，头脑清醒慢慢来，谨记大力出奇迹，相信不久你就将拥有一台属于你的机械臂了

海豚机械臂资料 |文件下载|BOM表|安装教程|工具表 By阿奇设计分享 - 飞书云文档 (feishu.cn)

4、控制工程代码编写

组装好机械臂之后就可以编写代码来控制机械臂运动啦

那么该怎么调整舵机角度呢，以下是阿奇的运动学计算方法以及运动运算表格

海豚机械臂运动学计算 - 飞书云文档 (feishu.cn)

机械臂运动运算表格 - 飞书云文档 (feishu.cn)

接下来就是代码组成，那么在原有的多舵机速度控制上，我们想用调用函数多文件来控制，以便之后可以加入蓝牙模块等等更多的功能时，我们可以在原有工程上加入两个文件

三个文件代码如下：

em_motor.cpp

#include "em_motor.h"
#include <Ticker.h>
#define SERVO_NUM 4

#define RESET_ANGLE 0

#define PIN_SERVOA 19
#define PIN_SERVOB 21
#define PIN_SERVOC 22
#define PIN_SERVOG 23

typedef struct
{
    Servo servo[SERVO_NUM];
    uint8_t last_angle[SERVO_NUM];
    uint8_t set_angle[SERVO_NUM];
} t_servo_list;

t_servo_list list;

Ticker read_state_timer;

static uint8_t em_motor_speed_ctl_run(uint8_t id, uint8_t set_angle, uint8_t now_angle)
{
    if (set_angle > now_angle)
    {
        now_angle++;
        list.servo[id].write(now_angle);
    }
    else if (set_angle < now_angle)
    {
        now_angle--;
        list.servo[id].write(now_angle);
    }
    return now_angle;
}

static void motor_timer_callbackfun()
{
    for (int index = 0; index < SERVO_NUM; index++)
    {
        list.last_angle[index] = em_motor_speed_ctl_run(index, list.set_angle[index], list.last_angle[index]);
    }
}

static bool check_angle(uint8_t *angle)
{
    for (int index = 0; index < SERVO_NUM; index++)
    {
        if (angle[index] < 0 || angle[index] > 180)
            return false;
    }
    return true;
}

void em_motor_run(uint8_t *angle)
{
    if (check_angle(angle) == false)
        return;
    for (int index = 0; index < SERVO_NUM; index++)
    {
        list.set_angle[index] = angle[index];
    }
}

void em_motor_run_by_angle(uint8_t angle1, uint8_t angle2, uint8_t angle3, uint8_t angle4)
{
    list.set_angle[0] = angle1;
    list.set_angle[1] = angle2;
    list.set_angle[2] = angle3;
    list.set_angle[3] = angle4;
}

void em_motor_init()
{
    // 例如，如果范围是500us到2000us， 500us等于0的角，1500us等于90度，2500us等于1800 度。
    list.servo[0].attach(PIN_SERVOA, 500, 2500);
    list.servo[1].attach(PIN_SERVOB, 500, 2500);
    list.servo[2].attach(PIN_SERVOC, 500, 2500);
    list.servo[3].attach(PIN_SERVOG, 500, 2500);
    // timer init 15ms转1°
    read_state_timer.attach_ms(15, motor_timer_callbackfun);
    // 初始化位置
    for (int index = 0; index < SERVO_NUM; index++)
    {
        list.set_angle[index] = RESET_ANGLE;
        // 设置当前角度，因为舵机上电时无法知道自身角度，所以这里假设角度为原点+1
        // 这就要求我们上电前把机械臂调整到安装的原点角度，也就是每个轴都在0°附近
        // 否则会导致机械臂第一次控制运动非常快
        //  list.last_angle[index] = RESET_ANGLE +1;
        //  if(index == 2){
        //      list.set_angle[index] = RESET_ANGLE+180;
        //      list.last_angle[index] = 179;
        //  }
    }
}

em_motor.h

#ifndef _EM_MOTOR_H_
#define _EM_MOTOR_H_

#include <Arduino.h>
#include <ESP32Servo.h>
#include <Ticker.h>

void em_motor_init();

void em_motor_run(uint8_t *angle);

void em_motor_run_by_angle(uint8_t angle1, uint8_t angle2, uint8_t angle3, uint8_t angle4);

#endif

main.cpp

#include <Arduino.h>
#include "hal/em_motor.h"

void setup()
{
    em_motor_init();
}

void loop()
{

    em_motor_run_by_angle(250,50,140,30);
    delay(5000);
    em_motor_run_by_angle(90,0,180,0);
    delay(5000);
}

那么修改main文件就可以实现让机械臂执行规定运动啦，如果发现机械臂每次运动时会有抽搐动作甚至大幅度摆动，那就要检查以下是否有调零点或是电流是否不稳定了

5、BLE APP功能

那其实在上一部分我们可以通过修改代码来让其实现特定任务，但会发现，每次任务都需要重新修改代码然后烧录编译进去，那可不可以通过蓝牙控制来实现手机APP来控制机械臂的运动呢

源码工程组成如下：

main.cpp

#include "em_config.h"
#include "hal/em_ble.h"
#include "hal/em_motor.h"
#include "hal/em_alg.h"

void setup()
{
  Serial.begin(115200);
  Serial.print("init_task\n");
  init_ble();
  em_motor_init();
}

void loop()
{
  // em_motor_run_by_angle(30,30,150,30);
  // delay(5000);
  // em_motor_run_by_angle(0,0,180,0);
  // delay(5000);
}

em_config.h

#ifndef _EM_CONFIG_H_
#define _EM_CONFIG_H_

#include <Arduino.h>

#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"

#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

#define BLE_NAME "Mini-Arm"

#endif

em_alg.cpp

#include "em_alg.h"
#include "hal/em_motor.h"

//移动方向
int moveX;
int moveY;
int moveZ;

//舵机角度
float angleA;
float angleB;
float angleC;
//夹爪绝对坐标
float  absoluteX;
float  absoluteY;
float  absoluteZ;

#define MIN(a, b) ((a) < (b) ? (a) : (b))

float square(float n) {
  return n * n;
}

float sgn(float num)
{
    if(num > 0) {
        return 1.0;
    } else if(num < 0) {
        return -1.0;
    } else {
        return 0.0;
    }
}

/**
 * @brief 输入绝对坐标、输出舵机角度
 * 
 * @param x 
 * @param y 
 * @param z 
 */
void inverse_operation(float x,float y,float z){
  //=-DEGREES(ATAN(D10/B10))*(72/28)+90
  angleA = -degrees(atan(z/x))*(72.0/28.0)+90.0;
  // Serial.printf("angleA = %f\n",angleA);

  //=180-69-(DEGREES( ACOS((135^2+(-SIGN(B15)*SQRT(B15^2+D15^2)-7-60)^2+(C15-65.5)^2-145^2)/(2*135*SQRT((-SIGN(B15)*SQRT(B15^2+D15^2)-7-60)^2+(C15-65.5)^2)))))-(DEGREES(ATAN((C15-65.5)/(-SIGN(B15)*SQRT(B15^2+D15^2)-7-60))))
  //DEGREES(ATAN((C15-65.5)/(-SIGN(B15)*SQRT(B15^2+D15^2)-7-60))))
  float temp1 = degrees(atan((y-65.5)/(-sgn(x)*sqrt(x*x+z*z)-7.0-60.0)));
  // Serial.printf("temp1 = %f\n",temp1);
  //DEGREES( ACOS((135^2+(-SIGN(B15)*SQRT(B15^2+D15^2)-7-60)^2+(C15-65.5)^2-145^2)/(2*135*SQRT((-SIGN(B15)*SQRT(B15^2+D15^2)-7-60)^2+(C15-65.5)^2))))
  float temp2 = degrees(acos((135.0*135.0+square(-sgn(x)*sqrt(x*x+z*z)-7.0-60.0)+square(y-65.5)-145.0*145.0)/(2.0*135.0*sqrt(square(-sgn(x)*sqrt(x*x+z*z)-7.0-60.0)+square(y-65.5)))));
  // Serial.printf("temp2 = %f\n",temp2);
  angleB = 180.0-69.0-temp2-temp1;
  // Serial.printf("angleB = %f\n",angleB);

  //=180-(83.5+(180-69-(temp2)-(temp1)-(DEGREES(ACOS((145^2+135^2-(-SIGN(B15)*SQRT(B15^2+D15^2)-67)^2-(C15-65.5)^2)/(2*145*135)))))
  float temp3 = degrees(acos((145.0*145.0+135.0*135.0-square(-sgn(x)*sqrt(x*x+z*z)-67.0)-square(y-65.5))/(2.0*145.0*135.0)));
  angleC =180.0-(83.5+(180.0-69.0-temp2-temp1)-temp3);
  // Serial.printf("angleC = %f\n",angleC);

  Serial.printf("angle=[%f,%f,%f]\n",angleA,angleB,angleC);
} 

/**
 * @brief 输入角度，输出绝对坐标
 * 
 * @param angleA 
 * @param angleB 
 * @param angleC 
 */
void alg_positive_operation(float angleA,float angleB,float angleC){
  float temp = -(135.0*cos(radians(111.0-angleB))+145.0*sin(radians((83.5+angleB-(180.0-angleC))-angleB+21.0))+67.0);
  
  //=COS(RADIANS((B8-90)/(72/28)))*-(135*COS(RADIANS(111-C8))+145*SIN(RADIANS((83.5+C8-(180-D8))-C8+21))+67)
  absoluteX = cos(radians((angleA-90.0)/(72.0/28.0)))*temp;
  // Serial.printf("x = %f\n",x);

  //=65.5+135*SIN(RADIANS(111-C8))-(145*COS(RADIANS((83.5+C8-(180-D8))-C8+21)))
  absoluteY = 65.5+135.0*sin(radians(111.0-angleB))-(145.0*cos(radians((83.5+angleB-(180.0-angleC))-angleB+21.0)));
  // Serial.printf("y = %f\n",y);

  //=-SIN(RADIANS((B8-90)/(72/28)))*-(135*COS(RADIANS(111-C8))+145*SIN(RADIANS((83.5+C8-(180-D8))-C8+21))+67)
  absoluteZ = -sin(radians((angleA-90.0)/(72.0/28.0)))*temp;
  // Serial.printf("z = %f\n",z);
  Serial.printf("coordinate=[%f,%f,%f]\n",absoluteX,absoluteY,absoluteZ);
}

/**
 * @brief 判断角度是否在可到达范围内
 * 
 * @param angleA 
 * @param angleB 
 * @param angleC 
 * @return true 
 * @return false 
 */
bool check_angle(int angleA,int angleB,int angleC){
  if(angleA < 0 || angleA > 180){
    Serial.printf("angleA error %d , must in 0<a<180\n",angleA);
    return false;
  }

  if(angleB < 0 || angleB > 85){
    Serial.printf("angleB error %d , must in 0<b<85\n",angleB);
    return false;
  }

  float angleCMin = 140-angleB;
  float angleCMax = MIN((196-angleB),180);
  if(angleC < angleCMin || angleC > angleCMax){
    Serial.printf("angleC error %d , must in %f<c<%f\n",angleC,angleCMin,angleCMax);
    return false;
  }
  return true;
}

/**
 * @brief 设置移动动作
 * 
 */
void alg_set_move_action(uint8_t *data){
  moveX = data[0];
  moveY = data[1];
  moveZ = data[2];
  if(moveX == 255)
    moveX = -1;
  if(moveY == 255)
    moveY = -1;
  if(moveZ == 255)
    moveZ = -1;
}

/**
 * @brief 根据动作运行舵机
 * 
 */
void alg_move_run(){
  float offset = 0.4;
  if(moveX == 0 && moveY == 0 && moveZ == 0){
    return;
  }
  if(moveX > 0)
    absoluteX = absoluteX + offset;
  else if(moveX < 0)
    absoluteX = absoluteX - offset;

  if(moveY > 0)
    absoluteY = absoluteY + offset;
  else if(moveY < 0)
    absoluteY = absoluteY - offset;

  if(moveZ > 0)
    absoluteZ = absoluteZ + offset;
  else if(moveZ < 0)
    absoluteZ = absoluteZ - offset;
  inverse_operation(absoluteX,absoluteY,absoluteZ);
  if(check_angle(angleA,angleB,angleC) == true){
    //TODO run
    em_motor_run_by_angle(angleA,angleB,angleC,0);
  }else{
    //TODO reset
    if(moveX > 0)
    absoluteX = absoluteX - offset;
    else if(moveX < 0)
      absoluteX = absoluteX + offset;

    if(moveY > 0)
      absoluteY = absoluteY - offset;
    else if(moveY < 0)
      absoluteY = absoluteY + offset;

    if(moveZ > 0)
      absoluteZ = absoluteZ - offset;
    else if(moveZ < 0)
      absoluteZ = absoluteZ + offset;
    }
}

em_alg.h

#ifndef _EM_ALG_H_
#define _EM_ALG_H_

#include <math.h>
#include "em_config.h"

void alg_positive_operation(float angleA,float angleB,float angleC);

void alg_set_move_action(uint8_t *data);

void alg_move_run();

#endif

em_ble.cpp

#include "em_ble.h"
#include "em_motor.h"
#include "em_alg.h"

BLECharacteristic *pCharacteristic = NULL;
bool bleConnected = false;

bool get_ble_connect()
{
    return bleConnected;
}

// Server回调函数声明
class bleServerCallbacks : public BLEServerCallbacks
{
    void onConnect(BLEServer *pServer)
    {
        bleConnected = true;
        Serial.println("现在有设备接入~");
    }

    void onDisconnect(BLEServer *pServer)
    {
        bleConnected = false;
        Serial.println("现在有设备断开连接~");
        // 在有设备接入后Advertising广播会被停止，所以要在设备断开连接时重新开启广播
        // 不然的话只有重启ESP32后才能重新搜索到
        pServer->startAdvertising(); // 该行效果同 BLEDevice::startAdvertising();
    }
};

class bleCharacteristicCallbacks : public BLECharacteristicCallbacks
{
    void onRead(BLECharacteristic *pCharacteristic)
    { // 客户端读取事件回调函数
        Serial.println("触发读取事件");
    }

    void onWrite(BLECharacteristic *pCharacteristic)
    { // 客户端写入事件回调函数
        size_t length = pCharacteristic->getLength();
        uint8_t *pdata = pCharacteristic->getData();
        if (length == 8)
        {
            // 0xA5 0xA5 0x01 angle1 angle2 angle3 angle4
            if (pdata[0] == 0xA5 && pdata[1] == 0xA5 && pdata[2] == 0x01)
            {
                // 控制指令
                //  Serial.printf("控制指令\n");
                Serial.write(pdata, 8);
                em_motor_run(pdata + 3);
            }
        }
        if (length == 6)
        {
            if (pdata[0] == 0xA5 && pdata[1] == 0xA5 && pdata[2] == 0x02)
            {
                alg_set_move_action(pdata + 3);
            }
        }
        for (int index = 0; index < length; index++)
        {
            Serial.printf(" %d", pdata[index]);
        }
        Serial.printf("\n");
    }
};

void init_ble()
{
    BLEDevice::init(BLE_NAME);     // 填写自身对外显示的蓝牙设备名称，并初始化蓝牙功能
    BLEDevice::startAdvertising(); // 开启Advertising广播

    BLEServer *pServer = BLEDevice::createServer();  // 创建服务器
    pServer->setCallbacks(new bleServerCallbacks()); // 绑定回调函数

    BLEService *pService = pServer->createService(SERVICE_UUID); // 创建服务
    pCharacteristic = pService->createCharacteristic(            // 创建特征
        CHARACTERISTIC_UUID,
        BLECharacteristic::PROPERTY_READ |
            BLECharacteristic::PROPERTY_NOTIFY |
            BLECharacteristic::PROPERTY_WRITE);
    // 如果客户端连上设备后没有任何写入的情况下第一次读取到的数据应该是这里设置的值
    pCharacteristic->setCallbacks(new bleCharacteristicCallbacks());
    pCharacteristic->addDescriptor(new BLE2902()); // 添加描述
    pService->start();                             // 启动服务
    BLEDevice::startAdvertising();
}

em_ble.h

#ifndef _EM_BLE_H_
#define _EM_BLE_H_

#include <BLEDevice.h>
#include <BLE2902.h>

#include "em_config.h"

/**
 * @brief ³õʼ»¯BLE
 *
 */
void init_ble();

#endif

 em_motor.cpp

#include "em_motor.h"
#include <Ticker.h>
#define SERVO_NUM 4

#define RESET_ANGLE 0

// #define PIN_SERVOA 23
// #define PIN_SERVOB 22
// #define PIN_SERVOC 21
// #define PIN_SERVOG 19

#define PIN_SERVOA 19
#define PIN_SERVOB 21
#define PIN_SERVOC 22
#define PIN_SERVOG 23

typedef struct
{
    Servo servo[SERVO_NUM];
    uint8_t last_angle[SERVO_NUM];
    uint8_t set_angle[SERVO_NUM];
} t_servo_list;

t_servo_list list;

Ticker read_state_timer;

static uint8_t em_motor_speed_ctl_run(uint8_t id, uint8_t set_angle, uint8_t now_angle)
{
    if (set_angle > now_angle)
    {
        now_angle++;
        list.servo[id].write(now_angle);
    }
    else if (set_angle < now_angle)
    {
        now_angle--;
        list.servo[id].write(now_angle);
    }
    return now_angle;
}

static void motor_timer_callbackfun()
{
    for (int index = 0; index < SERVO_NUM; index++)
    {
        list.last_angle[index] = em_motor_speed_ctl_run(index, list.set_angle[index], list.last_angle[index]);
        alg_move_run();
    }
}

static bool check_angle(uint8_t *angle)
{
    for (int index = 0; index < SERVO_NUM; index++)
    {
        if (angle[index] < 0 || angle[index] > 180)
            return false;
    }
    return true;
}

void em_motor_run(uint8_t *angle)
{
    if (check_angle(angle) == false)
        return;
    for (int index = 0; index < SERVO_NUM; index++)
    {
        list.set_angle[index] = angle[index];
    }
    alg_positive_operation(angle[0], angle[1], angle[2]);
}

void em_motor_run_by_angle(uint8_t angle1, uint8_t angle2, uint8_t angle3, uint8_t angle4)
{
    list.set_angle[0] = angle1;
    list.set_angle[1] = angle2;
    list.set_angle[2] = angle3;
    list.set_angle[3] = angle4;
}

void em_motor_init()
{
    // 例如，如果范围是500us到2000us， 500us等于0的角，1500us等于90度，2500us等于1800 度。
    list.servo[0].attach(PIN_SERVOA, 500, 2500);
    list.servo[1].attach(PIN_SERVOB, 500, 2500);
    list.servo[2].attach(PIN_SERVOC, 500, 2500);
    list.servo[3].attach(PIN_SERVOG, 500, 2500);
    // timer init 15ms转1°
    read_state_timer.attach_ms(15, motor_timer_callbackfun);
    // 初始化位置
    for (int index = 0; index < SERVO_NUM; index++)
    {
        list.set_angle[index] = RESET_ANGLE;
        // 设置当前角度，因为舵机上电时无法知道自身角度，所以这里假设角度为原点+1
        // 这就要求我们上电前把机械臂调整到安装的原点角度，也就是每个轴都在0°附近
        // 否则会导致机械臂第一次控制运动非常快
        list.last_angle[index] = RESET_ANGLE + 1;
        if (index == 0)
        {
            list.set_angle[index] = RESET_ANGLE + 90;
            list.last_angle[index] = 89;
        }
        if (index == 2)
        {
            list.set_angle[index] = RESET_ANGLE + 180;
            list.last_angle[index] = 179;
        }
    }
    //初始化原点角度，计算原点绝对坐标 absoluteX、absoluteY、absoluteZ
    alg_positive_operation(90, 0, 180);
}

 em_motor.h

#ifndef _EM_MOTOR_H_
#define _EM_MOTOR_H_

#include <ESP32Servo.h>

#include "em_config.h"
#include "em_alg.h"

void em_motor_init();

void em_motor_run(uint8_t *angle);

void em_motor_run_by_angle(uint8_t angle1,uint8_t angle2,uint8_t angle3,uint8_t angle4);

#endif

编写好机械臂源码工程之后再下载下APP软件安装包之后就可以控制机械臂进行移动啦

Docs

不过这个APP只能在Android系统小于12的时候才能用否则会闪退，可以尝试下退到低版本运行

也可以选择修改这个app的Android源代码使其适配高版本的Android系统，源代码文档中也能找到 

---

那么以上就是基于ESP32 Arduino控制海豚机械臂制作的全部内容啦，希望各位能有所收获！