ROS2中用MoveIt2控制自己的舵机机械手（4）_ros控制舵机不动

1.前言

在上一篇【ROS2中用MoveIt2控制自己的舵机机械手（3）】，我们已经实现了用stm32单片机通过pca9685对舵机进行了允许范围内任意角度的控制。
这一篇，我们来实现上下位机的通讯协议，从而能够把机械手的运动轨迹发送至下位机执行。

2.通讯协议介绍

我们自定义一个通讯协议，包括了帧头、数据、帧尾等要素，具体信息如下所示：

2.1下位机实现

其中下位机（stm32）的实现为：
串口为usart1,然后上位机通过协议发送下来10字节数据，表示5个short（1个short占两字节）类型的数据。其中short类型数据表示对应关节需要转动到的角度。由于传递float比较麻烦，因此我们将原来的角度[-90, 90]乘以100后得到一个short类型，范围[-9000, 9000]，使用前记得要除以100.0

...
uint8_t  aRxBuffer[1];	    // HAL库USART接收Buffer
uint8_t  dataFrameBuffer[DF_BUFFER_LEN];	  // 数据帧缓存Buffer
uint16_t dataFrameIdx = 0;                    // 用于指示缓存位置
uint16_t dataLen = 0;                         // 数据长度
uint8_t  frameStatus = 0;                     // 当前数据帧状态

uint8_t  hasBuffer = 0; // 是否接收到了数据帧
float    poseBuffer[5]; // 上位机下发的关节状态，在main循环中再写入到pca9685


uint16_t getUInt16(uint8_t *dataPtr)
{
	return (*dataPtr << 8) | *(dataPtr + 1);
}
int16_t getInt16(uint8_t *dataPtr)
{
	return (*dataPtr << 8) | *(dataPtr + 1);
}

void clearAllStatus()
{
	dataFrameIdx = 0;
	frameStatus = 0;
}

// 对接收完整的数据帧帧进行处理
void processCmd()
{
	uint16_t dataLen = getUInt16(dataFrameBuffer + 2);

	int16_t pose1 = getInt16(dataFrameBuffer + 4);
	int16_t pose2 = getInt16(dataFrameBuffer + 6);
	int16_t pose3 = getInt16(dataFrameBuffer + 8);
	int16_t pose4 = getInt16(dataFrameBuffer + 10);
	int16_t pose5 = getInt16(dataFrameBuffer + 12);


	hasBuffer = 1;

	poseBuffer[0] = pose1;
	poseBuffer[1] = pose2;
	poseBuffer[2] = pose3;
	poseBuffer[3] = pose4;
	poseBuffer[4] = pose5;
}

// 处理上位机发送下来的串口信息
int processDataByte(uint8_t data)
{

	// 0 初始状态
	// 1 已收到帧头第一个字节   1byte
	// 2 已收到帧头第二个字节   1byte
	// 3 已收到数据长度       2byte
	// 4 已接收完数据        nbytes
	// 5 已收到校验位且校验正确  1byte
	// 6 已收到帧尾第一个字节   1byte
	// 7 已收到帧尾第二个字节   1byte
	switch(frameStatus)
	{
	case 0:{ //初始状态
		if(data != 0x12)
		{
			clearAllStatus();
			return -1;
		}

		dataFrameIdx = 0;
		dataFrameBuffer[dataFrameIdx] = data;
		frameStatus = 1;
	};break;
	case 1:{ //已经收到帧头第一个字节
		if(data != 0x34)
		{
			clearAllStatus();
			return -2;
		}

		dataFrameIdx = 1;
		dataFrameBuffer[dataFrameIdx] = data;
		frameStatus  = 2;
	};break;
	case 2:{ //已收到帧头第二个字节
		dataFrameIdx++;
		dataFrameBuffer[dataFrameIdx] = data;

		if(dataFrameIdx == 3)
		{
			// 计算接下来要接送的数据长度
			dataLen = getUInt16(dataFrameBuffer + 2);
			frameStatus = 3;
		}
	};break;
	case 3:{ //已收到数据长度, 开始接收数据
		dataFrameIdx++;
		if(dataFrameIdx > (DF_BUFFER_LEN - 1)) // 超过了buffer长度
		{
			clearAllStatus();
			return -3;
		}
		dataFrameBuffer[dataFrameIdx] = data;

		if(dataFrameIdx >= dataLen + 3) // 接收完数据
		{
			frameStatus = 4;
		}

	};break;
	case 4:{ //已接收完成数据
		dataFrameIdx++;
		dataFrameBuffer[dataFrameIdx] = data; // 此数据为校验位

		uint8_t sum = 0;
		for(int i = 4; i < dataFrameIdx; i++)
		{
			sum += dataFrameBuffer[i];
		}

		if(sum == data)
		{
			frameStatus = 5;
		}
		else
		{
			clearAllStatus();
			return -4;
		}

	};break;
	case 5:{ //收到校验位且校验正确
		if(data != 0x56)
		{
			clearAllStatus();
			return -5;
		}

		dataFrameIdx++;
		dataFrameBuffer[dataFrameIdx] = data;

		frameStatus = 6;
	};break;
	case 6:{ //收到帧尾第一个字节
		if(data != 0x78)
		{
			clearAllStatus();
			return -6;
		}

		dataFrameIdx++;
		dataFrameBuffer[dataFrameIdx] = data;

		processCmd(); // 处理数据帧

		clearAllStatus(); // 复位
	};break;
	default:break;
	}

	return 0;
}

// 串口接收中断函数
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle)
{

  if(UartHandle == &huart1)//如果是串口1
  {
	  int ret = 0;
	  ret = processDataByte(*aRxBuffer);

	  if(ret != 0)
	  {
		  printf("process ret:%d\r\n", ret);
	  }
	  HAL_UART_Receive_IT(&huart1, (uint8_t *)aRxBuffer, 1);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178

在接收到上位机的数值后，先缓存在poseBuffer中，然后再在main循环中慢慢再进行处理，防止中断函数的耗时操作

int main(void)
{
...

HAL_UART_Receive_IT(&huart1, (uint8_t *)aRxBuffer, 1); // 启动接收中断

while (1)
  {
  
	  // 将上位机下发的角度值写到PCA9685中
	  if(hasBuffer != 0)
	  {
		  hasBuffer = 0;

       // 还原原来的角度数据
		  float pose1 = poseBuffer[0] / 100.0;
		  float pose2 = poseBuffer[1] / 100.0;
		  float pose3 = poseBuffer[2] / 100.0;
		  float pose4 = poseBuffer[3] / 100.0;
		  float pose5 = poseBuffer[4] / 100.0;

      // 通过pca9685设置对应舵机的角度
		  set_angle(0, pose1);
		  set_angle(1, pose2);
		  set_angle(2, pose3);
		  set_angle(3, pose4);
		  set_angle(4, pose5);

		  printf("has set: %f, %f, %f, %f, %f\r\n",
				  pose1,
				  pose2,
				  pose3,
				  pose4,
				  pose5);
	  }

	  // 由于用的是开环的舵机，这里就不反馈实时的角度了，实际上也无法反馈。因为无法读取舵机的实时角度

...
	  HAL_Delay(50);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

2.1上位机实现

上位机的通讯主要程序（Qt）：

QByteArray uint16ToByteArray(quint16 val)
{
    QByteArray array;
    array.push_back(val >> 8);
    array.push_back(val & 0xff);
    return array;
}

quint8 checkSum(QByteArray data)
{
    quint8 sum = 0;

    for(int i = 0; i < data.length(); i++)
    {
        sum += *((uchar *)(data.data() + i));
    }

    return sum;
}

int sendData(QByteArray data)
{
    QByteArray dataBuffer;
    dataBuffer += QByteArray::fromHex("12 34");     // 帧头
    dataBuffer += uint16ToByteArray(data.length()); // 数据长度
    dataBuffer += data;                             // 数据
    dataBuffer += checkSum(data);                   // 校验字
    dataBuffer += QByteArray::fromHex("56 78");     // 帧尾

    qDebug() << dataBuffer.toHex();

    mSerialPort->write(dataBuffer);

    return 0;
}

// 发送若干个舵机的pose给下位机
int sendPose(QList<float> poseList)
{
    QByteArray data;
    foreach (float pose, poseList)
    {
        quint16 tmpPos = pose * 100;

//        qDebug() << tmpPos << pose;

        data += uint16ToByteArray(tmpPos);
    }

    return sendData(data);
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

3.总结

好了，这下子应该全链路打通了。下一篇应该大结局了。