学习记录：自平衡莱洛三角形v1（原理，代码）学习记录（二）

 

来源：

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()中的操作！

        晚安！！