1.前言

在前一篇中，已经基本确认了整个的硬件架构。
接下来先搞定机械臂的Solidworks建模以及导出urdf文件，以及PCA9685对舵机的测试。

2.机械手Solidworks建模及其URDF文件导出

2.1.机械手建模

等了几天，购买的舵机机械手配件都已经到了。粗略组装了一下。

然后根据商家提供的一些配件图纸，利用solidworks基本建立了模型。但是由于提供的图像缺失了一些尺寸，以及某些尺寸的标注得奇奇怪怪的，因此绘制出来的模型与实际并不完全相符，另外有些地方我也做了简化（比如爪子）处理、懒得绘制复杂的外形（底座），因此绘制的模型仅供参考。不过，对于本次“科研项目”，只要不影响moveit使用就行。几百块，只能将就将就了😄。

solidworks里面的模型的底座和实际的底座不一样，但是都是起到支撑作用，且提供了绕Z轴转动的功能，在一定范围内不影响轨迹的计算。

2.2.urdf文件导出

这里需要用到一个插件，可以参考这里【Solidworks导出URDF总结（Noetic）】
注意导出之前，先把模型的各个关节摆弄到0度位置，否则后面控制需要反复核对角度。
我用的是Solidworks2020_sp0，然后导出时选择保存mesh，这样我们就得到可以用在ros的模型了。
但是该插件导出的演示工程是适用于ROS1的，我们的ROS2用不了，所以我们只要urdf、meshes这两个文件夹就行了。
每次（修改）导出后，记得都要将meshes、urdf两个文件夹都拷贝覆盖过去，不能只拷贝一个，否则可能会对不上。

假如导出的模型在导入moveit时出现零件错位，可以看看这里，主要是坐标旋转问题（将3.1416改成0）。
但是我的是要将某个角度从0改成3.1416，这个可能要实际问题，实际分析吧。

2.3.urdf文件微调

导出后，参考【8.2 RVIZ2可视化移动机器人模型】先利用ros的joint_state_publisher_gui观察、测试一下模型，注意根据实际情况，修改旋转轴的方向，使其与实际的马达旋转方向一致：
假如不一致，修改一下轴的正负号。

2.4.moveit导入urdf测试

参考前面的【在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手】，将urdf文件经过moveit-setup-assistant配置好之后，应该就可以在rviz中摆弄了。
这次只是简单测试一下solidwork导出的urdf模型是否能够在moveit中使用，其他的（节点通讯、轨迹规划等）等下次再搞。

3.PCA9685通讯及舵机调试

3.1.PCA9685通讯

一般来说，舵机的控制周期为20ms，也就是频率为50Hz。然后有些舵机的0-180/0-360度分辨对应0.5ms-2.5ms的脉宽。360度舵机、180度舵机可能不一样，具体要自己试一下，不过好像都是以1.5ms为基准位置，然后向左右偏移0.5ms或者1.0ms。具体信息可以参考【PCA9685：I2C转16路PWM，助力你的系统】，以及【舵机的控制原理】。我目前用的DS3115的角度范围是0-180度，对应脉宽是0.5ms-2.5ms。
因此，需要将PCA9685设置为50HZ，然后在一定范围内调节脉冲宽度。具体的设置流程可以参考【SG90模拟舵机控制及PCA9685模块的使用】，以及【pca9685使用教程以及proteus仿真】
本次我使用与PCA9685进行IIC通信的芯片为STM32F103C8T6，关于stm32的开发，可以查看我之前的一篇博客【stm32cubeIDE的使用】，只要手动点击几下就可以将IIC通讯配置好了。
与PCA9685通讯的主要代码如下：
pca9685.h

/*
 * pca9685.h
 *
 *  Created on: 2023年3月24日
 *      Author: Administrator
 */

#ifndef INC_PCA9685_H_
#define INC_PCA9685_H_

typedef unsigned char u8;
typedef unsigned short u16;

#define pca9685Addr 0x40 // pca9685模块的地址

#define PCA9685_MODE1    0x00   //MODE1寄存器地址
#define PCA9685_PRESCALE 0xFE   //PRE_SCALE寄存器地址


u8 IIC_Write_Len(u8 addr, u8 reg, u8 len, u8 *buf);
u8 IIC_Read_Len(u8 addr, u8 reg, u8 len, u8 *buf);

u8 pca9685_Read_Reg(u8 reg);
void pca9685_Write_Reg(u8 reg, u8 dat);

void Set_Duty(u8 num, u16 off);

void pca9685_setPWMFreq(float freq);
void pca9685_Init(float Hz);

// -180: 对应占空比 0.5ms
// -90:  对应占空比 1ms
// 0:    对应占空比 1.5ms
// 90:   对应占空比 2ms
// 180:  对应占空比 2.5ms
// 经过别人的计算，角度精度好像是0.439， https://zhuanlan.zhihu.com/p/67336644
int set_angle(u8 channel, float angle);


#endif /* INC_PCA9685_H_ */

pca9685.c

/*
 * pca9685.c
 *
 *  Created on: 2023年3月24日
 *      Author: Administrator
 */
#include "pca9685.h"

#include "i2c.h"
#include "usart.h"
#include <math.h>

//IIC连续写
 u8 IIC_Write_Len(u8 addr, u8 reg, u8 len, u8 *buf)
 {
	 addr = (addr << 1) | 0;

	 u8 data[10] = {0};
	 data[0] = reg;
	 for(int i = 0; i < len; i++)
	 {
		 data[1 + i] = buf[i];
	 }

	 if(HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)addr, (uint8_t*)data, len + 1, 10000)!= HAL_OK)
	 {
		 printf("++write len byte fail\r\n");
		 return 1;
	 }

	 return 0;
 }

 //IIC连续读
 u8 IIC_Read_Len(u8 addr, u8 reg, u8 len, u8 *buf)
 {
	 u8 tmp  = (addr << 1) | 0;
	 if(HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)tmp, (uint8_t*)&reg, 1, 10000)!= HAL_OK)
	 {
		 printf("++write byte fail\r\n");
		 return 1;
	 }

	 tmp  = (addr << 1) | 1;
	 if(HAL_I2C_Master_Receive(&hi2c1, (uint16_t)tmp, (uint8_t *)buf, len, 10000) != HAL_OK)
	 {
		 printf("--read byte fail\r\n");
		 return 2;
	 }

	 return 0;
 }


 unsigned char pca9685_Read_Reg(u8 reg)
 {
	 unsigned char dat;
	 IIC_Read_Len(pca9685Addr, reg, 1, &dat);
	 return dat;
 }
 void pca9685_Write_Reg(u8 reg, u8 dat)
 {
	 IIC_Write_Len(pca9685Addr, reg, 1, &dat);
 }

 void pca9685_setPWMFreq(float freq)
 {
 	u8 prescale, oldmode, newmode;

 	float prescaleval;

 	freq *= 0.92;   							//纠正频率设置中的过冲，进行校准
 	prescaleval = 25000000;						//根据公式计算prescale的值
 	prescaleval /= 4096;						//prescaleval=round(osc_cloc/4096/freq)-1;
 	prescaleval /= freq;
 	prescaleval -= 1;
 	prescale = floor(prescaleval + 0.5);

 	oldmode = pca9685_Read_Reg(PCA9685_MODE1);		  //获得MODE1寄存器值
 	newmode = (oldmode & 0xEF) | 0x10; 			      //SLEEP位 置1
 	pca9685_Write_Reg(PCA9685_MODE1, newmode);  	  //进入SLEEP模式
 	pca9685_Write_Reg(PCA9685_PRESCALE, prescale);    //设置频率
 	pca9685_Write_Reg(PCA9685_MODE1, oldmode);		  //退出SLEEP模式

    HAL_Delay(2);

    pca9685_Write_Reg(PCA9685_MODE1, oldmode | 0xa1); //RESTART、AI、ALLCALL三个位 置1
 }

 // 初始化pca9685
 void pca9685_Init(float Hz)
 {
//     pca9685_Write_Reg(0x00,0x10);
//     pca9685_Write_Reg(0xfe,(char)((25000000/4096)/Hz)-1);
//     pca9685_Write_Reg(0x00,0x00);

   // 这个函数有时可以，有时不可以，不知道为啥.不行的时候，用上面那三行进行初始化
	 pca9685_setPWMFreq(Hz);

     HAL_Delay(1);
 }

 void Set_Duty(u8 num, u16 off)//占空比乘上4096为off的值
 {
     pca9685_Write_Reg(num*4+6, 0);
     pca9685_Write_Reg(num*4+7, 0);
     pca9685_Write_Reg(num*4+8, off&0xff); // L
     pca9685_Write_Reg(num*4+9, off>>=8);  // H
 }

 int set_angle(u8 channel, float angle)
 {
	 if(angle < -90 || angle > 90)
	 {
		 return -1;
	 }

	 float ratio = 2.0 / 180.0;
	 float pulseWidth = 0.5 + (angle + 90) * ratio; // 计算当前所需要设置的脉宽

	 Set_Duty(channel, (pulseWidth / 20.0) * 4096);
 }

main.c

...

int main(void)
{
 ...
  pca9685_Init(50.0);
 ...
  while (1)
  {
   ...
	  static float angle = -90.0;
	  if(angle <= 90)
	  {
		  angle += 1.0;
	  }
	  else
	  {
		  angle = -90.0;
	  }
	  set_angle(0, angle);

	  HAL_Delay(50);
 ...
  }
}

3.2.舵机调试及安装

针对每个舵机都给到0度的pwm信号，然后按照此处位置安装到机械臂上。机械臂安装完成后，理应和solidworks中的模型的0度位置一致。
可以在调试好舵机后，在舵机上用记号笔做一些标记，表明舵机转动的起点、终点、方向等，方便整体机械手的安装。
我这里人为规定舵机的转动范围为[-90, 90],然后安转时，以0度安装。

另外，DS3115电机据说支持50-330HZ的脉冲，本来以为脉冲上去，转动速度会快一些，但是经过测试，速度还是一样的。
但是由于电机的控制还是通过脉宽范围【0.5ms-2.5ms】来控制的，而对于pca9685而已，pwm周期可以进行4096等分，在脉宽范围不变的前提下，脉宽范围覆盖到了更多的等分区域，因此可以进行了更精细的控制。比如在50HZ的脉冲中，从【PCA9685：I2C转16路PWM，助力你的系统】可以知道舵机的步进角度为0.438度，而把频率改为100HZ后，步进角度就可以达到原来的一半：0.219度，提高了控制精度。

4.总结

经过上面的操作，舵机就基本在我们的掌控之中了。
下一步就可以通过串口建立电脑与stm32的通讯，从而实现电脑上发送相应的指令控制任意舵机的转动角度。