Tutorial III: Motor With Encoder Control Demo 3

Modules Usage Tutorial

Motor With Encoder Control Demo 3

This tutorial integrates the functionalities of Demo 1 and Demo 2, aiming for closed-loop control of motor speed and inputting the target rotation speed.

Demo

Upload Demo

After downloading the motorCtrl.ino, use the USB cable to connect the multifunctional driver board and the computer (here inserted into the USB Type-C port of the multifunctional driver board), click on "Tools" → "Ports", and then click on the newly appeared COM port.

In Arduino IDE, click "Tools" → "Development Board" → "ESP32" → "ESP32 Dev Module", select the development board and the port and then upload the demo. After uploading the demo, connect the motor interface PH2.0 2P on the motor driver board to the motor without an encoder. Connect the XH2.54 power port to the power supply. Upon doing this, you will observe the motor rapidly rotating in the positive direction for 3 seconds, then slowly rotating in the opposite direction for 3 seconds, followed by a pause of 3 seconds, all in a continuous loop.

Demo Analysis

// --- --- --- Encoder Part --- --- ---

// Encoder A pin definition 
const uint16_t AENCA = 35;        // Encoder A input A_C2(B)
const uint16_t AENCB = 34;        // Encoder A input A_C1(A)

// Encoder B pin definition   
const uint16_t BENCB = 16;        // Encoder B input B_C2(B)
const uint16_t BENCA = 27;        // Encoder B input B_C1(A)

//To calculate the number of transitions between high and low levels of a Hall sensor of the encoder within a given "interval" time (in milliseconds)
//If you are using RISING edge detection for initializing interrupts, you'll specifically count the number of transitions from low to high level
volatile long B_wheel_pulse_count = 0;
volatile long A_wheel_pulse_count  = 0;

//To calculate the period for calculating speed, you need to specify the interval at which you want to compute the speed
int interval = 100;

//The current time  
long currentMillis = 0;

// The reduction ratio of the motor means that the motor speed and the output shaft speed of the reduced motor are different.
// For example, with the DCGM3865 motor, with a reduction ratio of 1:42, it means that for every 42 revolutions of the motor, the output shaft completes 1 revolution.
// For each revolution of the output shaft, the motor needs to complete more revolutions as the reduction ratio increases. Typically, higher reduction ratios result in greater torque.
// Take the DCGM3865 motor (reduction ratio: 1：42) as an example:
double reduction_ratio = 42;

//Number of encoder lines, one revolution of the motor, the number of high and low level changes of one Hall sensor of the encoder
int ppr_num = 11;

// When the output shaft completes one revolution, the number of transitions observed in a single Hall sensor
double shaft_ppr = reduction_ratio * ppr_num;

// The callback function of the interrupt function, refer to the attachInterrupt() function later
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--;
  }
}
// --- --- --- --- --- --- --- --- ---

// --- --- --- Motor Part --- --- ---
// The following defines how to control the ESP32 pin of TB6612
// Motor 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

// Motor 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 frequency of pins used for PWM outputs
int freq = 100000;

// Define PWM channel
int channel_A = 5;
int channel_B = 6;

// Defines PWM accuracy, when it is 8, the PWM value is 0-255 (2^8-1)
const uint16_t ANALOG_WRITE_BITS = 8;
// The maximum PWM value 
const uint16_t MAX_PWM = pow(2, ANALOG_WRITE_BITS)-1;
// The minimum PWM, due to the poor low-speed characteristics of DC motors, may not reach the motor's rotation threshold
const uint16_t MIN_PWM = MAX_PWM/5;

// Motor A control 
void channel_A_Ctrl(float pwmInputA){
  // Round the pwmInput value to the nearest integer
  int pwmIntA = round(pwmInputA);

  // If pwmInput is 0, it stops rotating  
  if(pwmIntA == 0){
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, LOW);
    return;
  }

  // Determine the direction of rotation by determining the positive or negative pwmInput value
  if(pwmIntA > 0){
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, HIGH);
    // constrain() function is for limiting the pwmIntA value between MIN_PWM and 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));
  }
}

// Motor B control 
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));
  }
}
// --- --- --- --- --- --- --- --- ---

// --- --- --- Closed-loop Control  --- --- ---
// PID Controller Parameters  
double Kp = 0.05;   // Scale factor
double Ki = 0.05;   // Integral coefficient
double Kd = 0;   // Differentiation factor

// Target rotation speed and the actual rotation speed  
double targetSpeed_A = 100.0;  // Target rotation speed (Adjustable on request)  
double actualSpeed_A = 0.0;   // The actual rotation speed 
double targetSpeed_B = 100.0;  // The target rotation speed (Adjustable on request) 
double actualSpeed_B = 0.0;   // The actual rotation speed 

// PID controller variables 
double previousError_A = 0.0;
double integral_A = 0.0;
double previousError_B = 0.0;
double integral_B = 0.0;
// --- --- --- --- --- --- --- --- ---

void setup(){
  // Set up the working mode of the encoder's related pins 
  pinMode(BENCB , INPUT_PULLUP);
  pinMode(BENCA , INPUT_PULLUP);

  pinMode(AENCB , INPUT_PULLUP);
  pinMode(AENCA , INPUT_PULLUP);
 
  // Set the interrupt and the corresponding callback function to call the B_wheel_pulse function when BEBCB changes from low to high (RISING)
  attachInterrupt(digitalPinToInterrupt(BENCB), B_wheel_pulse, RISING);
  // Set the interrupt and the corresponding callback function to call the A_wheel_pulse function when AEBCB changes from low to high (RISING) 
  attachInterrupt(digitalPinToInterrupt(AENCB), A_wheel_pulse, RISING);

  //Initialize the serial port, and the baud rate is 115200
  Serial.begin(115200);
  // Wait for serial port initialization to complete 
  while(!Serial){}

  // Setting the operating mode of the ESP32 pin used to control the TB6612FNG  
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  // Setting the channel, frequency, and accuracy of the ESP32 pin used for PWM outputs  
  ledcSetup(channel_A, freq, ANALOG_WRITE_BITS);
  ledcAttachPin(PWMA, channel_A);

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

  // The pin used to control rotation is placed at a low level, the motor stops rotating to avoid starting rotation immediately after initialization
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, LOW);
}

void loop(){
  // Calculate the speed of the output shaft of the B-channel motor in revolutions per minute
  actualSpeed_B = (float)((B_wheel_pulse_count / shaft_ppr) * 60 * (1000 / interval));
  B_wheel_pulse_count = 0;

  // Calculate the speed of the output shaft of the A-channel motor in revolutions per minute 
  actualSpeed_A = (float)((A_wheel_pulse_count / shaft_ppr) * 60 * (1000 / interval));
  A_wheel_pulse_count = 0;

  // Calculation error and control volume  
  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;

  // Compute PID output 
  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;
 
  // Limits the output range  
  output_A = constrain(output_A, -MAX_PWM, MAX_PWM);
  output_B = constrain(output_B, -MAX_PWM, MAX_PWM);

  // Output the PWM signal, control the motor rotation speed 
  channel_A_Ctrl(-output_A);
  channel_B_Ctrl(-output_B);

  // Update the last error value
  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);
}

Resource

Demo

Navigation menu