教程一 带编码器电机控制例程

各模块使用教程

• 序章 安装Arduino IDE
• 教程一 带编码器电机控制例程
• 教程二 无编码器电机控制例程
• 教程三 ST3215总线舵机控制例程
• 教程四 PWM舵机控制例程
• 教程五 IMU数据读取例程
• 教程六 SD卡读取例程
• 教程七 INA219电压电流监测例程
• 教程八 OLED屏幕控制例程
• 教程九 激光雷达和在ROS2中发布雷达话题
• General Driver for Robots 主页

带编码器电机

带编码器的电机可以获得反馈，从而达成对速度的闭环控制，以下提供读取编码器电机速度的例程。

例程

上传程序

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

在Arduino IDE中，点击“工具”→“开发板”→“ESP32”→“ESP32 Dev Module”，开发板以及端口都选择好后上传程序。上传程序后，将有编码器电机和驱动板上的电机接口PH2.0 6P连接上，将XH2.54供电接口接上电源后运行程序，再打开Arduino IDE的串口监视器即可读取左右电机的速度。

程序解析

#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);
}

资料

• 例程下载