回到首页 返回首页
回到顶部 回到顶部
返回上一页 返回上一页

【Arduino 动手做】使用编码器 AS5600 的自平衡小车 简单

头像 驴友花雕 2025.06.25 50 0

I. 功能简介
日期:2024 年 8 月。
带有减速齿轮的有刷直流电机在齿轮之间不可避免地存在间隙,这可能导致低分辨率编码器(如霍尔效应传感器)的实际速度读数不准确。BLDC 电机、12 位 AS5600 编码器和磁场定向控制 (FOC) 可以解决这个问题。尽管如此,我还是使用级联 PID 到控制回路。
代码、CAD 模型 (STL) 和电路原理图位于 GitHub 上。

II. 使用 FOC 进行速度控制和位置控制
由于编码器 AS5600 和电机是分开的,我需要构建一些东西来将它们放在一起,所以我就开始构建自平衡机器人。

III. 打印和组装

VI. 站起来
它静止不动,因为它卡在一些奇怪的位置。我猜是它后面的 USB 数据线固定着它。
我只是粗略地调整了 PID,它已经具有不错的稳定性。
第二天,我对 PID 进行了多次微调,它比上一个视频中展示的要稳定得多。
带位置控制的稳定性测试。后来我发现位置控制效果不佳,所以我在蓝牙控制上添加了一个开关来启用/禁用位置控制。此视频之后的所有视频都没有位置控制。
静止不动,几乎没有移动。
蓝牙控制测试静止不动
无位置控制的稳定性测试,
我猜这是由于钟摆的动力学特性,它需要倾斜才能加速,但要保持恒定速度,所以它有时会来回摆动。它掉落是因为车轮达到了最大速度,并且无法再加速。此限制可能来自 PID 输出限制和/或高速换向问题

02-.jpg
02-1.jpg
03.jpg
04.jpg
04-.jpg
05.jpg
06.jpg
07.jpg
08.jpg
09.jpg
10.jpg

项目代码

代码
////////////////////////////////////////////////////////////// IMU //////////////////////////////////////////////////////////////
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>
#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter

Adafruit_MPU6050 mpu;

// accel
float ax_float, ay_float, az_float;
float ax_error, ay_error, az_error; // error of linear acceleration

// gyro
float wx, wy, wz; // rad/s
float wx_error, wy_error, wz_error;// rad/s
Kalman kalmanY; // Create the Kalman instances
double p_kf=0;
uint32_t timer_kf;
////////////////////////////////////////////////////////////// IMU //////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////// Encoder //////////////////////////////////////////////////////////////
#include "AS5600.h"

AS5600 as5600_0(&Wire);
AS5600 as5600_1(&Wire1);

////////////////////////////////////////////////////////////// Encoder //////////////////////////////////////////////////////////////

void setup() {
  Serial.begin(460800);
  Serial.println("开始初始化");

  AS5600_setup();
  mpu6050_setup();

  Serial.println("初始化完成");

}

void loop() {
  // 读取AS5600传感器的角度
  float angle0 = float(as5600_0.readAngle())/4096*2*M_PI;
  float angle1 = float(as5600_1.readAngle())/4096*2*M_PI;

  // 输出数据到串口
  Serial.print("Ang 1: ");
  Serial.print(angle0);
  Serial.print("\tAng 2: ");
  Serial.print(angle1);
  read_mpu6050_angle_loop();
  Serial.println();

  delay(4);
}

////////////////////////////////////////////////////////////// Encoder //////////////////////////////////////////////////////////////
void AS5600_setup(){
  Wire.begin(21, 22);    // SDA=21, SCL=22
  Wire1.begin(19, 23);   // SDA=19, SCL=23

  if (!as5600_0.begin()) {
    Serial.println("No 1 AS5600 Found");
    while (1);
  }
  if (!as5600_1.begin()) {
    Serial.println("No 2 AS5600 Found");
    while (1);
  }

}

////////////////////////////////////////////////////////////// Encoder //////////////////////////////////////////////////////////////
////////////////////////////////////////////// IMU //////////////////////////////////////////////
void mpu6050_setup(){ 
  Serial.flush();
  delay(2500);
  mpu6050_start();

  delay(2000);

  read_accel_gyro_raw();
  ax_float-=ax_error;
  ay_float-=ay_error;
  az_float-=az_error;

  double pitch=atan(-1*ax_float/sqrt(sq(ay_float)+sq(az_float)));

  kalmanY.setAngle(pitch); // Set starting angle

  timer_kf = micros();
}

void mpu6050_start(){
  // Try to initialize!
  Serial.println("MPU6050 Start");delay(1000);
  // DO NOT USE: if (!mpu.begin()) { 
  //        USE: ↓↓↓↓↓↓↓↓↓↓↓↓
  if (!mpu.begin(0x68,&Wire)) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }
}

void read_accel_gyro_raw(){
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  ax_float=a.acceleration.x;
  ay_float=a.acceleration.y;
  az_float=a.acceleration.z;

  wx=g.gyro.x;
  wy=g.gyro.y;
  wz=g.gyro.z;
}

void read_mpu6050_angle_loop(){
  read_accel_gyro_raw();
  ax_float-=ax_error;
  ay_float-=ay_error;
  az_float-=az_error;
  wx-=wx_error;
  wy-=wy_error;
  wz-=wz_error;

  double dt = (double)(micros() - timer_kf) / 1000000; // Calculate delta time
  timer_kf = micros();
  
  double pitch=atan(ay_float/sqrt(sq(ax_float)+sq(az_float)))*180/M_PI;
  p_kf = kalmanY.getAngle(pitch, wx*180/M_PI, dt); // pitch
  // Serial.print(-50); // To freeze the lower limit
  // Serial.print(" ");
  // Serial.print(50); // To freeze the upper limit
  // Serial.print(" ");

  // Serial.print("Pitch: ");
  // Serial.print(p_kf); Serial.print("\t");
  Serial.print("p_kf:");Serial.print(p_kf); Serial.print("\t");
}
////////////////////////////////////////////// IMU //////////////////////////////////////////////

00.jpg
01.jpg
02.jpg
00174.gif
00174-.gif
00174--.gif

【Arduino 动手做】使用编码器 AS5600 的自平衡小车
项目链接:https://www.dingminglu-lab.com/personal-project-bldc-self-balance-bot/
项目作者:Dingming Lu 实验室

项目视频 :
https://www.dingminglu-lab.com/wp-content/uploads/2024/08/PP-BLDCBB-51.mp4
https://www.dingminglu-lab.com/wp-content/uploads/2024/08/PP-BLDCBB-53.mp4
项目代码:https://github.com/DML-0205Michael/Brushless-Self-Balance-Bot-FOC
3D 文件:https://github.com/DML-0205Michael/Brushless-Self-Balance-Bot-FOC/tree/main/stl

评论

user-avatar
icon 他的勋章
    展开更多