查看: 15|回复: 2

[项目] 【Arduino 动手做】使用编码器 AS5600 的自平衡小车

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

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

III. 打印和组装

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

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

使用道具举报

精彩评论(2)

驴友花雕 中级技神
楼主|

发表于 5 小时前

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

项目代码

////////////////////////////////////////////////////////////// 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 //////////////////////////////////////////////
复制代码