Tutorial I: Motor With Encoder Control Demo

Modules Usage Tutorial

Motor With Encoder

A motor with an encoder can obtain feedback for closed-loop speed control. The following provides a demo for reading the speed of the encoder motor.

Demo

Upload Demo

After downloading the zip package, open speedget.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 encoder motor and the motor interface PH2.0 6P on the driver board, connect the XH2.54 power supply interface to the power supply and run the demo, and then open the serial monitor of Arduino IDE to read the speed of the left and right motors.

Demo Analysis

#define ENC_COUNT_REV 555

float wheel_radius = 0.0529;

double rpm_right = 0;
double rpm_left = 0;

int interval = 100;

double ang_velocity_right = 0;
double ang_velocity_right_deg = 0;
double linear_velocity_right = 0;


double ang_velocity_left = 0;
double ang_velocity_left_deg = 0;
double linear_velocity_left = 0;

volatile long right_wheel_pulse_count = 0;
volatile long left_wheel_pulse_count = 0;

bool Direction_right = true;
bool Direction_left = true;

const float rpm_to_radians = 0.10471975512;
const float rad_to_deg = 57.29578;

const uint16_t PWMA = 25;        
const uint16_t AIN2 = 17;         
const uint16_t AIN1 = 21;         
const uint16_t BIN1 = 22;       
const uint16_t BIN2 = 23;       
const uint16_t PWMB = 26;

const uint16_t AENCA = 35;       
const uint16_t AENCB = 34;

const uint16_t BENCB = 16;       
const uint16_t BENCA = 27;     

const uint16_t ANALOG_WRITE_BITS = 8;

int freq = 100000;
int channel_A = 0;
int channel_B = 1;
int resolution = ANALOG_WRITE_BITS;

void IRAM_ATTR right_wheel_pulse() {
  Direction_right = digitalRead(BENCA);
  if(Direction_right){
    right_wheel_pulse_count++;
  }
  else{
    right_wheel_pulse_count--;
  }
}

void IRAM_ATTR left_wheel_pulse() {
  Direction_left = digitalRead(AENCA);
  if(Direction_left){
    left_wheel_pulse_count++;
  }
  else{
    left_wheel_pulse_count--;
  }
}

void pinInit(){
  // Set pin states of the encoder
  pinMode(BENCB , INPUT_PULLUP);
  pinMode(BENCA , INPUT);

  pinMode(AENCB , INPUT_PULLUP);
  pinMode(AENCA , INPUT);
 

  attachInterrupt(digitalPinToInterrupt(BENCB), right_wheel_pulse, RISING);
  attachInterrupt(digitalPinToInterrupt(AENCB), left_wheel_pulse, RISING);


  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  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 right_speed_calculate() {
  rpm_right = (float)(right_wheel_pulse_count * 60 * (1000/interval) / ENC_COUNT_REV);
  ang_velocity_right = rpm_right * rpm_to_radians;
  ang_velocity_right_deg = ang_velocity_right * rad_to_deg;
  linear_velocity_right = ang_velocity_right * wheel_radius;
  right_wheel_pulse_count = 0;
}


void left_speed_calculate() {
  rpm_left = (float)(left_wheel_pulse_count * 60 * (1000/interval) / ENC_COUNT_REV);
  ang_velocity_left = rpm_left * rpm_to_radians;
  ang_velocity_left_deg = ang_velocity_left * rad_to_deg;
  linear_velocity_left = ang_velocity_left * wheel_radius;
  left_wheel_pulse_count = 0;
}
void speed(){
  Serial.print("Leftspeed:");
  Serial.println(linear_velocity_left);
  Serial.print("Rightspeed:");
  Serial.println(linear_velocity_right);
}

void setup(){
  Serial.begin(115200);
  pinInit();
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
}
  
void loop(){
  ledcWrite(channel_A,500);
  ledcWrite(channel_B,100);
  right_speed_calculate();
  left_speed_calculate();
  speed();
  delay(1000);
}

Resources

Demo

Navigation menu