小车运行

小车接收线速度和角速度

小车通过USART5接收上位机发送的线速度和角速度

两轮差速运动

小车是一个刚体,所以轮子的每个位置角速度都是相同的, 而由于转弯半径不同,所以每个轮子的线速度会有所不同.

速度推导左右轮转速
已知线速度为V,角速度为:w 那么左右轮速度应该等于多少呢?

左右轮转速推导速度
已知左右轮的速度,我们推导当前小车前进的线速度和角速度
小车线速度公式:

小车角速度公式:

根据线速度和角速度计算左右轮子速度代码实现

//属性
//轮子直径
#define WHEEL_DIAMETER 0.032 
//一圈的信号数
#define WHEEL_TPR 1400
//左右轮子距离
#define WHEEL_DISTANCE 0.11
#define PI 3.14159265158
//根据线速度和角速度计算左右两个轮子的线速度
void calc_left_right_vel(float vel,float ang,float*vl,float *vr){
    *vl = vel - ang*0.5*WHEEL_DISTANCE;
    *vr = vel + ang*0.5*WHEEL_DISTANCE;
}

pid调节左右轮子移动速度

//接收上位机的线速度和角速度
float vel = 0.1;
float ang = -0.3;
//左右轮子目标速度
float vl = 0;
float vr = 0;
float hz = 20;
//左右轮子当前速度
float cur_vl = 0;
float cur_vr = 0;
float kp = 200;
float ki = 0;
float kd = 150;
//左轮误差
float pre_error_left = 0;
float pre_error_right = 0;
float total_error_left = 0;
float total_error_right = 0;
//左右轮子的pwm
float pwm_left = 0;
float pwm_right = 0;
//左侧轮子pid控制
int pid_left(float tar,float cur){
    float cur_error = tar-cur;
    pwm_left+=kp*cur_error;
    total_error_left+=cur_error;
    pwm_left+=ki*total_error_left;
    pwm_left+=kd*(cur_error-pre_error_left);
    pre_error_left=cur_error;
    return (int)pwm_left;
}
//右侧轮子pid控制
int pid_right(float tar,float cur){
    float cur_error = tar-cur;
    pwm_right+=kp*cur_error;
    total_error_right+=cur_error;
    pwm_right+=ki*total_error_right;
    pwm_right+=kd*(cur_error-pre_error_right);
    pre_error_right=cur_error;
    return (int)pwm_right;
}

控制主逻辑

while(1){
    //根据线速度和角速度计算
    calc_left_right_vel(vel,ang,&vl,&vr);
    //当前速度
    cur_vl = (bsp_encoder_get_left()*hz*1.0)/WHEEL_TPR*WHEEL_DIAMETER*PI;
    cur_vr = (bsp_encoder_get_right()*hz*1.0)/WHEEL_TPR*WHEEL_DIAMETER*PI;
    printf("vl:%f,vr:%f,cur_vl:%f,cur_vr:%f\n",vl,vr, cur_vl,cur_vr);
    //通过pid计算两侧的轮子pwm
    //bsp_motor_set(pid_left(vl,cur_vl),pid_right(vr,cur_vr));
    delay_1ms(1000/hz);
}