21 "Wanli" Raspberry Pi car - motor control learning (4-wheel speed control)

lb8820265 · Published on 2021-12-24 23:32

21 "Wanli" Raspberry Pi car - motor control learning (4-wheel speed control) [Copy link]

We have introduced the Raspberry Pi speed control before, but that was to control the speed of one wheel. As a result, the speed was sometimes slow and sometimes fast, but the error was acceptable. The next step is to control the speed of four wheels to see if the control effect is consistent.

Create a new file

Since we need to control the speed of the four wheels, it is necessary to write functions to achieve code reuse. Create two new files, Control.cpp and Control.h, and place them in the root directory, then add files to the "tasks.json" file.

"${fileDirname}/Control.cpp",

Coding

Control.h, writing of structure and function definitions.

#ifndef _CONTROL_H
#define _CONTROL_H

#include <stdio.h>
#include <iostream>
#include <wiringPi.h>
#include "Emakefun_MotorShield.h"
#define SPEED_INTEGRAL_MAX 1
struct Motor
{
int Pin_A;//广电编码A引脚
int Pin_B;//广电编码B引脚
float Speed_Now;//当前速度
float Speed_Set;//设置的速度
uint8_t Direction;//设置的方向
int Encoder_Cnt;//编码器计数
float Encoder_Integral;//编码器积分
Emakefun_DCMotor *Handle;//电机句柄
float Motor_out_float;//计算出来的电机输出值
uint8_t Motor_out;//转化为驱动输入的字节格式
};
void Encoder_Init();
void Motor_Init();
void Do_Motor(long Time_Period);
#endif

Control.cpp, implements the specific function and sets the parameters. Secondly, I slightly modified the motor driver library file. In the original driver library, direction control and torque control were two functions, which I merged into one.

#include "Control.h"
struct Motor M1={
.Pin_A=21,
.Pin_B=22,
};
struct Motor M2={
.Pin_A=23,
.Pin_B=24,
};
struct Motor M3={
.Pin_A=28,
.Pin_B=29,
};
struct Motor M4={
.Pin_A=25,
.Pin_B=27,
};
float Speed_Kp=255,Speed_Ki=100;
float Speed=0;
float Set_PWM_float=0;
uint8_t Set_PWM=0;
Emakefun_MotorShield Pwm ;
//外部中断回调函数，用来计数电机速度
void Interrupt_M1 (void) {
M1.Encoder_Cnt++;
}
void Interrupt_M2 (void) {
M2.Encoder_Cnt++;
}
void Interrupt_M3 (void) {
M3.Encoder_Cnt++;
}
void Interrupt_M4 (void) {
M4.Encoder_Cnt++;
}
//电机驱动初始化
void Motor_Init(){
Pwm = Emakefun_MotorShield();
  Pwm.begin(50);
 M1.Handle = Pwm.getMotor(1);
 M2.Handle = Pwm.getMotor(2);
 M3.Handle = Pwm.getMotor(3);
 M4.Handle = Pwm.getMotor(4);
  M1.Speed_Set=0.5;
  M2.Speed_Set=0.5;
  M3.Speed_Set=0.5;
  M4.Speed_Set=0.5;
  M1.Direction=FORWARD;
  M2.Direction=FORWARD;
  M3.Direction=FORWARD;
  M4.Direction=FORWARD;
}
//编码器驱动初始化
void Encoder_Init(){
wiringPiSetup () ;
 wiringPiISR (M1.Pin_A, INT_EDGE_BOTH, &Interrupt_M1) ;
 wiringPiISR (M1.Pin_B, INT_EDGE_BOTH, &Interrupt_M1) ;
   wiringPiISR (M2.Pin_A, INT_EDGE_BOTH, &Interrupt_M2) ;
 wiringPiISR (M2.Pin_B, INT_EDGE_BOTH, &Interrupt_M2) ;
   wiringPiISR (M3.Pin_A, INT_EDGE_BOTH, &Interrupt_M3) ;
 wiringPiISR (M3.Pin_B, INT_EDGE_BOTH, &Interrupt_M3) ;
   wiringPiISR (M4.Pin_A, INT_EDGE_BOTH, &Interrupt_M4) ;
 wiringPiISR (M4.Pin_B, INT_EDGE_BOTH, &Interrupt_M4) ;
}
void Get_Speed(long Time_Period)
{
   M1.Speed_Now=(float)M1.Encoder_Cnt/Time_Period*50;
   M1.Encoder_Cnt=0;
   M2.Speed_Now=(float)M2.Encoder_Cnt/Time_Period*50;
   M2.Encoder_Cnt=0;
   M3.Speed_Now=(float)M3.Encoder_Cnt/Time_Period*50;
   M3.Encoder_Cnt=0;
   M4.Speed_Now=(float)M4.Encoder_Cnt/Time_Period*50;
   M4.Encoder_Cnt=0;
}

//电机速度PI控制函数
float Speed_PI(float * Encoder_Integral,float Now_Speed,float Set_Speet)
{
 float fP;
 fP=Set_Speet-Now_Speed;
 *Encoder_Integral+=fP;
 if(*Encoder_Integral>SPEED_INTEGRAL_MAX)  *Encoder_Integral=SPEED_INTEGRAL_MAX; 
 return fP*Speed_Kp+(*Encoder_Integral)*Speed_Ki; 
}
//执行电机控制输出
void Out_Motro(){
 M1.Handle->my_run(M1.Motor_out,M1.Direction);
 M2.Handle->my_run(M2.Motor_out,M2.Direction);
 M3.Handle->my_run(M3.Motor_out,M3.Direction);
 M4.Handle->my_run(M4.Motor_out,M4.Direction);
}
//
void Speed_Control(){
M1.Motor_out_float=Speed_PI(&M1.Encoder_Integral,M1.Speed_Now,M1.Speed_Set);
M2.Motor_out_float=Speed_PI(&M2.Encoder_Integral,M2.Speed_Now,M2.Speed_Set);
M3.Motor_out_float=Speed_PI(&M3.Encoder_Integral,M3.Speed_Now,M3.Speed_Set);
M4.Motor_out_float=Speed_PI(&M4.Encoder_Integral,M4.Speed_Now,M4.Speed_Set);
if (M1.Motor_out_float>255){M1.Motor_out=255;}
else if(M1.Motor_out_float<0){M1.Motor_out=0;}
else{M1.Motor_out=(uint8_t)M1.Motor_out_float;}
if (M2.Motor_out_float>255){M2.Motor_out=255;}
else if(M2.Motor_out_float<0){M2.Motor_out=0;}
else{M2.Motor_out=(uint8_t)M2.Motor_out_float;}
if (M3.Motor_out_float>255){M3.Motor_out=255;}
else if(M3.Motor_out_float<0){M3.Motor_out=0;}
else{M3.Motor_out=(uint8_t)M3.Motor_out_float;}
if (M4.Motor_out_float>255){M4.Motor_out=255;}
else if(M4.Motor_out_float<0){M4.Motor_out=0;}
else{M4.Motor_out=(uint8_t)M4.Motor_out_float;}
}
void Do_Motor(long Time_Period){
Get_Speed( Time_Period);
Speed_Control();
Out_Motro();
//printf ("%ld,%f,%d,%f\n",Time_Period,M1.Speed_Now,M1.Motor_out,M1.Encoder_Integral);
printf ("%f,%f,%f,%f\n",M1.Speed_Now,M2.Speed_Now,M3.Speed_Now,M4.Speed_Now);
}

Speed_Control_4.cpp mainly sets a 50ms timer, then runs the speed control function in the timer, sets the speed of the four motors to 0.5, and then continuously prints out the speeds of the four wheels.

#include <wiringPi.h>
#include "Control.h"
long Cha;
long now,pre_time;
//定时器线程函数
PI_THREAD (timer)
{
for (;;)
{
  now=micros();
Cha=(now-pre_time);
  Do_Motor(Cha);
  pre_time= now;
  delay(50);
}
}
int main () {
  Motor_Init();
Encoder_Init();
  piThreadCreate (timer) ;
  while(1) {
  delay(1000);
}
}

Operation effect

When running, the speed of the wheel changes more dramatically than controlling one wheel. The speed of the same wheel changes greatly at different times, and the speed of different wheels also changes greatly at the same time. The maximum speed difference can be 20%. So using the Raspberry Pi to control the speed of 4 motors is obviously a failure.

Since speed feedback and PID control have failed, the only option is to use open-loop control, which is to directly give the control amount regardless of the actual speed. However, the control amount and speed need to be calibrated for each motor.

question

How to improve the real-time performance of Linux is a problem that must be solved. How should we do it?

Source code

链接已隐藏，如需查看请登录或者注册