学习记录:自平衡莱洛三角形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()中的操作!

        晚安!!

  • 5
    点赞
  • 19
    收藏
    觉得还不错? 一键收藏
  • 3
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值