本项目是在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和每一个帮助过任何人的人
