嵌入式硬件篇-手柄控制控制麦克纳姆轮子
嵌入式硬件篇—手柄控制控制麦克纳姆轮子
前言
这段代码的作用是通过 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. 原理总结
- PS2手柄的 摇杆值通过计算与中立值 127 的差值 ，确 定摇杆的偏移方向和幅度 。
- 根据摇杆的偏移方向和幅度，调整四个电机的转速，实现小车的前后、左右、旋转等运动。
- 通过设置 死区（num） ，避免摇杆微小变化导致电机频繁调整。
- 最终通过 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 - 27 *2 + (-23) 2 - 47 2 = -194
car_right1 = 0 + 27* 2 + (-23) 2 + 47 2 = 102
car_left2 = 0 - 27 *2 + (-23) 2 + 47 2 = -6
car_right2 = 0 + 27* 2 + (-23) 2 - 47 2 = -86
通过 motor_speed_set 函数将这些值传递给电机控制器，实现小车的运动。