Arduino电机测速与PID原理及代码实现

闭环控制

霍尔传感器编码器测速原理

来源：长话短说霍尔传感器编码器测速原理分析_哔哩哔哩_bilibili

编码器不仅可检测脉冲，还可以检测边沿

通过两个霍尔编码器可判断转向→输出有相位差的两组方波信号

eg. 在A的上升沿计数，B是高电平就正转、B是低电平就反转

电机测速代码1

来源：06_Arduino_PID教程_哔哩哔哩_bilibili

①定时器中断：在一定时间内计数

//定时器中断方法1：调库
#include<Timerone.h>  // <MsTimer2.h>
void setup()

{
　　Timer1.initialize(50000); // 每隔50000微秒调用一次中断函数
　　Timer1.attachInterrupt(timerIsr); // 中断函数为timerIsr

}
void timerIsr()

{
　　rpm = count / period * 60 / edges_per_cycle / reduction_ratio;
　　//rpm: 转/分，电机前端真实转速
　　//count: 50ms内的信号(上升)边沿数
　　//period:调用中断函数的时间间隔，此处是0.05s
　　//edges_per_cycle:电机后端每圈霍尔脉冲数，理论值为13
　　//reduction ratio: 电机减速比，理论值为20
　　count = 0;

}

【能调用库还是推荐调用库，前提是知道该库的功能】

//定时器中断方法2：手写（不推荐）

unsigned long curtime,oldtime;
void loop()
{
　　curtime = millis(); // 返回当前时间，单位为ms
　　int dt = abs(curtime - oldtime);
　　if(dt > 50)

　　{
　　　　rpm = count / period * 60 / edges_per_cycle /reduction_ratio;
　　　　count =0;
　　　　oldtime = millis();
　　}

}

②外部中断：计数的方法

如图：

假设希望计算A在某一段时间内的脉冲，需要知道何时产生脉冲（脉冲的上升沿在哪里）

因此检测边沿可用外部中断→出现一次下降沿就调用一次中断函数

//外部中断
int ENC A=2;//电机的编码器A端
int ENC_B=3;//电机的编码器B端
int count =0;//上升沿（脉冲）数量
void Code()
{
　　if(digitalRead(ENCB) == Low) //正转
　　{
　　　　count +1;
　　}
　　if(digitalRead(ENC_B) == HIGH) //反转

　　{
　　　　count -1;
　　}
void setup()
{
　　pinMode(ENC_A, INPUT);
　　pinMode(ENC_B, INPUT);
　　attachInterrupt(0, Code, FALLING);
　　//UN0管脚2对应参数为0,3对应参数为1
　　//在ENCA下降沿处执行Code
}

【更高级的方式：四倍频技术，计上下降沿，提高四倍精度→同时检测A、B的外部中断】

void setup()

{
　　pinMode(ENC_A,INPUT);
　　pinMode(ENC_B,INPUT);
　　attachInterrupt(0,Code0,CHANGE);
　　attachInterrupt(1,Code1,CHANGE);

}

void Code0()
{
　　if(ENCA=LOW) // 判断A为下降沿
　　{

　　　　if(digitalRead(ENC_B) == LOW) // 正转

　　　　{
　　　　　　count += 1;

　　　　}
　　　　if(digitalRead(ENC_B) == HIGH) // 反转

　　　　{
　　　　　　count -= 1;

　　　　}

　　}
　　else // 未完成

}
void Code1(){}
//未完成

电机测速代码2

来源：用示波器理解 Arduino 小车的测速方法_哔哩哔哩_bilibili

//Include the TimerOne Library from Paul Stoffregen
#include "TimerOne.h"

//Constants for Interrupt Pins
//Change values if not using Arduino Uno

const byte MOTOR1 = 2;  //Motor 1 Interrupt Pin - INT 0
const byte MOTOR2 = 3;  //Motor 2 Interrupt Pin - INT 1

//Integers for pulse counters
unsigned int counter1 = 0;
unsigned int counter2 = 0;
//Float for number of slots in encoder disk
float diskslots 30; //Change to match value of encoder disk

//Interrupt Service Routines

//Motor 1 pulse count ISR
void ISR_count1()
{
    counter1++;    // increment Motor 1 counter value
}
//Motor 2 pulse count ISR
void ISR_count2()
{ 
   counter2++;    // increment Motor 2 counter value
}

//TimerOne ISR
void ISR timerone (
{
    Timerl.detachInterrupt ();    //    Stop the timer
    Serial.print ("Motor Speed 1:");
    float rotationl = (counterl / diskslots) * 60.00;    //calculate RPM for Motor 1
    Serial.print (rotationl);
    Serial.print (" RPM - ");
    counterl = 0; // reset counter to zero
    Serial.print ("Motor Speed 2:");
    float rotation2 = (counter2 / diskslots) * 60.00; //calculate RPM for Motor 2
    Serial.print (rotation2);
    Serial.println (" RPM");
    counter2 = 0; // reset counter to zero
    Timerl.attachInterrupt ( ISR_timerone ); // Enable the timer
}

void setup()
{
    Serial.begin(9600);

    Timerl.initialize(1000000); // set timer for lsec
    attachInterrupt (digitalPinToInterrupt (MOTOR1), ISR_count1, RISING); // Increase counter 1 when speed sensor pin goes High
    attachInterrupt (digitalPinToInterrupt (MOTOR2), ISR_count2, RISING); // Increase counter 2 when speed sensor pin goes High
    Timer1.attachInterrupt( ISR_timerone ); // Enable the timer
}

void loop（）
{
    //Nothing in the loop!
    //You can place code here
}

PID算法

根据反馈进行控制

1.误差比例控制 : $K_p$增大，加快系统响应

$err(t) = goal - rpm(t);$

$pwm(t) = K_p × err(t);$

2.微分阻尼项 : $K_d = K_p × T_d$

$pwm(t) = K_p × (err(t) + T_d × \frac{d(err(t))}{dt}$

3.积分消除静态误差：$K_i = K_p / T_i$

$pwm(t) = K_p × (err(t) + \frac{1}{T_i}\int ^t_0err(\tau)d\tau + T_d × \frac{d(err(t))}{dt})$

位移式PID

【连续时间】$pwm(t) = K_p × err(t) + K_i\int ^t_0err(\tau)d\tau + K_d × \frac{d(err(t))}{dt}$

【离散时间】$pwm(k) = K_p × err(k) + {K_i} × \sum ^k_0err(i) + K_d × (err(k) - err(k-1)$

适用：执行机构不带积分部件的对象

缺点：误差积累大

增量式PID

【连续时间】$dpwm(t) = K_p × \frac{d(err(t))}{dt}+ K_i × err(t) + K_d × \frac{d^2(err(t))}{dt^2}$

【离散时间】$dpwm(k) = K_p × (err(k)-err(k-1)) + K_i × err(k) + K_d × [(err(k) - err(k-1))- (err(k-1)-err(k-2)]$

适用：执行机构带积分部件的对象 eg.步进电机

缺点：积分截断大、有静态误差

接线

电池正极连接L298N12V供电
电池负极连接L298N供电GND
电机M+连接L298NOUT1
电机M-连接L298NOUT2
L298NIN1连接UNO10
L298NIN2连接UNO11
L298NENA连接UNO6
L298N5V连接UNO5V
L298N供电GND连接UNO GND
电机VCC接UNO 5V
电机GND接JNO GND
电机编码器A端接UNO端口2
电机编码器B端接UNO端口3

float err = 0,derr = 0,dderr = 0;
float Kp,Ki,Kd;
unsigned Long curtime,oldtime;
void loop()
{
　　curtime=millis();/返回当前时间，单位为ms
　　int dt abs(curtime = oldtime);
　　if(dt > 50)
　　{
　　　　oldtime = millis();
　　　　rpm = count / period * 60 / edges_per_cycle / reduction ratio;
　　　　count = 0;
　　　　Serial.print("  rpm:");/打印
　　　　Serial.println(rpm);
　　　　pwm += PID(50, rpm);


　　　　if(abs(pwm)>255)/判断PWM是否超过最大值
　　　　if(pwm<0)//控制电机正反转…
　　　　else
　　　　analogwrite(PWM,pwm);

　　}

}

 

int PID(fLoat goal,fLoat now)

{

　　dderr = goal-now - err-derr;
　　derr = goal - now-err;
　　err = goal - now;
　　float dPWM = Ki * (err) + Kp * (derr) + Kd * (dderr);
　　return dPWM;

}