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

喜暖知寒

已于 2022-04-19 18:21:10 修改

阅读量3.5k

点赞数 10

分类专栏：自平衡莱洛三角形文章标签：单片机

于 2022-04-19 18:20:07 首次发布

本文链接：https://blog.csdn.net/qq_41650023/article/details/124278635

版权

自平衡莱洛三角形专栏收录该内容

6 篇文章 30 订阅

订阅专栏

来源：

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

分析loop函数问题

由几个部分组成，读数据、算数据、写数据、写FOC控制、写wifi参数。

理解起来不是很难，主要和硬件电路结合来说就这么几个功能。软件实现比较简单，参考开源FOC和基于灯哥的开源FOC控制的倒立摆项目就可以理解FOC的控制。kalman基本上就是给mpu6050处理数据用的，这一点可参考CH32读取MPU6050姿态数据(卡尔曼滤波法)_哔哩哔哩_bilibili

motor.loopFOC();
  if (1)
  {
    //读MPU6050的数据吧
    //============================================================================== 
    while (i2cRead(0x3B, i2cData, 14));      
    //读14位正好读完所需要值
        /*
         * mpu6050寄存器地址
         
       0X3B  //加速度值,X轴高8位寄存器
       0X3C  //加速度值,X轴低8位寄存器
       0X3D  //加速度值,Y轴高8位寄存器
       0X3E  //加速度值,Y轴低8位寄存器
       0X3F  //加速度值,Z轴高8位寄存器
       0X40  //加速度值,Z轴低8位寄存器
    
       0X41  //温度值高八位寄存器
       0X42  //温度值低8位寄存器
    
       0X43  //陀螺仪值,X轴高8位寄存器
       0X44  //陀螺仪值,X轴低8位寄存器
       0X45  //陀螺仪值,Y轴高8位寄存器
       0X46  //陀螺仪值,Y轴低8位寄存器
       0X47  //陀螺仪值,Z轴高8位寄存器
       0X48  //陀螺仪值,Z轴低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]);
    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; //计算增量时间，微秒算秒
    timer = micros();
    
    double pitch = acc2rotation(accX, accY);    // mpu6050加速度转换为角度函数
    double gyroZrate = gyroZ / 131.0;           //   转换为速度
    
    if(abs(pitch-last_pitch)>100)     //abs()：绝对值函数
      kalmanZ.setAngle(pitch);      //用于设置角度，应将其设置为起始角度
 
    kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);
                        //角度应以度为单位，速率应以度/秒为单位，增量时间应以秒为单位 
    
    last_pitch = pitch;
    
    gyroZangle += (gyroZrate + gyroZ_OFF) * dt;       //使用陀螺仪计算角度
    compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;                 
                                                    //使用互补滤波器计算角度 

                //当陀螺仪角度漂移过大时，重置陀螺仪角度
    if (gyroZangle < -180 || gyroZangle > 180)
      gyroZangle = kalAngleZ;     //kalAngleZ:用卡尔曼滤波器计算角度
      
    float pendulum_angle = constrainAngle(fmod(kalAngleZ,120)-target_angle);
                                    // 将角度限制在-60~60之间的函数
    //============================================================================== 
    //控制部分
    //==============================================================================
            //pendulum_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) > 140)
          target_velocity = _sign(target_velocity) * 140;   //目标速度
          
        //设置要使用的运动控制循环 
        motor.controller = MotionControlType::velocity;

        //运动控制功能 
        motor.move(target_velocity);
      }
      else // else do swing-up
      {    
        //设置电机的上摆电压，以便上摆
        motor.controller = MotionControlType::torque;
        target_voltage = -_sign(gyroZrate) * swing_up_voltage;  //目标速度
        motor.move(target_voltage);
      }
    }
    else if(test_flag == 1)
    {
       motor.controller = MotionControlType::torque;
       motor.move(target_voltage);
    }
    else
    {
       motor.controller = MotionControlType::velocity;
       motor.move(target_velocity);
    }
     //==============================================================================
//串口输出数据部分，不需要的情况可以改为0
#if 1

Serial.print(pitch);Serial.print("\t");
Serial.print(kalAngleZ);Serial.print("\t");
Serial.print(target_voltage);Serial.print("\t");
Serial.print(motor.shaft_velocity);Serial.print("\t");
Serial.print(motor.voltage.q);Serial.print("\t");
Serial.print(target_angle);Serial.print("\t");
Serial.print(pendulum_angle);Serial.print("\t");
Serial.print(gyroZrate);Serial.print("\t");
Serial.print("\r\n");
#endif

     //wifi传输数据
     //==============================================================================
    //可以使用该方法wifi发送udp信息
    if(wifi_flag)
    {
      //语言初始化函数，将buf中的当前位置后strlen(buf)个字节用0替代并返回buf
      memset(buf, 0, strlen(buf));  
      //调用wifi_print()函数
      wifi_print("v", motor.shaft_velocity);//旋转方向
      wifi_print("vq",motor.voltage.q);     //当前其中一项的期望电压
      wifi_print("p",pendulum_angle);       //当前期望角度与真实角度差值
      wifi_print("t",target_angle);         //期望角度，应该是固定不变的
      wifi_print("k",kalAngleZ);            //卡尔曼滤波后的当前角度
      wifi_print("g",gyroZrate);            //当前左右加速度
      
      //广播数据
      udp.writeTo((const unsigned char*)buf, strlen(buf), IPAddress(192,168,4,2), 
                                                            localUdpPort); 
      }
  }
}

倾斜角度计算

MPU6050加速度转换为角度，MPU6050可以计算两个方向的角度，但在本项目中，只需要一个方向的角度即可。参考有关MPU6050的例程来看，有两个函数只需用其中一个函数。

/* mpu6050加速度转换为角度
            两个方向的角度：
            acc2rotation(ax, ay)
            acc2rotation(az, ay) */
//只需要一个方向角度
double acc2rotation(double x, double y)
{
  double tmp_kalAngleZ = (atan(x / y) / 1.570796 * 90);   //atan():反正切函数
  if (y < 0)
  {
    return (tmp_kalAngleZ + 180);
  }
  else if (x < 0)
  {
    //将当前值与前值比较，当前差值大于100则认为异常
    if (!isnan(kalAngleZ) && (tmp_kalAngleZ + 360 - kalAngleZ) > 100) 
    {
      //Serial.print("X<0"); Serial.print("\t");
      //Serial.print(tmp_kalAngleZ); Serial.print("\t");
      //Serial.print(kalAngleZ); Serial.print("\t");
      //Serial.print("\r\n");
      if (tmp_kalAngleZ < 0 && kalAngleZ < 0) //按键右边角
        return tmp_kalAngleZ;
      else  //按键边异常处理
        return tmp_kalAngleZ;
    } 
    else
      return (tmp_kalAngleZ + 360);
  }
  else
  {
    return tmp_kalAngleZ;
  }
}

限制角度-60~60之间函数

// 将角度限制在-60~60之间的函数 
float constrainAngle(float x)
{
  float a = 0;
  if(x < 0)
  {
    a = 120+x;
    if(a<abs(x))    //abs():求整数的绝对值
      return a;
  }
  return x;
}

LQR稳定控制器

这一部分LQR控制不太明白。没学过现代控制相关的问题。等学学相关知识再更新吧！！！

//LQR稳定控制器功能
//计算稳定摆锤所需的电机电压 
float controllerLQR(float p_angle, float p_vel, float m_vel)
{
  if (abs(p_angle) > 5) //摆角大于5则进入非稳态，记录非稳态时间
  {
    last_unstable_time = millis();
    if (stable) //如果是稳态进入非稳态则调整为目标角度
    {
      //target_angle = EEPROM.readFloat(0) - p_angle;
      target_angle = EEPROM.readFloat(0);
      stable = 0;
    }
  }
  if ((millis() - last_unstable_time) > 1000 && !stable)  
                    //非稳态进入稳态超过500ms检测，更新目标角为目标角+摆角，假设进入稳态
  {
    //target_angle  -= _sign(target_velocity) * 0.4;
    target_angle = target_angle+p_angle;
    stable = 1;
  }

  if ((millis() - last_stable_time) > 2500 && stable) 
  { //稳态超过2000ms检测，更新目标角
    if (abs(target_velocity) > 5 ) 
    { 
      //稳态速度偏大校正
      last_stable_time = millis();
      target_angle  -= _sign(target_velocity) * 0.2;
    }
  }

  //Serial.println(stable);
  float u;

  if (!stable)  //非稳态计算
  {
    motor.PID_velocity.P = v_p_1;
    motor.PID_velocity.I = v_i_1;
    u = LQR_K3_1 * p_angle + LQR_K3_2 * p_vel + LQR_K3_3 * m_vel;
  }
  else
  {
    motor.PID_velocity.P = v_p_2;
    motor.PID_velocity.I = v_i_2;
    u = LQR_K4_1 * p_angle + LQR_K4_2 * p_vel + LQR_K4_3 * m_vel;
  }
  return u;
}

wifi输出函数

在loop函数中调用的输出函数！！



void wifi_print(char * s,double num)
{
  char str[255];
  char n[255];
  sprintf(n, "%.2f",num);     //用于将输出的字符存储到n中，格式化输出函数
  strcpy(str,s);      //复制字符串
  strcat(str, n);     //附加字符串
  strcat(buf+strlen(buf), str);   //strlen()取字符串长度
  strcat(buf, ",\0");     //附加字符串

}

至此，分析完了我接触的第一个Arduino相关的ESP32的项目，也是第一次接触C++和Arduino。希望能够多看、多分析、多思考一些项目的实现方法。本项目中，值得学习的知识点有无线调参函数。这一点和正点原子的STM32的串口调试代码有类似之处，值得在以后项目中应用。以及项目中的FOC、Kalman、LQR等都需要找些资料继续学习一下。OK软件部分都已经分析到这个程度啦~~！以后就用CSDN来记录研究项目的笔记吧，省的以后找不到了。哈哈哈~开心！

硬件电路分析

项目地址：

自平衡的莱洛三角_esp32_可充电_10*10版本 - 立创EDA开源硬件平台

作者分的很清楚，硬件电路非常简单。

上位机软件

Python的GUI是SimpleFOC的SimpleFOCStudio。

SimpleFOCStudio的下载可参考：SimpleFOCStudio安装使用说明及PID调试_loop222的博客-CSDN博客_pid调试软件

最后

基本上本项目通过参考SimpleFOC、SimpleFOCStudio以及MPU6050等关键部分就可以理解原理。

继续学习呀~~

加油学习~

喜暖知寒

关注

10
点赞
踩
73

收藏

觉得还不错? 一键收藏
17
评论
学习记录：自平衡莱洛三角形v1（原理，代码）学习记录（三）（完）

来源：v1版本:：【开源】百元内可自制的自平衡莱洛三角形_哔哩哔哩_bilibiliv2版本：【开源】自平衡莱洛三角形V2 | 我加了RGB性能翻倍_哔哩哔哩_bilibili原作者开源链接：https://gitee.com/coll45/foc说明：第一次接触Arduino和ESP32参考文件：灯哥开源FOC：【发布】不足百元！双路无刷电机驱动器 -灯哥双路无刷FOC驱动板正式开源！支持四足机器人！_哔哩哔哩_bilibilisimpleFOC库Kalman...
复制链接

扫一扫