ARDUINO ,TB6612 ,MG310 电机，PID调速

 ARDUINO ,TB6612 ,MG310 电机，PID调速

#include <PID_v1.h>

int AIN1 = 9;
int AIN2 = 8;
int PWMA = 3;
int STBY = 10;

int motor_A = 21;  // 编码器连接的引脚
int motor_B = 20;  // 编码器连接的引脚
volatile int count = 0;  // 编码器计数

void count_A() {
     //单频计数实现
  //手动旋转电机一圈，输出结果为 一圈脉冲数 * 减速比
  /*if(digitalRead(motor_A) == HIGH){

    if(digitalRead(motor_B) == LOW){//A 高 B 低
      count++;  
    } else {//A 高 B 高
      count--;  
    }


  }*/

  //2倍频计数实现
  //手动旋转电机一圈，输出结果为 一圈脉冲数 * 减速比 * 2
  if(digitalRead(motor_A) == HIGH){

    if(digitalRead(motor_B) == HIGH){//A 高 B 高
      count++;  
    } else {//A 高 B 低
      count--;  
    }


  } else {

    if(digitalRead(motor_B) == LOW){//A 低 B 低
      count++;  
    } else {//A 低 B 高
      count--;  
    }  

  }

}

void count_B() {
    if(digitalRead(motor_B) == HIGH){

    if(digitalRead(motor_A) == LOW){//B 高 A 低
      count++;
    } else {//B 高 A 高
      count--;
    }


  } else {

    if(digitalRead(motor_A) == HIGH){//B 低 A 高
      count++;
    } else {//B 低 A 低
      count--;
    }

  }

}

int reducation = 20;  // 减速比
int pulse = 260;  // 编码器旋转一圈产生的脉冲数
int per_round = pulse * reducation * 4;  // 车轮旋转一圈产生的脉冲数
long start_time = millis();  // 一个计算周期的开始时刻，初始值为 millis()
long interval_time = 50;  // 一个计算周期 50ms
double current_vel;

// 获取当前转速的函数
void get_current_vel() {
    long right_now = millis();
    long past_time = right_now - start_time;
    if (past_time >= interval_time) {
        // 禁止中断
        noInterrupts();
        // 计算转速
        current_vel = (double)count / per_round / past_time * 1000 * 60;
        // 清零计数器
        count = 0;
        // 重置开始时间
        start_time = right_now;
        // 重启中断
        interrupts();

        Serial.println(current_vel);
    }
}

// 创建 PID 对象
// 1.当前转速 2.计算输出的pwm 3.目标转速 4.kp 5.ki 6.kd 7.当输入与目标值出现偏差时，向哪个方向控制
double pwm;  // 电机驱动的PWM值
double target = 40;
double kp = 1, ki = 3.0, kd = 0.1;
PID pid(&current_vel, &pwm, &target, kp, ki, kd, DIRECT);

// 速度更新函数
void update_vel() {
    // 获取当前速度
    get_current_vel();
    // 计算需要输出的PWM
    pid.Compute();
    // 激活电机驱动
    digitalWrite(STBY, HIGH);
    // 根据pwm的正负值设置电机转向和PWM
    if (pwm >= 0) {
        digitalWrite(AIN1, HIGH);
        digitalWrite(AIN2, LOW);
        analogWrite(PWMA, pwm);
    } else {
        digitalWrite(AIN1, LOW);
        digitalWrite(AIN2, HIGH);
        analogWrite(PWMA, -pwm); // 取pwm的绝对值
    }
}


void setup() {
    Serial.begin(57600);  // 设置波特率
    pinMode(motor_A, INPUT);
    pinMode(motor_B, INPUT);
    // 设置电机驱动引脚为输出
    pinMode(AIN1, OUTPUT);
    pinMode(AIN2, OUTPUT);
    pinMode(PWMA, OUTPUT);
    pinMode(STBY, OUTPUT);

    // 设置编码器中断
    attachInterrupt(digitalPinToInterrupt(motor_A), count_A, CHANGE);
    attachInterrupt(digitalPinToInterrupt(motor_B), count_B, CHANGE);

    pid.SetMode(AUTOMATIC);
}

void loop() {
    delay(10);
    update_vel();
}