• 回复
  • 收藏
  • 点赞
  • 分享
  • 发新帖

【 DigiKey DIY原创大赛】自平衡莱罗三角形(四)- 程序结构及逻辑

本项目是在Arduino2.3.3下开发的,需要安装esp32_package_1.0.6.exe库环境。还需要安装电机的驱动代码库我使用的是Arduino-FOC-2.2.1“SimpleFOC.h”。

莱罗三角形涉及到的技术比较复杂。主要有电机的速度控制、整机的平衡控制PID算法、卡尔曼滤波 "Kalman.h"等等。

电机的控制FOC(Field-Oriented Control)是一种用于电机控制的方法,广泛应用于现代交流电机驱动系统中。流程包括初始化系统:包括硬件初始化、变量初始化等。读取传感器数据:从编码器或其他传感器读取转子位置信息。计算转子位置:通过读取的传感器数据计算出转子的实际位置。计算d-q轴电流:将三相电流转换为d-q轴系下的电流。计算转矩和磁通:根据d-q轴电流计算电机的电磁转矩和磁通。计算电压指令:根据FOC算法计算出所需的电压指令。生成PWM信号:将电压指令转化为PWM信号。输出PWM信号:将生成的PWM信号发送给逆变器,以驱动电机。循环等待下一个周期:等待下一个控制周期开始,重复上述过程。 

对电机的操作代码如下

//连接motor对象与传感器对象
  motor.linkSensor(&sensor);
  //供电电压设置 [V]
  driver.voltage_power_supply = 12;
  driver.init();
  //连接电机和driver对象
  motor.linkDriver(&driver);
  //FOC模型选择
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  //运动控制模式设置
  motor.controller = MotionControlType::torque;
    //速度PI环设置
  motor.PID_velocity.P = v_p_1;
  motor.PID_velocity.I = v_i_1;
  //最大电机限制电机
  motor.voltage_limit = 12;
  //速度低通滤波时间常数
  motor.LPF_velocity.Tf = 0.01;
  //设置最大速度限制
  motor.velocity_limit = 40;
  motor.useMonitoring(Serial);
读取MPU6050数据
while(i2cRead(0x3B, i2cData, 14));
    accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
    accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
    accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
//    tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
    gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
    gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
    gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
    timer = micros();
double pitch = acc2rotation(accX, accY);
double gyroZrate = gyroZ / 131.0; // Convert to deg/s
    kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);
    gyroZangle += (gyroZrate + gyroZ_OFF) * dt;
    compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;
angle当前角度与期望角度差值,在差值大的时候进行摇摆,差值小的时候LQR控制电机保持平衡
if(test_flag == 0)//正常控制
{
if(abs(pendulum_angle) < swing_up_angle) // if angle small enough stabilize 0.5~30°,1.5~90°
{
     target_velocity = controllerLQR(pendulum_angle, gyroZrate, motor.shaft_velocity);
if(abs(target_velocity) > 120)
        target_velocity = _sign(target_velocity) * 120;
motor.controller = MotionControlType::velocity;
motor.move(target_velocity);
}
else // else do swing-up
{    // sets swing_up_voltage to the motor in order to swing up
motor.controller = MotionControlType::torque;
          target_voltage = -_sign(gyroZrate) * swing_up_voltage;
motor.move(target_voltage);
}
}
使用esp32串口实现下载程序,下载软件是flash_download_tool_3.9.2

选择正确串口就可以下载。

整机成品入下图

总结:

经历这个项目,开始了无钢网焊接之旅。上手了ESP32,学习了FOC,AS5600,MPU6050,EG2133,卡尔曼滤波,PID算法等等等等。

致谢!

感谢电源网和DigiKey

感谢45roll和每一个帮助过任何人的人

全部回复(0)
正序查看
倒序查看
现在还没有回复呢,说说你的想法