本項目是在Arduino2.3.3下開發(fā)的，需要安裝esp32_package_1.0.6.exe庫環(huán)境。還需要安裝電機的驅動代碼庫我使用的是Arduino-FOC-2.2.1“SimpleFOC.h”。
萊羅三角形涉及到的技術比較復雜。主要有電機的速度控制、整機的平衡控制PID算法、卡爾曼濾波 "Kalman.h"等等。
電機的控制FOC（Field-Oriented Control）是一種用于電機控制的方法，廣泛應用于現(xiàn)代交流電機驅動系統(tǒng)中。流程包括初始化系統(tǒng)：包括硬件初始化、變量初始化等。讀取傳感器數據：從編碼器或其他傳感器讀取轉子位置信息。計算轉子位置：通過讀取的傳感器數據計算出轉子的實際位置。計算d-q軸電流：將三相電流轉換為d-q軸系下的電流。計算轉矩和磁通：根據d-q軸電流計算電機的電磁轉矩和磁通。計算電壓指令：根據FOC算法計算出所需的電壓指令。生成PWM信號：將電壓指令轉化為PWM信號。輸出PWM信號：將生成的PWM信號發(fā)送給逆變器，以驅動電機。循環(huán)等待下一個周期：等待下一個控制周期開始，重復上述過程。
對電機的操作代碼如下
//連接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環(huán)設置
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串口實現(xiàn)下載程序，下載軟件是flash_download_tool_3.9.2
選擇正確串口就可以下載。
整機成品入下圖
以下為視頻演示：
總結：
經歷這個項目，開始了無鋼網焊接之旅。上手了ESP32，學習了FOC，AS5600，MPU6050，EG2133，卡爾曼濾波，PID算法等等等等。
致謝！
感謝電源網和DigiKey
感謝45roll和每一個幫助過任何人的人
