本帖最后由 lb8820265 于 2024-3-9 21:44 编辑
先上视频：
前面有使用树莓派4B采集编码器的值计算电机速度，进而进行速度控制，最后以失败告终，电机控制学习（4轮速度控制）,我总结是树莓派操作系统为非实时的，这次换成嵌入式的PicoW加上500线的编码器，应该是不存在这个问题的。
速度控制的原理是在定时器中获取电机的速度，然后使用PD控制算法，通过控制PWM占空比对电机速度进行控制。前面有介绍PWM，定时器和PIO编码器外设的使用，可以参考。
定时器
在初始化中设置50ms定时器，代码如下：
add_repeating_timer_ms(-50, repeating_timer_callback, NULL, &timer);
在定时器回调函数中获取电机速度，首先获取两次定时器的时间差，这样是确保获取更加准确的时间差。然后用当前的编码器值减去上次获取的编码器值，然后乘以一个校准值就是当前的速度值。然后就是Car_Control()和Servo_Control()分别用来控制小车和舵机，代码如下：
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;
}
Car_Control()函数
Car_Control()函数是小车控制的主要函数，函数中首先判断控制模式，分为及时控制以及距离控制，在这两个控制模式中又都分为PWM直接控制与速度控制。其中距离控制还需要在主函数中进行配合，代码如下：
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);
}
距离控制判断函数
在距离控制中，需要在主函数中需要不断的判断是否达到设定的距离，然后需要马上停止，但是实际操作起来还是挺复杂的，需要有多个标志位配合来判断，且在接收到距离控制指令后需要保存编码器当时的值，且保存接收到的距离控制指令，因为距离指令通常不会一直发，代码代码如下：
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;
}
}
}
速度PI控制函数
在Car_Control()函数中有对速度进行控制，将获取的速度与实际的速度进行PI控制，函数中有P、I和积分上限参数需要调整。代码如下：
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;
}
电机控制函数
无论什么控制模式，最后都要对电机进行控制，TB6612FNG电机驱动模块可以控制两个电机，一个PWM口用来控制电机，两个IO口控制正反转和急停，函数参数为电机位置和PWM占空比。代码如下：
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);
}
}
}
UDP接收函数
PicoW接收手机发送过来的指令，对指令进行解析，然后对结构体进行赋值，同时将电机的速度和距离返回给手机端。这其中数据的位数和符号需要格外注意，要用memcpy而不是strcpy来复制数据。代码如下：
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[0]!=0x7F){
return;
}
Control_State.CMD_Speed_Mode= buffer[1]&0x0f;
Control_State.CMD_Dis_Mode= (buffer[1]&0xf0)>>4;
Control_State.Motor_PWM_A= (buffer[4]>126) ? buffer[4]-256 : buffer[4];
Control_State.Motor_PWM_B= (buffer[5]>126) ? buffer[5]-256 : buffer[5];
Control_State.SPD_Set_A= buffer[6]*256+buffer[7];
if(Control_State.SPD_Set_A>32768)Control_State.SPD_Set_A-=65536;
Control_State.SPD_Set_B= buffer[8]*256+buffer[9];
if(Control_State.SPD_Set_B>32768)Control_State.SPD_Set_B-=65536;
Control_State.DIS_Set_A= buffer[10]*256+buffer[11];
if(Control_State.DIS_Set_A>32768)Control_State.DIS_Set_A-=65536;
Control_State.DIS_Set_B= buffer[12]*256+buffer[13];
if(Control_State.DIS_Set_B>32768)Control_State.DIS_Set_B-=65536;
Control_State.Sover_PWM_A=(buffer[14]>126) ? buffer[14]-256 : buffer[14];
Control_State.Sover_PWM_B=(buffer[15]>126) ? buffer[15]-256 : buffer[15];
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[0]=254;
Tx_buffer[1]=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);
}
手机端
手机端主要是发送控制指令与接收小车发过来的数据，主要界面和功能如下图所示。代码太分散可以直接参考源码。
提升卡
变色卡
千斤顶
1/6 
京公网安备 11010802033920号
Copyright © 2005-2025 EEWORLD.com.cn, Inc. All rights reserved
