若机械组将新步兵底盘设计为三个全向轮,且使用键盘控制,将如何修改上一题的代码 #include<stdio.h> typedef struct { float ch[5]; } rc; int main(){ } void mecanum_wheel_speed(){ } void RC_control(){ } Void PID_calc{ }
时间: 2023-06-18 18:05:06 浏览: 164
如果机械组将新步兵底盘设计为三个全向轮,并且使用键盘控制,那么需要修改的部分就是mecanum_wheel_speed()函数中的计算方式。以下是修改后的代码示例:
```
#include <stdio.h>
#include <conio.h> //需要使用getch()函数来读取键盘输入
typedef struct {
float ch[5];
} rc;
rc rc_data; //定义一个全局变量,用于存储遥控器数据
int main() {
while (1) { //无限循环,实时读取键盘输入并控制底盘
if (kbhit()) { //如果有键盘输入
char c = getch(); //读取键盘输入
switch (c) { //根据不同的按键进行不同的操作
case 'w': //向前走
rc_data.ch[0] = 0.5f; //设定左前轮和右前轮的速度为0.5
rc_data.ch[1] = 0.5f;
rc_data.ch[2] = 0.0f; //设定后轮的速度为0
break;
case 's': //向后走
rc_data.ch[0] = -0.5f; //设定左前轮和右前轮的速度为-0.5
rc_data.ch[1] = -0.5f;
rc_data.ch[2] = 0.0f; //设定后轮的速度为0
break;
case 'a': //向左平移
rc_data.ch[0] = 0.0f; //设定左前轮的速度为0
rc_data.ch[1] = 0.5f; //设定右前轮的速度为0.5
rc_data.ch[2] = -0.5f; //设定后轮的速度为-0.5
break;
case 'd': //向右平移
rc_data.ch[0] = 0.0f; //设定左前轮的速度为0
rc_data.ch[1] = -0.5f; //设定右前轮的速度为-0.5
rc_data.ch[2] = 0.5f; //设定后轮的速度为0.5
break;
case 'q': //左转
rc_data.ch[0] = 0.0f; //设定左前轮和后轮的速度为-0.5
rc_data.ch[1] = 0.5f;
rc_data.ch[2] = -0.5f;
break;
case 'e': //右转
rc_data.ch[0] = 0.0f; //设定左前轮和后轮的速度为0.5
rc_data.ch[1] = -0.5f;
rc_data.ch[2] = 0.5f;
break;
default: //其他按键
rc_data.ch[0] = 0.0f; //设定所有轮的速度为0
rc_data.ch[1] = 0.0f;
rc_data.ch[2] = 0.0f;
break;
}
}
mecanum_wheel_speed(); //计算三个轮子的速度
RC_control(); //通过串口发送控制信号
PID_calc(); //计算PID控制器输出
}
}
void mecanum_wheel_speed() {
float vx = rc_data.ch[0]; //vx表示x方向的速度
float vy = rc_data.ch[1]; //vy表示y方向的速度
float w = rc_data.ch[2]; //w表示角速度
float v1 = vx - w; //计算左前轮的速度
float v2 = vy + w; //计算右前轮的速度
float v3 = vx + w; //计算后轮的速度
//将三个轮子的速度存储到全局变量中,以便后续的控制操作使用
rc_data.ch[0] = v1;
rc_data.ch[1] = v2;
rc_data.ch[2] = v3;
}
void RC_control() {
//通过串口发送遥控器数据
}
void PID_calc() {
//计算PID控制器输出
}
```
在上述代码中,我们使用了getch()函数来实时读取键盘输入,并根据不同的按键进行不同的操作。在mecanum_wheel_speed()函数中,我们使用了全向轮的运动学模型来计算出三个轮子的速度,并将这三个速度存储到rc_data结构体中,以便后续的控制操作使用。其他部分的代码与之前的代码基本相同,不再赘述。