机械臂的最本质就是控制电机

阶段目标

简单运行电机：电机连接电脑，然后随便转转

solidworks建模部分：用3d建模软件画出3d模型

3d打印部分：找淘宝商家打印出3d模型，验证下是否可以安装

前端项目篇部分：使用threejs + vue3做个类似数字孪生-网页操作控制电机旋转

尝试运行单片机：运行乐鑫官方的helloworld项目

蓝牙部分：单片机运行乐鑫官方的蓝牙项目，并使用手机连接通信

TWAI部分：单片机运行乐鑫官方的TWAI项目，并与小米电机通信

改写单片机项目：根据需求需要将蓝牙项目和TWAI项目组合一起，以实现网页控制esp32单片机向小米电机发送信息

完善前端项目：使用网页控制电机精准旋转

组装，最终成品：组装电路，并于上位机（网页）联调

所有物料的清单：

成品：

简单运行电机

物料: 小米电机 * 1 usb-can转换器 * 1 24V10A电源 * 1 艾迈斯线 * 1 DC5.5转XT30转接线 * 1
连线部分：

can-usb参数设置：

打开小米电机的上位机：

电机成功连接，点击jog运动的加、减号，电机就可以动起来了：

solidworks建模部分

下臂：
中臂：
上臂：
组合起来如下图：
还有个底座，底板没做（暂时这么称呼），大概是用来控制重心的，使机械臂能稳定立在桌面上，以下是稚慧君的机械臂模型，参考看看：

3d打印部分

导出stl文件格式给淘宝的3d打印商家

模型设计的问题很多，比如螺丝孔太细，管壁太薄变形，蛮多缺陷，先勉强用用。如下图所示：

前端项目部分

目标：做个网页用来控制机械臂
技术： vue3 + threejs
步骤：

solidworks导出3d模型：在此使用glb格式(不使用.gltf格式的原因是：gltf格式会生成.bin文件，在本地预览失败，在项目中调用失败，可能是用的方式不对)。

搭建前端工程并搭建threejs场景

调整模型并添加控制轴：因为机械臂的每个关节只能固定一个方向旋转(参考文章： )，所以用transformControls来控制，并将其他其他轴隐藏。如下图所示：

模型调整：重新使用solidworks装配并导出(因为臂需要和电机是一个整体)，并且调整模型的原点，以便更容易控制。
控制优化：想要的效果是骨骼系统那样，比如： three.js示例，但solidworks中似乎没有骨骼系统，所以就先用THREE.Object3D做个简单控制，transformControls好像无法控制Object3D，所以舍弃，效果如下图所示。

运行单片机

目标：运行esp32官方的hello world项目
物料选择:

esp32-s3 N8R16单片机，主要原因是支持蓝牙，wifi和can通信。
一个typeC线
如下图：

esp-idf环境搭建

根据官方文档： esp快速入门我选择的是在线安装，按默认配置安装，
成功之后桌面会有快捷键图标：
双击运行会自动初始化环境，如下图，接下来编译，烧录都会用到，记得使用管理员身份运行

尝试helloworld

从esp-idf项目中复制出helloworld项目，粘贴至你的项目文件夹下（猜测：以后的项目应该也是去改官方的项目示例）：
接下来就是按照官方文档步骤来走：
1. 设置编译的目标平台: 成功则会生成build文件夹：
2. 编译代码: 编译成功之后在build文件夹下会生成.bin文件。
3. 烧录：将程序写入单片中，端口可不写，会自动寻找（注意: typeC插入esp32的正确的口，我用的是COM口）:
4. 执行命令idf.py monitor可看到程序的输出:
到此算简单入门了，接下来就是探索蓝牙和TWAI协议了。

蓝牙部分

目标：运行官网的蓝牙Master项目，并尝试用LightBlue（功能类比postman，用来调试蓝牙）连接蓝牙并传输数据
物料：
esp32-s3单片机 * 1
usb-typec线 * 1
蓝牙的概念：参考文档

运行官方项目:

从esp-idf项目中复制出gatt-server项目，粘贴至你的项目文件夹下
烧录进单片机并运行
手机下载lightblue软件
打开lightblue并搜索单片机的蓝牙信号：
连接成功之后，尝试调用蓝牙的write方法给单片机传递消息:
单片机成功接收到来自的lightblue传输的消息

TWAI部分

目标：运行官方的TWAI-network项目，并尝试使用can调试工具互通数据，然后尝试发送指令给小米点机
物料：
esp32-s3单片机 * 1
usb-typec线 * 1
TJA1050 can模块 * 1
杜邦线 * 6根
usb-can转换器 * 1
小米电机 * 1

Q&A：
问为什么要用这个TWAI？
答：因为小米电机是can协议通信，而esp32-s3支持TWAI协议，TWAI兼容can2.0协议（TWAI官方文档）。
问：需要其他什么硬件支持吗？
答：esp32-s3单片机只有TWAI模块，但没有can消息的收发器，所以单片机上要额外接入TJA1050 can收发器。

CAN协议

CAN协议的参考文章，简单看下协议结构，如下图所示（小米电机采用扩展数据帧）:

硬件连线

正确的连线情况是：运行官方项目，电脑的can调试器能收到相关消息

TJA1050与usb转can转换器连线如下图，usb-can转换器需要短接内置的120欧姆电阻：

检查单片机与TJA1050收发器是否正常连接

运行官方示例项目： twai_self_test
正常运行的console:

测试收发消息

正确连接TJA1050与usb-can转换器的线
运行官方示例项目: twai_network_master
修改参数: usb-can转换器的波特率要与单片机的一致(在此我设置为1Mbits), 还有就是设置为包模式。can调试工具是usb-can转换器商家开发的，不同商家软件界面可能不同，但功能参数应该是相似的
修改代码：因为twai_network_master项目需要与twai_network_slave项目配合，意味着需要两块单片机（但我只有一块，想法是esp32跟usb-can转换器通信，后来证实可行）。展示了ping信号，收发消息和命令任务控制等功能。而我只需要最简单的收发消息，故简化代码逻辑，并与usb-can转换器通信

发送指令控制小米电机

观察下小米上位机通信时的指令，来分析其中含义：
根据can协议看了很久没分析出什么（好像不同的can master有不同的格式），又看了遍官方文档，有单独的文件进行说明：
数据帧得到简单的四条测试指令
1. 使能指令：AA 01 00 08 03 00 FD 15 00 00 00 00 00 00 00 00 7A 帧头： AA
  是否扩展帧: 01中的1
  data_length_code: 08中的8
  帧id： 03 00 FD 15 即通信类型3 发送者的的canid为0xFD 接收者的canid为0x15
  数据： 00 00 00 00 00 00 00 00
  帧尾： 7A
2. jog +5rad/s指令：AA 01 00 08 12 00 FD 15 05 70 00 00 07 01 6A AA 7A
3. jog停止指令： AA 01 00 08 12 00 FD 15 05 70 00 00 07 00 7F FF 7A
4. 停止指令： AA 01 00 08 04 00 FD 15 00 00 00 00 00 00 00 00 7A
连线小米电机（电机的波特率也是1Mbits），运行项目

改写单片机项目

目标：将蓝牙和TWAI项目集合在一起，单片机的最终功能是：① 接收蓝牙信息，将其转成can消息帧，传给小米电机 ② 将接收到的can消息帧通过蓝牙进行回传。如下图：

关键代码

将接收到蓝牙消息用TWAI发送：


    // 将接收到的蓝牙数据做处理，提取出想要格式的帧id和帧数据
    unsigned int frameId = 0;
    int frameData[8];
    for(int i = 0; i < param->write.len; i++) {
      printf("遍历蓝牙%d值: %d", i, param->write.value[i]);
      if(i < 4) { 
        frameId = frameId * 16 * 16 + param->write.value[i];
      }else { 
        frameData[i - 4] = param->write.value[i];
      }
    }
    printf("frameId%u值: %d, --- %d", frameId, frameData[0], frameData[7]);

    // printf("in app_main the min free stack size is %ld \r\n", (int32_t)uxTaskGetStackHighWaterMark(NULL));
    ESP_LOGI(EXAMPLE_TAG, "-----发送can数据指令------");
    // unsigned int testId = 0x0300FD15;
    // int testArr[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    emitMsg(frameId, frameData);
    ESP_LOGI(EXAMPLE_TAG, "-----发送can数据指令结束-----");

将收到的can信号用蓝牙转发给网页：


static void twai_receive_task(void *arg)
{
    while (1) {
      rx_task_action_t action;
      xQueueReceive(rx_task_queue, &action, portMAX_DELAY);

      while(1) {
          twai_message_t rx_msg;
          twai_receive(&rx_msg, portMAX_DELAY);
          ESP_LOGI(EXAMPLE_TAG, "接收到can信号: %lu", rx_msg.identifier);

          // 发送通知
          uint8_t notify_data[8];
          for (int i = 0; i < sizeof(rx_msg.data); ++i) {
              notify_data[i] = rx_msg.data[i];
          }
          
          // 向 GATT 客户端发送指示或通知。将参数 need_confirm 设置为 false 将发送通知，否则为指示。注意：指示或通知数据的大小需要小于 MTU 大小，请参阅“esp_ble_gattc_send_mtu_req”。
          esp_ble_gatts_send_indicate(current_gatts_if, current_param->write.conn_id, gl_profile_tab[PROFILE_A_APP_ID].char_handle,
                                  sizeof(notify_data), notify_data, false);
        }
    }
    vTaskDelete(NULL);
}

完善前端项目

目标：与单片机建立蓝牙通信，新增控制面板模块，用网页控制小米电机的运行。

关键代码：

蓝牙部分：


 // 判断蓝牙是否可用
 let available = await navigator.bluetooth.getAvailability();
 ...
 // 必须用户点击'连接蓝牙'按钮触发，选择对应的蓝牙进行配对连接
 let device = await navigator.bluetooth.requestDevice({
    filters: [{ namePrefix: "ESP" }],
    optionalServices: [0x00ff],
  });
 let server = await device.gatt.connect();
 let service = await server.getPrimaryService(0x00ff);
 let characteristic = await service.getCharacteristic(0xff01);
 ...
 // 调用蓝牙服务的writeValue功能，给单片机传输消息
 characteristic.writeValue(cmdFrame);
 ...
 // 接收蓝牙的通知，并将其数据转换为数组
characteristic.addEventListener("characteristicvaluechanged", (e) => {
    console.log("蓝牙Notification通知: ", e, "值:");
    let CAN_frame_data = Array.from(new Uint8Array(e.target.value.buffer)).map(
      (num) => {
        return num.toString(16);
      }
    );

位置模式的指令生成

位置模式是最符合网页中控制面板的操作

如何运行这个位置模式

可以看到，设置位置模式主要是需要分发两条指令： ①. 发送设置速度指令，即设置limit_spd ②. 发送设置具体位置，即loc_ref

指令的内容

速度指令的速度（0-30rad）需要4个字节来表示
角度指令的弧度（rad）需要4个字节来表示

但是我们的控制面板用的是角度 C = 2Πr, 弧度rad = 角度 * Π / 180 就行了

到此可以计算出角度对应弧度的值，


CODE3：设置参数limit_spd         速度5rad/s
AA 01 00 08 12 00 01 05 17 70 00 00 00 00 A0 40 7A
CODE4：设置参数loc_ref        位置12.5rad
AA 01 00 08 12 00 01 05 16 70 00 00 00 00 48 41 7A

那如何转换成can数据帧需要的格式，比如上边例子：

5 -> 00 00 A0 40 和 12.5 -> 00 00 48 41

去查查这个浮点数的原理简析
看到参考文章里的IEEE754是不是很熟悉，js的Number是遵循这个标准，那可以直接用吗？
查了MDN：number属于64位，意味着是8个字节，与我们想要的有些不同，没发现可以如何调用的方法，遂放弃


/**
 * @description 获取数字（64位, Number类型即双精度double）的32位（单精度float）原始码。 
 *              举个例子： 12.5的float原始码： 符号位： 0  E指数：1000 0010 尾数：100 1000 0000 0000 0000 0000
 *              
 *              原理参考（https://blog.csdn.net/whyel/article/details/81067989）
 *              目前项目中使用的数值范围在： -3.14 ~ 3.14
 * @param { Number } num 
 * @return { 32位Array<0, 1> } 例子  ['0','1','0','0','0','0','0','1','0','1','0','0','1','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0']
 */
export function double2floatCode(num) {
  if(num == 0) {
    return fillZero([], 32);
  }
  let num0b = (num).toString(2).replace('-', '');
  console.log('num: ', num, '二进制: ', num0b);
  let integer0b = num0b.split('.')[0];
  let fraction = num0b.split('.').length === 2 ? num0b.split('.')[1] : [];
  // debugger;

  // 符号
  let signal = num >= 0 ? '0' : '1';
  // -----  指数部分  -----
  let e = 127;
  
  let first1Index = num0b.indexOf('1');  // 二进制1第一次出现的位置
  let pointIndex = num0b.indexOf('.') || 0;  // 小数点位置
  // 表示整数部分大于等于1,   例子1： 10.011 => first1Index：0 , pointIndex: 2  e为1   例子2： 0.011 first1Index：3 , pointIndex: 1  e为-2
  if(first1Index < pointIndex) {
    // 由于二进制都是1开头，所以大于的1的数字e为 pointIndex - 1
    e += pointIndex - 1;
  }else {
    e += pointIndex - first1Index;
  }
  let e0bArr = e.toString(2).split('');
  // 128的二进制是1111 1111，两种情况： 小于128的往前补0， 大于128的不用补 122： 0111 1010      133： 10000101
  if(e < 128) {
    e0bArr = fillZero(e0bArr, 8, 'head');
  }
  // -----  指数部分结束  -----
  
  // -----尾数部分-----
  // float的尾数为23位 
  /*
    mantissaBasis尾数取值的开始坐标，first1Index由于小数点的前后位置，可能会差1，所以在first1Index < pointIndex（num > 1）时候，加一位
    例子1： num < 1
        0.55的二进制: 0.1000 1100 1100 1100 1100 1100  
        第一次出现1的位置 first1Index = 2 ; 小数点位置 pointIndex = 1;
        [...integer0b, ...fraction] = [0, 1,0,0,0, 1,1,0,0, 1,1,0,0, 1,1,0,0, 1,1,0,0, 1,1,0,0 ]
        mantissaBasis = 2 即：first1Index
        尾数部分是 000 1100 1100 1100 1100 1100
    例子2： num > 1
        3.06 二进制:  11.0000 1111 0101 1100 0010 1000 1111 0101 1100 0010 1000 111 1011
        第一次出现1的位置 first1Index = 0 ; 小数点位置 pointIndex = 2;
        [...integer0b, ...fraction] = [1,1, 0,0,0,0, 1,1,1,1, 0,1,0,1, 1,1,0,0, 0,0,1,0, 1,0,0,0]
        mantissaBasis = 1 即：first1Index + 1 (二进制的小数点在后边)
        最后尾数部分是 100 0111 1010 1110 0001 0100
  */
  let mantissaBasis = first1Index + (first1Index < pointIndex ? 1: 0)
  // 尾数
  let mantissa = [...integer0b, ...fraction].slice(mantissaBasis, mantissaBasis + 23);
  // -----  尾数部分结束  -----

  // debugger;
  console.log('single', signal, 'e', e, 'mantissa: ', mantissa.join(''));

  // 32位浮点数编码数组
  const floatCodeArr = [signal, ...e0bArr, ...fillZero(mantissa, 23)];
  // debugger;
  console.log('floatCodeArr: ', floatCodeArr.join(''));
  return floatCodeArr;

   /**
   * @description 补充0位， 指数e和尾数需要8和23位，在此补充不足的尾数，用'0'填充
   *              例子: 12.5 的尾数是: 1001 需要补充到23位： 100 1000 0000 0000 0000 0000
   * @param { Array } arr
   * @param { number } len
   * @param { number } type head 往前添加； tail 往后添加 
   */
   function fillZero(arr, len, type = 'tail') {
    if(type === 'head') {
      return [...new Array(len - arr.length).fill('0'), ...arr];
    }else {
      // return arr.concat(new Array(len - arr.length).fill('0'));
      return [...arr , ...new Array(len - arr.length).fill('0')]
    }
    
  }
}


/**
 * 获取四个字节的Uint8Array， 比如 12.5 表示为 [65, 72, 0, 0]
 * 转化为4字节的十六进制： 41 48 00 00, 二进制，十进制，十六进制表示的数都一样，传给Uint8Array就行了, 直接用Uint8Array.from([0x41, 0x48, 0x00, 0x00])；
 * 例子 
   ['0','1','0','0','0','0','0','1','0','1','0','0','1','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0'] 
   => [0x41, 0x48, 0x00, 0x00] 或者十进制 [65, 72, 0, 0]
 * @param { double } num 
 * @return { Uint8Array }
 */
export function numToUnit8Array(num) {
  let floatCodeArr = double2floatCode(num);
  // debugger;
  let binaryArr = [];
  for(let i = 0; i < floatCodeArr.length; i = i + 8) {
    // 获取二进制字符串
    let binaryStr = '0b' + floatCodeArr.slice(i, i + 8).join('');
    binaryArr.push(binaryStr);
  }
  // 观察小米电机的数据，发现需要倒序
  return new Uint8Array(binaryArr.reverse());
}

可以生成位置模式指令需要的数据，所以可以进行下一步：生成指令，用策略模式来优化下


/**
 * @description 生成指令，采用策略模式，注意： 这里生成的是单一的指令,
 *              策略的命名为了简单，跟小米电机的文档保持命名一致
 * @param { string } type 策略类型
 * @param { obj } params 所需的参数，暂无法全部确定
 */
export function generateCMD(type, params = {}) {
  // ifelse判断的代码
  // let TWAI_id = new Array(4).fill(0);
  // let TWAI_data = new Array(8).fill(0);

  // if (type === "enable") {
  //   TWAI_id = [0x03, 0x00, 0xfd, 0x15];
  //   // TWAI_data = []
  // } else if (type === "disable") {
  //   TWAI_id = [0x04, 0x00, 0xfd, 0x15];
  // } else if (type === "jog5") {
  //   TWAI_id = [0x12, 0x00, 0xfd, 0x15];
  //   TWAI_data = [0x05, 0x70, 0x00, 0x00, 0x07, 0x01, 0x95, 0x54];
  // } else if (type === "jog0") {
  //   TWAI_id = [0x12, 0x00, 0xfd, 0x15];
  //   TWAI_data = [0x05, 0x70, 0x00, 0x00, 0x07, 0x00, 0x7f, 0xff];
  // }

  // console.log("TWAI_id: ", TWAI_id, "TWAI_data: ", TWAI_data);
  // let cmdFrame = Uint8Array.from([...TWAI_id, ...TWAI_data]);
  
  // 策略模式优化
  let TWAI_id = new Array(4).fill(0);
  let TWAI_data = new Array(8).fill(0);

  var Strategies =  {
    enable: () => {
      TWAI_id = [0x03, 0x00, 0xfd, 0x15];
    },
    disable: () => {
      TWAI_id = [0x04, 0x00, 0xfd, 0x15];
    },
    jog5: () => {
      TWAI_id = [0x12, 0x00, 0xfd, 0x15];
      TWAI_data = [0x05, 0x70, 0x00, 0x00, 0x07, 0x01, 0x95, 0x54];
    },
    jog0: () => {
      TWAI_id = [0x12, 0x00, 0xfd, 0x15];
      TWAI_data = [0x05, 0x70, 0x00, 0x00, 0x07, 0x00, 0x7f, 0xff];
    },
    // 设置速度，通信类型18， limit_spd: [0 - 30]
    limit_spd: ({motorId, limit_spd}) => {
      TWAI_id = [0x12, 0x00, 0xfd, motorId];
      TWAI_data = [0x17, 0x70, 0x00, 0x00, ...numToUnit8Array(limit_spd)];
    },
    // 设置要旋转的位置, 通信类型18
    loc_ref: ({motorId, loc_ref}) => {
      TWAI_id = [0x12, 0x00, 0xfd, motorId];
      TWAI_data = [0x16, 0x70, 0x00, 0x00, ...numToUnit8Array(loc_ref)];
    },
    // 更改运行模式
    run_mode: ({motorId, run_mode}) => {
      TWAI_id = [0x12, 0x00, 0xfd, motorId];
      TWAI_data = [0x05, 0x70, 0x00, 0x00, run_mode, 0x00, 0x00, 0x00];
    }
  }

  Strategies[type](params);
  console.log('策略模式: ', type , [...TWAI_id, ...TWAI_data]);
  return Uint8Array.from([...TWAI_id, ...TWAI_data]);
}

运行位置模式需要四条指令：

设置运行模式
使能
设置速度
设置位置

为了方便使用，也为了未来的扩展，因为还有其他三种模式：运控模式，电流模式，速度模式。所以在此使用建造者模式来优化：


/**
 * @description bulider类 使用建造者模式来构建指令数组
 * @return { Array<cmd> }
 */
class MotorRotateBuilder {
  motorId
  cmdArr = []

  setMotorId(motorId){
    this.motorId = motorId;
    return this;
  }

  enableCmd() {
    this.cmdArr.push(generateCMD('enable'))
    return this;
  }
  
  disableCmd() {
    this.cmdArr.push(generateCMD('disable'))
    return this;
  }

  runModeCmd() { }

  // 核心指令
  coreCmd() { }

  getCmdArr() {
    return this.cmdArr;
  }
}

/**
 * @description location_mode_bulid 位置模式的具体的实现类
 */
export class LocBuilder extends MotorRotateBuilder {
  constructor({ motorId }) {
    super();
    this.setMotorId(motorId)
  }
  runModeCmd() {
    let cmd = generateCMD('run_mode', {motorId: this.motorId, run_mode: 1})
    this.cmdArr.push(cmd);
    return this;
  }
  coreCmd({limit_spd, loc_ref}) {
    let limit_spd_cmd = generateCMD('limit_spd', {motorId: this.motorId, limit_spd});
    let loc_ref_cmd = generateCMD('loc_ref', {motorId: this.motorId, loc_ref});
    this.cmdArr.push(limit_spd_cmd, loc_ref_cmd);
    return this;
  }
}

/**
 * @description location_mode_director位置模式的指挥者
 * @param { object } { motorId: number, limit_spd: number, loc_ref: number(0-30) } 
 * @returns { Array<cmd> }
 */
export function LocDirector({motorId, limit_spd, loc_ref}) {
  // 小米电机的规范要求， 0 < limit_spd < 30
  if(limit_spd > 30) {
    limit_spd = 30;
  }else if(limit_spd < 0) {
    limit_spd = 0;
  }
  let loc_instance = LocBuilder({motorId}).runModeCmd().enableCmd().coreCmd({ limit_spd, loc_ref });
  return loc_instance.getCmdArr();
}