嵌入式硬件篇---手柄控制控制麦克纳姆轮子

---

---

前言

这段代码的作用是通过PS2手柄的摇杆数据来控制小车的四个电机转速。下面我将详细解释这段代码的逻辑、PS2手柄的参数如何转化为小车转速，以及其背后的原理。

---

1. 变量定义

float car_left1=0, car_right1=0, car_left2=0, car_right2=0;
static float car_left1_bak = 0, car_right1_bak = 0, car_left2_bak = 0, car_right2_bak = 0;

car_left1, car_right1, car_left2, car_right2：分别表示小车四个电机的目标转速。
car_left1_bak, car_right1_bak, car_left2_bak, car_right2_bak：用于保存上一次的电机转速值，用于判断是否需要更新电机转速。

2. 摇杆死区设置

static u8 num = 10;

num 是一个阈值，用于设置摇杆的“死区”。当摇杆的值变化小于 num 时，忽略这些微小的变化，避免电机频繁调整。

摇杆的原始值范围是 0~255，中间值是 127。如果摇杆的值与 127 的差值小于 num，则认为摇杆处于“中立”位置。

3. 模式检查

if (psx_buf[1] != PS2_LED_RED)
	return;

psx_buf[1] 是PS2手柄的当前模式（例如红灯模式或绿灯模式）。
如果当前模式不是红灯模式（PS2_LED_RED），则直接返回，不处理摇杆数据。

4. 摇杆数据处理

PS2手柄的摇杆数据存储在 psx_buf 数组中：
psx_buf[5]：左摇杆的水平值（X轴）。
psx_buf[6]：左摇杆的垂直值（Y轴）。
psx_buf[7]：右摇杆的水平值（X轴）。
psx_buf[8]：右摇杆的垂直值（Y轴）。
摇杆值的范围是 0~255，中间值是 127。通过计算摇杆值与 127 的差值，可以确定摇杆的偏移方向和幅度。

4.1 右摇杆垂直值（psx_buf[7]）

if (abs_int(127 - psx_buf[7]) > num)
{
	car_left1 = car_left1 - (0x7f - psx_buf[7]) * 2;
	car_right1 = car_right1 + (0x7f - psx_buf[7]) * 2;
	car_left2 = car_left2 - (0x7f - psx_buf[7]) * 2;
	car_right2 = car_right2 + (0x7f - psx_buf[7]) * 2;
}

右摇杆的垂直值控制小车的前后运动。
(0x7f - psx_buf[7])：计算摇杆值与中立值 127 的差值。
如果摇杆向上推（psx_buf[7] < 127），差值为正，小车向前运动。
如果摇杆向下拉（psx_buf[7] > 127），差值为负，小车向后运动。
将差值放大，增加电机转速的灵敏度。
通过调整 car_left1, car_right1, car_left2, car_right2 的值，控制四个电机的转速。

4.2 右摇杆水平值（psx_buf[8]）

if (abs_int(127 - psx_buf[8]) > num)
{
	car_left1 = car_left1 + (0x7f - psx_buf[8]) * 2;
	car_right1 = car_right1 + (0x7f - psx_buf[8]) * 2;
	car_left2 = car_left2 + (0x7f - psx_buf[8]) * 2;
	car_right2 = car_right2 + (0x7f - psx_buf[8]) * 2;
}

右摇杆的水平值控制小车的左右平移。
(0x7f - psx_buf[8])：计算摇杆值与中立值 127 的差值。
如果摇杆向右推（psx_buf[8] > 127），差值为负，小车向右平移。
如果摇杆向左推（psx_buf[8] < 127），差值为正，小车向左平移。
将差值放大，增加电机转速的灵敏度。

4.3 左摇杆水平值（psx_buf[5]）

if (abs_int(127 - psx_buf[5]) > num)
{
	car_left1 = car_left1 - (0x7f - psx_buf[5]) * 2;
	car_right1 = car_right1 + (0x7f - psx_buf[5]) * 2;
	car_left2 = car_left2 + (0x7f - psx_buf[5]) * 2;
	car_right2 = car_right2 - (0x7f - psx_buf[5]) * 2;
}

左摇杆的水平值控制小车的旋转。
(0x7f - psx_buf[5])：计算摇杆值与中立值 127 的差值。
如果摇杆向右推（psx_buf[5] > 127），差值为负，小车顺时针旋转。
如果摇杆向左推（psx_buf[5] < 127），差值为正，小车逆时针旋转。
将差值放大，增加电机转速的灵敏度。

4.4 左摇杆垂直值（psx_buf[6]）

if (abs_int(127 - psx_buf[6]) > num)
{
	car_left1 = car_left1 + (0x7f - psx_buf[6]) * 2;
	car_right1 = car_right1 + (0x7f - psx_buf[6]) * 2;
	car_left2 = car_left2 + (0x7f - psx_buf[6]) * 2;
	car_right2 = car_right2 + (0x7f - psx_buf[6]) * 2;
}

左摇杆的垂直值控制小车的前后运动（与右摇杆垂直值类似）。
(0x7f - psx_buf[6])：计算摇杆值与中立值 127 的差值。
如果摇杆向上推（psx_buf[6] < 127），差值为正，小车向前运动。
如果摇杆向下拉（psx_buf[6] > 127），差值为负，小车向后运动。
将差值放大，增加电机转速的灵敏度。

5. 电机转速更新

if ((car_left1_bak != car_left1) || (car_right1_bak != car_right1) || (car_left2_bak != car_left2) || (car_right2_bak != car_right2))
{
	motor_speed_set(car_left1 / 1000, car_right1 / 1000, car_left2 / 1000, car_right2 / 1000);
}

如果当前计算的电机转速与上一次保存的值不同，则调用 motor_speed_set 函数更新电机转速。
car_left1 / 1000：将转速值缩小，可能是为了适配电机控制器的输入范围。

6. 保存当前转速值

car_left1_bak = car_left1;
car_right1_bak = car_right1;
car_left2_bak = car_left2;
car_right2_bak = car_right2;

保存当前计算的电机转速值，用于下一次比较。

7. 原理总结

1. PS2手柄的摇杆值通过计算与中立值 127 的差值，确定摇杆的偏移方向和幅度。
2. 根据摇杆的偏移方向和幅度，调整四个电机的转速，实现小车的前后、左右、旋转等运动。
3. 通过设置死区（num），避免摇杆微小变化导致电机频繁调整。
4. 最终通过 motor_speed_set 函数将计算出的转速值传递给电机控制器，实现小车的运动控制。

8. 参数转化示例

假设

psx_buf[7] = 100（右摇杆向上推）。
psx_buf[8] = 150（右摇杆向右推）。
psx_buf[5] = 80（左摇杆向左推）。
psx_buf[6] = 127（左摇杆中立）。

计算

右摇杆垂直值：127 - 100 = 27，差值为正，小车向前运动。
右摇杆水平值：127 - 150 = -23，差值为负，小车向右平移。
左摇杆水平值：127 - 80 = 47，差值为正，小车逆时针旋转。
左摇杆垂直值：127 - 127 = 0，无变化。

最终电机转速

car_left1 = 0 - 272 + (-23)2 - 472 = -194
car_right1 = 0 + 272 + (-23)2 + 472 = 102
car_left2 = 0 - 272 + (-23)2 + 472 = -6
car_right2 = 0 + 272 + (-23)2 - 472 = -86

通过 motor_speed_set 函数将这些值传递给电机控制器，实现小车的运动。

---