21“万里”树莓派小车——电机控制学习（4轮速度控制）

lb8820265 · 发表于2021-12-24 23:32

21“万里”树莓派小车——电机控制学习（4轮速度控制） [复制链接]

前面介绍了树莓派速度控制，但那是控制一个轮子的速度，结果是速度还是有时慢有时快，但误差还能接受，接下来是控制四个轮子的速度看看控制的效果是否一致。

新建文件

由于要控制四个轮子的速度，那么编写函数就非常有必要了，可以实现代码复用。新建Control.cpp和Control.h两个文件放在根目录，然后在“tasks.json”文件中添加文件。

"${fileDirname}/Control.cpp",

代码编写

Control.h，结构体和函数定义的编写。

#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，对具体函数功能进行实现，以及参数的设置。其次稍微修改了一下电机驱动库文件，原来的驱动库，方向控制和力矩控制是两个函数，我将其合并为了一个。

#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，主要是设置一个50ms的定时器，然后在定时器中运行速度控制函数，四个电机的速度都设为0.5，然后不断打印出四个轮子速度。

#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);
}
}

运行效果

运行起来，轮子的速度变化比控制一个轮子更加剧烈，同一轮子在不同时间速度变化差异大，不同轮子在同一时间速度变化也大，最大能有20%的速度差。那么使用树莓派来控制4个电机的速度，显然是失败的。