52“万里”树莓派小车——小车速度与距离控制（视频展示）

lb8820265 发表于 2024-3-9 21:48

<div class='showpostmsg'> 本帖最后由 lb8820265 于 2024-3-9 21:44 编辑

先上视频：

<iframe allowfullscreen="true" border="0" frameborder="no" framespacing="0" height="450px" scrolling="no" src="//player.bilibili.com/player.html?aid=1951581361&bvid=BV1RC411a7eP&cid=1464195945&p=1" width="700px"></iframe> 
    前面有使用树莓派4B采集编码器的值计算电机速度，进而进行速度控制，最后以失败告终，<a href="https://bbs.eeworld.com.cn/thread-1190177-1-1.html" target="_blank">电机控制学习（4轮速度控制）</a>,我总结是树莓派操作系统为非实时的，这次换成嵌入式的PicoW加上500线的编码器，应该是不存在这个问题的。

    速度控制的原理是在定时器中获取电机的速度，然后使用PD控制算法，通过控制PWM占空比对电机速度进行控制。前面有介绍<a href="https://bbs.eeworld.com.cn/thread-1261679-1-1.html" target="_blank">PWM，定时器</a>和<a href="https://bbs.eeworld.com.cn/thread-1262948-1-1.html" target="_blank">PIO编码器</a>外设的使用，可以参考。

定时器

    在初始化中设置50ms定时器，代码如下：  

<pre>
<code class="language-cpp"> add_repeating_timer_ms(-50, repeating_timer_callback, NULL, &timer);</code></pre>

    在定时器回调函数中获取电机速度，首先获取两次定时器的时间差，这样是确保获取更加准确的时间差。然后用当前的编码器值减去上次获取的编码器值，然后乘以一个校准值就是当前的速度值。然后就是Car_Control()和Servo_Control()分别用来控制小车和舵机，代码如下：

<pre>
<code class="language-cpp">
bool repeating_timer_callback(struct repeating_timer *t) {
static absolute_time_t t_from;
absolute_time_t t_to;
int64_t t_delta;
int Encoder_New_Value_1,Encoder_New_Value_2;
static int Encoder_Old_Value_1,Encoder_Old_Value_2;
t_to=get_absolute_time();
t_delta=absolute_time_diff_us(t_from,t_to);
t_from=t_to;
Encoder_New_Value_1 = -quadrature_encoder_get_count(pio, SM_Encoder_A);
Control_State.SPD_Real_A = (Encoder_New_Value_1 - Encoder_Old_Value_1)/(float)t_delta*1000;
Encoder_Old_Value_1 = Encoder_New_Value_1;
Encoder_New_Value_2 = quadrature_encoder_get_count(pio, SM_Encoder_B);
Control_State.SPD_Real_B = (Encoder_New_Value_2 - Encoder_Old_Value_2)/(float)t_delta*1000;
Encoder_Old_Value_2 = Encoder_New_Value_2;
Car_Control();
Servo_Control();
Trigger_LED_Flag=true;
}</code></pre>

Car_Control()函数

    Car_Control()函数是小车控制的主要函数，函数中首先判断控制模式，分为及时控制以及距离控制，在这两个控制模式中又都分为PWM直接控制与速度控制。其中距离控制还需要在主函数中进行配合，代码如下：

<pre>
<code class="language-cpp">void Car_Control(void){
static int CMD_Dis_Mode_Pre=0;
int Motor_PWM_A_Set=0,Motor_PWM_B_Set=0;
if(Control_State.DIS_Dis_Mode!=0){//说明此时距离模式启用
switch(Control_State.DIS_Speed_Mode)
{
case 1://距离控制下使用PWM直接控制
Motor_PWM_A_Set=(Control_State.DIS_Down_A==1)?0:(Control_State.DIS_Motor_PWM_A*10);
if(Control_State.DIS_Set_A<0)Motor_PWM_A_Set=-Motor_PWM_A_Set;
Motor_PWM_B_Set=(Control_State.DIS_Down_B==1)?0:(Control_State.DIS_Motor_PWM_B*10);
if(Control_State.DIS_Set_B<0)Motor_PWM_B_Set=-Motor_PWM_B_Set;
break;
case 2://距离控制下使用速度控制
Motor_PWM_A_Set=(Control_State.DIS_Down_A==1)?0:(Speed_PI(Control_State.DIS_Real_A,Control_State.DIS_Set_A,&Control_State.SPD_A_Encoder_Integral));
if(Control_State.DIS_Set_A<0)Motor_PWM_A_Set=-Motor_PWM_A_Set;
Motor_PWM_B_Set=(Control_State.DIS_Down_B==1)?0:(Speed_PI(Control_State.DIS_Real_B,Control_State.DIS_Set_B,&Control_State.SPD_B_Encoder_Integral));
if(Control_State.DIS_Set_B<0)Motor_PWM_B_Set=-Motor_PWM_B_Set;
break;
}
}else if(Control_State.CMD_Speed_Mode!=0){//说明及时控制模式启用
switch(Control_State.CMD_Speed_Mode)
{
case 1://及时控制模式下PWM直接控制
Motor_PWM_A_Set=Control_State.Motor_PWM_A*10;
Motor_PWM_B_Set=Control_State.Motor_PWM_B*10;
break;
case 2://及时控制下使用速度控制
Motor_PWM_A_Set=Speed_PI(Control_State.SPD_Real_A,Control_State.SPD_Set_A,&Control_State.SPD_A_Encoder_Integral);
Motor_PWM_B_Set=Speed_PI(Control_State.SPD_Real_B,Control_State.SPD_Set_B,&Control_State.SPD_B_Encoder_Integral);
break;
}
}else{//仅仅心跳
}
printf("A_PWM %6d,B_PWM %6d\n", Motor_PWM_A_Set,Motor_PWM_B_Set);
Motor_Control(1,Motor_PWM_A_Set);
Motor_Control(2,Motor_PWM_B_Set);
}</code></pre>

距离控制判断函数

    在距离控制中，需要在主函数中需要不断的判断是否达到设定的距离，然后需要马上停止，但是实际操作起来还是挺复杂的，需要有多个标志位配合来判断，且在接收到距离控制指令后需要保存编码器当时的值，且保存接收到的距离控制指令，因为距离指令通常不会一直发，代码代码如下：

<pre>
<code class="language-cpp">void Car_Distance_Stop_Check(void){
if(Control_State.DIS_Dis_Mode!=0){
switch(Control_State.DIS_Dis_Mode)
{
case 1:
Control_State.DIS_Real_A= quadrature_encoder_get_count(pio, SM_Encoder_A)+Control_State.DIS_Zero_Point_A;
Control_State.DIS_Real_B=-(quadrature_encoder_get_count(pio, SM_Encoder_B)-Control_State.DIS_Zero_Point_B); if(abs(Control_State.DIS_Real_A)>=abs(Control_State.DIS_Set_A)&&Control_State.DIS_Down_A==0){
Control_State.DIS_Down_A=1;
Motor_Control(1,0);
// printf("DIS_Down_A");
} if(abs(Control_State.DIS_Real_B)>=abs(Control_State.DIS_Set_B)&&Control_State.DIS_Down_B==0){
Control_State.DIS_Down_B=1;
Motor_Control(2,0);
// printf("DIS_Down_B");
}
if(Control_State.DIS_Down_A==1&&Control_State.DIS_Down_B==1){
Control_State.DIS_Dis_Mode=0;
}
break;
}
}
}</code></pre>

速度PI控制函数

    在Car_Control()函数中有对速度进行控制，将获取的速度与实际的速度进行PI控制，函数中有P、I和积分上限参数需要调整。代码如下：

<pre>
<code class="language-cpp">float Speed_PI(float Now_Speed,float Set_Speet,float * Encoder_Integral)
{
float Speed_Kp=3,Speed_Ki=1;
float SPEED_INTEGRAL_MAX=1000;
float fP;
fP=Set_Speet-Now_Speed;
*Encoder_Integral+=fP;
if(*Encoder_Integral>SPEED_INTEGRAL_MAX){
*Encoder_Integral=SPEED_INTEGRAL_MAX;
}else if(*Encoder_Integral<-SPEED_INTEGRAL_MAX){
*Encoder_Integral=-SPEED_INTEGRAL_MAX;
}
return fP*Speed_Kp+*Encoder_Integral*Speed_Ki;
}</code></pre>

电机控制函数

    无论什么控制模式，最后都要对电机进行控制，TB6612FNG电机驱动模块可以控制两个电机，一个PWM口用来控制电机，两个IO口控制正反转和急停，函数参数为电机位置和PWM占空比。代码如下：

<pre>
<code class="language-cpp">void Motor_Control(int M,int PWM){
if(PWM>1000){PWM=1000;}
else if(PWM<-1000){PWM=-1000;}
if(M==1){
if(PWM==0){
gpio_put(PIN_Motor_A_IN_1, 1);
gpio_put(PIN_Motor_A_IN_2, 1);
}else if(PWM>0){
gpio_put(PIN_Motor_A_IN_1, 1);
gpio_put(PIN_Motor_A_IN_2, 0);
pwm_set_gpio_level(PIN_Motor_A_PWM, PWM);
}else{
gpio_put(PIN_Motor_A_IN_1, 0);
gpio_put(PIN_Motor_A_IN_2, 1);
pwm_set_gpio_level(PIN_Motor_A_PWM, -PWM);
}
}
else if(M==2){
if(PWM==0){
gpio_put(PIN_Motor_B_IN_1, 1);
gpio_put(PIN_Motor_B_IN_2, 1);
}else if(PWM>0){
gpio_put(PIN_Motor_B_IN_1, 0);
gpio_put(PIN_Motor_B_IN_2, 1);
pwm_set_gpio_level(PIN_Motor_B_PWM, PWM);
}else{
gpio_put(PIN_Motor_B_IN_1, 1);
gpio_put(PIN_Motor_B_IN_2, 0);
pwm_set_gpio_level(PIN_Motor_B_PWM, -PWM);
}
}
}</code></pre>

UDP接收函数

    PicoW接收手机发送过来的指令，对指令进行解析，然后对结构体进行赋值，同时将电机的速度和距离返回给手机端。这其中数据的位数和符号需要格外注意，要用memcpy而不是strcpy来复制数据。代码如下：

<pre>
<code class="language-cpp">void RcvFromUDP(void * arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t*addr,u16_t port)
{
uint8_t *buffer=calloc(p->len+1,sizeof(uint8_t));
memcpy(buffer,(uint8_t *)p->payload,p->len);//注意这里复制数据不能用strcpy，否则0x00后面的数据就收不到了！！！
if(buffer!=0x7F){
return;
}
Control_State.CMD_Speed_Mode= buffer&0x0f;
Control_State.CMD_Dis_Mode= (buffer&0xf0)>>4;
Control_State.Motor_PWM_A= (buffer>126) ? buffer-256 : buffer;
Control_State.Motor_PWM_B= (buffer>126) ? buffer-256 : buffer;
Control_State.SPD_Set_A= buffer*256+buffer;
if(Control_State.SPD_Set_A>32768)Control_State.SPD_Set_A-=65536;
Control_State.SPD_Set_B= buffer*256+buffer;
if(Control_State.SPD_Set_B>32768)Control_State.SPD_Set_B-=65536;
Control_State.DIS_Set_A= buffer*256+buffer;
if(Control_State.DIS_Set_A>32768)Control_State.DIS_Set_A-=65536;
Control_State.DIS_Set_B= buffer*256+buffer;
if(Control_State.DIS_Set_B>32768)Control_State.DIS_Set_B-=65536;
Control_State.Sover_PWM_A=(buffer>126) ? buffer-256 : buffer;
Control_State.Sover_PWM_B=(buffer>126) ? buffer-256 : buffer;
if(Control_State.CMD_Dis_Mode!=0){//位置指令，该指令在执行完之前，一般只执行一次
Control_State.DIS_Real_A=0;//复位位置
Control_State.DIS_Real_B=0;
Control_State.DIS_Motor_PWM_A=Control_State.Motor_PWM_A;//保存PWM直接控制值
Control_State.DIS_Motor_PWM_B=Control_State.Motor_PWM_B;
Control_State.DIS_SPD_A=Control_State.SPD_Set_A;//保存SPD控制值
Control_State.DIS_SPD_B=Control_State.SPD_Set_B;
Control_State.DIS_Down_A=0;
Control_State.DIS_Down_B=0;//是否完成标志位
Control_State.DIS_Dis_Mode=Control_State.CMD_Dis_Mode;//将位置指令保存，以防被下次数据覆盖
Control_State.DIS_Speed_Mode=Control_State.CMD_Speed_Mode;//将速度指令保存，以防被下次数据覆盖
Control_State.DIS_Zero_Point_A=-quadrature_encoder_get_count(pio, SM_Encoder_A);//将当前电机A的距离值保存

Control_State.DIS_Zero_Point_B=quadrature_encoder_get_count(pio, SM_Encoder_B);//将当前电机B的距离值保存
}
free(buffer);
pbuf_free(p);
Tx_buffer=254;
Tx_buffer=253;
memcpy(Tx_buffer+2, &Control_State.SPD_Real_A, 4);
memcpy(Tx_buffer+6, &Control_State.DIS_Real_A, 4);
memcpy(Tx_buffer+10, &Control_State.SPD_Real_B, 4);
memcpy(Tx_buffer+14, &Control_State.DIS_Real_B, 4);
SendUDP(addr,port,Tx_buffer,18);
// printf("R1 %6d,R2 %6d\n", Control_State.Motor_PWM_A,Control_State.Motor_PWM_B);
}</code></pre>

手机端

    手机端主要是发送控制指令与接收小车发过来的数据，主要界面和功能如下图所示。代码太分散可以直接参考源码。



<div>PicoW源码：</div>

<div>Android源码：</div>
</div><script> var loginstr = '<div class="locked">查看精华帖全部内容，请<a href="javascript:;" style="color:#e60000" class="loginf">登录</a>或者<a href="https://bbs.eeworld.com.cn/member.php?mod=register_eeworld.php&action=wechat" style="color:#e60000" target="_blank">注册</a></div>';

if(parseInt(discuz_uid)==0){
(function($){
var postHeight = getTextHeight(400);
$(".showpostmsg").html($(".showpostmsg").html());
$(".showpostmsg").after(loginstr);
$(".showpostmsg").css({height:postHeight,overflow:"hidden"});
})(jQuery);
}
</script><script type="text/javascript">(function(d,c){var a=d.createElement("script"),m=d.getElementsByTagName("script"),eewurl="//counter.eeworld.com.cn/pv/count/";a.src=eewurl+c;m.parentNode.insertBefore(a,m)})(document,523)</script>

freebsder 发表于 2024-3-12 18:49

这款app开发的很有土味

xjjbilly 发表于 2024-3-27 11:09

楼主很棒

页: [1]

电子工程世界-论坛's Archiver

52“万里”树莓派小车——小车速度与距离控制（视频展示）