来源:
v1版本::【开源】百元内可自制的自平衡莱洛三角形_哔哩哔哩_bilibili
v2版本: 【开源】自平衡莱洛三角形V2 | 我加了RGB性能翻倍_哔哩哔哩_bilibili
原作者开源链接:https://gitee.com/coll45/foc
说明:第一次接触Arduino和ESP32
参考文件:
灯哥开源FOC:【发布】不足百元!双路无刷电机驱动器 -灯哥双路无刷FOC驱动板正式开源!支持四足机器人!_哔哩哔哩_bilibili
simpleFOC库
KalmanFilter
FOC算法入门:FOC算法入门_梦如南伐的博客-CSDN博客_foc
爱转的光凌 :CH32读取MPU6050姿态数据(卡尔曼滤波法)_哔哩哔哩_bilibili
分析setup中的操作
①相关写参数到EEPROM和读参数
void setup()
{
Serial.begin(115200); //设置串行数据通讯波特率
if (!EEPROM.begin(1000)) //申请ram内存空间并从flash中读取相应数据到内存 (4~4096)
{
Serial.println("Failed to initialise EEPROM"); //写入字符串数据+换行到串口
Serial.println("Restarting...");
delay(1000);
ESP.restart(); //esp重启函数
}
// eeprom 读取,判断是否有数据,没有就填入初始数据
int k,j;
j = 0;
for(k=0;k<=24;k=k+4)
{
float nan = EEPROM.readFloat(k); //读取数据操作
if(isnan(nan)) //用于检查其参数是否为数值,是数值返回false,不是返回true
//不是数值的情况下写入数值
{
j = 1;
Serial.println("frist write");
//EEPROM.write(address,value) 往内存控件写入数据,
// address:写入地址位置、value:写入的数据
//只是写到申请的内存空间
//EEPROM.cpmmit() 该功能用于把内存控件的数据覆盖到flash eeprom块去
//真正写回到flash空间
//各种参数====================================================
EEPROM.writeFloat(0, target_angle); delay(10);EEPROM.commit();
EEPROM.writeFloat(4, swing_up_voltage); delay(10);EEPROM.commit();
EEPROM.writeFloat(8, swing_up_angle); delay(10);EEPROM.commit();
EEPROM.writeFloat(12, v_p_1); delay(10);EEPROM.commit();
EEPROM.writeFloat(16, v_i_1); delay(10);EEPROM.commit();
EEPROM.writeFloat(20, v_p_2); delay(10);EEPROM.commit();
EEPROM.writeFloat(24, v_i_2); delay(10);EEPROM.commit();
}
}
if(j == 0) //判断到不是数值的情况下 置j = 1 ,
//而是数值的情况下,说明有数值存储了,故可以读取存储参数。
{
//读取数据操作
target_angle = EEPROM.readFloat(0);
swing_up_voltage = EEPROM.readFloat(4);
swing_up_angle = EEPROM.readFloat(8);
v_p_1 = EEPROM.readFloat(12);
v_i_1 = EEPROM.readFloat(16);
v_p_2 = EEPROM.readFloat(20);
v_i_2 = EEPROM.readFloat(24);
motor.PID_velocity.P = v_p_1; //写的是摇摆的PID参数
motor.PID_velocity.I = v_i_1;
}
②IIC相关参数设置(MPU6050)
//相关mpu6050的定义,地址是写的mpu6050的地址,配置mpu6050
Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz //初始化IIC连接,并加入IIC总线
//设置端口18、19为wire的线,MPU6050,SDA-19,SCL-18
//==============================================================================
//定义了uint8_t i2cDate[14]作为数据缓存区
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering,
//256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
//===============================================================================
//i2c中定义的i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop)
//和 i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length,
bool sendStop)
//还有 i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes)
//是有返回值的
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
//0X19:采样频率分频器
while (i2cWrite(0x6B, 0x01, true));
// PLL with X axis gyroscope reference and disable sleep mode
//0X6B:电源管理寄存器1
while (i2cRead(0x75, i2cData, 1)); //0X75:器件ID寄存器
if (i2cData[0] != 0x68)
{
// Read "WHO_AM_I" register
Serial.print(F("Error reading sensor")); //F()宏告诉编译器将此特定数组保留在PROGMEM中
while (1);
}
delay(100); // Wait for sensor to stabilize
③设置卡尔曼滤波问题
while (i2cRead(0x3B, i2cData, 6)); //0X3B:加速度值,X轴高8位寄存器
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
//mpu6050加速度转换为角度
double pitch = acc2rotation(accX, accY); //自定义的函数
//用于设置角度,应将其设置为起始角度
kalmanZ.setAngle(pitch);
gyroZangle = pitch; //使用陀螺仪计算的角度
timer = micros(); //此函数返回自程序启动后的微秒数(无符号长整型)
Serial.println("kalman mpu6050 init"); //串行口打印
④wifi、udp相关设置
WiFi.mode(WIFI_AP); //设置为接入点模式AP
while(!WiFi.softAP(ssid, password)){}; //启动AP,函数获取并打印软AP的IP
Serial.println("AP启动成功");
while (!udp.listen(localUdpPort)); //等待udp监听设置成功
udp.onPacket(onPacketCallBack); //注册收到数据包事件
⑤设置和simpleFOC库相关的函数
代码可参考《灯哥开源FOC》、simpleFOC库的例程
I2Ctwo.begin(23, 5, 400000); //SDA,SCL AS5600
//其实和wile.begin基本一样,但是两个接口这样!
//一开始已经定义了I2Ctwo!
sensor.init(&I2Ctwo);
//连接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::velocity;
//速度PI环设置
motor.PID_velocity.P = v_p_1;
motor.PID_velocity.I = v_i_1;
//最大电机限制电压
motor.voltage_limit = 12;
//速度低通滤波时间常数
motor.LPF_velocity.Tf = 0.02;
//设置最大速度限制
motor.velocity_limit = 40;
motor.useMonitoring(Serial);
//初始化电机
motor.init();
//初始化 FOC
motor.initFOC();
运行用户命令来配置电机(在docs.simplefoc.com中找到完整的命令列表)
Serial.println(F("Motor ready."));
Serial.println(F("Set the target velocity using serial terminal:"));
就这样吧!大概setup的文件就是这样了,明天再研究loop()中的操作!
晚安!!