docs 03 带编码器电机控制例程三

本教程是基于例程一和例程二的理解上，集成了前面两个例程的功能，用于闭环控制电机速度，输入目标转速。

上传程序

点击例程下载打开 speedLoopCtrl.ino，用 USB 线将多功能驱动板和计算机连接起来（此处插入的是多功能驱动板 USB 的 Type-C 接口），点击“工具”→“端口”，再点击新出现的 COM（这里新出现的 COM 为 COM28）。

Arduino IDE 端口选择

在 Arduino IDE 中，点击“工具”→“开发板”→“ESP32”→“ESP32 Dev Module”。开发板以及端口都选择好后上传程序。上传程序后，将无编码器电机和驱动板上的电机接口 PH2.0 2P 连接上，将 XH2.54 供电接口接上电源后，可以看见电机开始快速正方向转动 3s、慢速反方向转动 3s 再停转 3s 的循环。

程序解析

  // A 路编码器的引脚定义
const uint16_t AENCA = 35;        // Encoder A input A_C2(B)
const uint16_t AENCB = 34;        // Encoder A input A_C1(A)

// B 路编码器的引脚定义
const uint16_t BENCB = 16;        // Encoder B input B_C2(B)
const uint16_t BENCA = 27;        // Encoder B input B_C1(A)

// 用于计算在“interval”时间（单位 ms）内的编码器的某一个霍尔传感器的高低电平变化次数
// 因为后面初始化中断时使用的是 RISING，所以具体来说是低电平变高电平的次数
volatile long B_wheel_pulse_count = 0;
volatile long A_wheel_pulse_count  = 0;

// 计算速度的周期时间，每隔这么多 ms 机算一次速度
int interval = 100;

// 当前的时间
long currentMillis = 0;

// 电机的减速比，减速电机的电机转速和输出轴的转速是不一样的
// 例如 DCGM3865 这款电机，减速比是 1:42，意味着电机转动 42 圈，输出轴转动 1 圈
// 对应输出轴的一圈，电机需要转的圈数越多，减速比越大，通常扭矩越大
// 一下以 DCGM3865 电机（减速比 1:42）为例
double reduction_ratio = 42;

// 编码器线数，电机转动一圈，编码器的一个霍尔传感器的高低电平变化次数
int ppr_num = 11;

// 输出轴转动一圈，编码器的一个霍尔传感器的高低电平变化次数
double shaft_ppr = reduction_ratio * ppr_num;

// 中断函数的回调函数，参考后面的 attachInterrupt() 函数
void IRAM_ATTR B_wheel_pulse() {
  if(digitalRead(BENCA)){
    B_wheel_pulse_count++;
  }
  else{
    B_wheel_pulse_count--;
  }
}

void IRAM_ATTR A_wheel_pulse() {
  if(digitalRead(AENCA)){
    A_wheel_pulse_count++;
  }
  else{
    A_wheel_pulse_count--;
  }
}
// --- --- --- --- --- --- --- --- ---

// --- --- --- 电机控制部分 --- --- ---
// 以下定义了用于控制 TB6612 的 ESP32 引脚
// A 路电机
const uint16_t PWMA = 25;         // Motor A PWM control  Orange
const uint16_t AIN2 = 17;         // Motor A input 2      Brown
const uint16_t AIN1 = 21;         // Motor A input 1      Green

// B 路电机
const uint16_t BIN1 = 22;        // Motor B input 1       Yellow
const uint16_t BIN2 = 23;        // Motor B input 2       Purple
const uint16_t PWMB = 26;        // Motor B PWM control   White

// 用于 PWM 输出的引脚的 PWM 频率
int freq = 100000;

// 定义 PWM 通道
int channel_A = 5;
int channel_B = 6;

// 定义 PWM 的精度，精度为 8 时，PWM 数值在 0-255（2^8-1）
const uint16_t ANALOG_WRITE_BITS = 8;
// 最大的 PWM 数值
const uint16_t MAX_PWM = pow(2, ANALOG_WRITE_BITS)-1;
// 最小的 PWM 数值，由于直流电机的低速特性一般比较差，过低的 PWM 达不到电机的转动阈值
const uint16_t MIN_PWM = MAX_PWM/5;

// A 路电机控制
void channel_A_Ctrl(float pwmInputA){
  // 将 pwmInput 值取整
  int pwmIntA = round(pwmInputA);

  // 如果 pwmInput 是 0 则停止转动
  if(pwmIntA == 0){
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, LOW);
    return;
  }

  // 判断 pwmInput 值的正负来确定旋转方向
  if(pwmIntA > 0){
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, HIGH);
    // constrain() 函数用于将 pwmIntA 的值限制在 MIN_PWM 与 MAX_PWM 之间
    ledcWrite(channel_A, constrain(pwmIntA, MIN_PWM, MAX_PWM));
  }
  else{
    digitalWrite(AIN1, HIGH);
    digitalWrite(AIN2, LOW);
    ledcWrite(channel_A,-constrain(pwmIntA, -MAX_PWM, 0));
  }
}

// B 路电机控制
void channel_B_Ctrl(float pwmInputB){
  int pwmIntB = round(pwmInputB);
  if(pwmIntB == 0){
    digitalWrite(BIN1, LOW);
    digitalWrite(BIN2, LOW);
    return;
  }

  if(pwmIntB > 0){
    digitalWrite(BIN1, LOW);
    digitalWrite(BIN2, HIGH);
    ledcWrite(channel_B, constrain(pwmIntB, 0, MAX_PWM));
  }
  else{
    digitalWrite(BIN1, HIGH);
    digitalWrite(BIN2, LOW);
    ledcWrite(channel_B,-constrain(pwmIntB, -MAX_PWM, 0));
  }
}
// --- --- --- --- --- --- --- --- ---

// --- --- --- 闭环控制部分 --- --- ---
// PID 控制器参数
double Kp = 0.05;   // 比例系数
double Ki = 0.05;   // 积分系数
double Kd = 0;   // 微分系数

// 目标转速和实际转速
double targetSpeed_A = 100.0;  // 目标转速（可根据需求调整）
double actualSpeed_A = 0.0;   // 实际转速
double targetSpeed_B = 100.0;  // 目标转速（可根据需求调整）
double actualSpeed_B = 0.0;   // 实际转速

// PID 控制器变量
double previousError_A = 0.0;
double integral_A = 0.0;
double previousError_B = 0.0;
double integral_B = 0.0;
// --- --- --- --- --- --- --- --- ---

void setup(){
  // 设置编码器相关引脚的工作模式
  pinMode(BENCB , INPUT_PULLUP);
  pinMode(BENCA , INPUT_PULLUP);

  pinMode(AENCB , INPUT_PULLUP);
  pinMode(AENCA , INPUT_PULLUP);
 
  // 设置中断和对应的回调函数，当 BEBCB 由低电平变高电平时（RISING），调用 B_wheel_pulse 函数。
  attachInterrupt(digitalPinToInterrupt(BENCB), B_wheel_pulse, RISING);
  // 设置中断和对应的回调函数，当 AEBCB 由低电平变高电平时（RISING），调用 A_wheel_pulse 函数。
  attachInterrupt(digitalPinToInterrupt(AENCB), A_wheel_pulse, RISING);

  // 初始化串口，波特率 115200
  Serial.begin(115200);
  // 等待串口初始化完成
  while(!Serial){}

  // 设置用于控制 TB6612FNG 的 ESP32 引脚的工作模式
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  // 设置用于 PWM 输出的 ESP32 引脚的通道、频率和精度
  ledcSetup(channel_A, freq, ANALOG_WRITE_BITS);
  ledcAttachPin(PWMA, channel_A);

  ledcSetup(channel_B, freq, ANALOG_WRITE_BITS);
  ledcAttachPin(PWMB, channel_B);

  // 用于控制转动的引脚置于低电平，电机停止转动，避免初始化后立即开始转动
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, LOW);
}

void loop(){
  // 计算 B 通道电机输出轴的转速，单位为 转/分钟
  actualSpeed_B = (float)((B_wheel_pulse_count / shaft_ppr) * 60 * (1000 / interval));
  B_wheel_pulse_count = 0;

  // 计算 A 通道电机输出轴的转速，单位为 转/分钟
  actualSpeed_A = (float)((A_wheel_pulse_count / shaft_ppr) * 60 * (1000 / interval));
  A_wheel_pulse_count = 0;

  // 计算误差和控制量
  double error_A = targetSpeed_A - actualSpeed_A;
  integral_A += error_A;
  double derivative_A = error_A - previousError_A;

  double error_B = targetSpeed_B - actualSpeed_B;
  integral_B += error_B;
  double derivative_B = error_B - previousError_B;

  // 计算 PID 输出
  double output_A = Kp * error_A + Ki * integral_A + Kd * derivative_A;
  double output_B = Kp * error_B + Ki * integral_B + Kd * derivative_B;

  // output_A += Kp * error_A;
  // output_B += Kp * error_B;
 
  // 限制输出范围
  output_A = constrain(output_A, -MAX_PWM, MAX_PWM);
  output_B = constrain(output_B, -MAX_PWM, MAX_PWM);

  // 输出 PWM 信号，控制电机转速
  channel_A_Ctrl(-output_A);
  channel_B_Ctrl(-output_B);

  // 更新上一个误差值
  previousError_A = error_A;
  previousError_B = error_B;

  Serial.print("RPM_A: ");Serial.print(actualSpeed_A);Serial.print("   RPM_B: ");Serial.println(actualSpeed_B);
  Serial.println("--- --- ---");

  delay(interval);
}

文档反馈

上传程序​

程序解析​

上传程序

程序解析