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

来源：

v1版本:：【开源】百元内可自制的自平衡莱洛三角形_哔哩哔哩_bilibili

v2版本： 【开源】自平衡莱洛三角形V2 | 我加了RGB性能翻倍_哔哩哔哩_bilibili

原作者开源链接：https://gitee.com/coll45/foc

说明：第一次接触Arduino和ESP32

参考文件：

灯哥开源FOC：【发布】不足百元！双路无刷电机驱动器 -灯哥双路无刷FOC驱动板正式开源！支持四足机器人！_哔哩哔哩_bilibili

simpleFOC库

KalmanFilter

FOC算法入门：FOC算法入门_梦如南伐的博客-CSDN博客_foc

项目使用卡尔曼滤波、SimpleFOC库完成操作

与计算角度相关的参数定义

Kalman kalmanZ;       //创建Kalman实例
/* ----IMU Data---- */
double accX, accY, accZ;  //存储x、y、z轴加速度值
double gyroX, gyroY, gyroZ;   //存储x、y、z轴陀螺仪数据
int16_t tempRaw;            //使用加速度计算倾斜角度

double gyroZangle; //使用陀螺仪计算角度
double compAngleZ; //使用互补滤波器计算角度 
double kalAngleZ;  //使用卡尔曼滤波器计算角度

uint32_t timer;    //暂时存储时间

定义了I2C数据缓冲区

uint8_t i2cData[14]; // Buffer for I2C dataI2C  数据缓冲区 

因为需要使用两个I2C接口，定义第二个I2C接口

TwoWire I2Ctwo = TwoWire(1);


/*
 *参考
 * TwoWire Wire = TwoWire(0);
 * TwoWire Wire1 = TwoWire(1);
 */

关于simpleFOC库的定义

        查看了simpleFOC库的原文件，其中应该支持两种传感器进行检测。分别是AS5600和AS5048。文件目录：Arduino-FOC-master\src\sensors。是直接在giehub下载的包。

MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
                    //在simpleFOC库中，定义了AS5600的I2C传输格式

LowPassFilter lpf_throttle{0.00}; //在simpleFOC库中定义的低通滤波器时间常数

BLDCMotor motor = BLDCMotor(5); //simpleFOC中，定义电机极对数

BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);     
                    //在simpleFOC库中定义的pwm pin

其他参数定义

//倒立摆参数
float LQR_K3_1 = 10;   //摇摆到平衡
float LQR_K3_2 = 1.7;   //
float LQR_K3_3 = 1.75; //

float LQR_K4_1 = 2.4;   //摇摆到平衡
float LQR_K4_2 = 1.5;   //
float LQR_K4_3 = 1.42; //

float target_velocity = 0;    //目标速度
float target_angle = 89.3;    //目标角度
float target_voltage = 0;     //目标电压

float swing_up_voltage = 1.8;  //上摆电压
float swing_up_angle = 20;     //上摆角度

//速度PI环参数
float v_i_1 = 20;    //摆动
float v_p_1 = 0.5;
float v_i_2 = 10;    //稳定
float v_p_2 = 0.2;

WiFi调参相关函数

        函数定义

Command comm; 
bool Motor_enable_flag = 0;
int test_flag = 0;

//EEPROM.writeFloat:写入数据操作
//comm.scalar:把cmd转换成浮点函数，并存储到&地址中！

void do_TA(char* cmd) { comm.scalar(&target_angle, cmd);
                            EEPROM.writeFloat(0,target_angle); }
void do_SV(char* cmd) { comm.scalar(&swing_up_voltage, cmd); 
                            EEPROM.writeFloat(4, swing_up_voltage); }
void do_SA(char* cmd) { comm.scalar(&swing_up_angle, cmd);
                            EEPROM.writeFloat(8, swing_up_angle); }
void do_START(char* cmd) {  wifi_flag = !wifi_flag; }   //wifi标志位翻转
void do_MOTOR(char* cmd) {  
  if(Motor_enable_flag) motor.enable();
  else motor.disable();
  Motor_enable_flag = !Motor_enable_flag; }
void do_TVQ(char* cmd) {
  if(test_flag == 1) test_flag = 0;
  else test_flag = 1; }
void do_TVV(char* cmd){
  if(test_flag == 2) test_flag = 0;
  else test_flag = 2;}
void do_VV(char* cmd) { comm.scalar(&target_velocity, cmd); }
void do_VQ(char* cmd) { comm.scalar(&target_voltage, cmd); }
void do_vp1(char* cmd) { comm.scalar(&v_p_1, cmd); EEPROM.writeFloat(12, v_p_1);}
void do_vi1(char* cmd) { comm.scalar(&v_i_1, cmd);EEPROM.writeFloat(16, v_i_1); }
void do_vp2(char* cmd) { comm.scalar(&v_p_2, cmd); EEPROM.writeFloat(20, v_p_2);}
void do_vi2(char* cmd) { comm.scalar(&v_i_2, cmd);EEPROM.writeFloat(24, v_i_2); }
void do_tv(char* cmd) { comm.scalar(&target_velocity, cmd); }
void do_K31(char* cmd) { comm.scalar(&LQR_K3_1, cmd); }
void do_K32(char* cmd) { comm.scalar(&LQR_K3_2, cmd); }
void do_K33(char* cmd) { comm.scalar(&LQR_K3_3, cmd); }
void do_K41(char* cmd) { comm.scalar(&LQR_K4_1, cmd); }
void do_K42(char* cmd) { comm.scalar(&LQR_K4_2, cmd); }
void do_K43(char* cmd) { comm.scalar(&LQR_K4_3, cmd); }

        函数运用方式

        在Command.cpp文件中，可看到函数编成组，调用等函数

        ①编译成组

void Command::add(char* id, CommandCallback onCommand)
{
  call_list[call_count] = onCommand;  //函数名
  call_ids[call_count] = id;          //参数
  call_count++;
}

        ②运行函数

void Command::run(char* str)
{
  for(int i=0; i < call_count; i++)
  {
        if(isSentinel(call_ids[i],str))
        {  
          // case :   call_ids = "T2"   str = "T215.15" 
          call_list[i](str+strlen(call_ids[i]));  // get 15.15  input function 
          //这里直接是  函数名（参数）  形式  调用函数！！！这个方法值得学习！！！
          break;
        }
   }
}




void Command::scalar(float* value,  char* user_cmd)
{
  *value = atof(user_cmd);    //atof()把字符串转换成浮点数
}



bool Command::isSentinel(char* ch,char* str)
{
  char s[strlen(ch)+1];
  strncpy(s,str,strlen(ch));
  s[strlen(ch)] = '\0';      //strncpy need add end '\0'     
  if(strcmp(ch, s) == 0)
      return true;
  else 
      return false;

        在主函数 setup() 函数中

        ①一次添加函数到command.app中的组中

 //命令设置
  comm.add("TA",do_TA);
  comm.add("START",do_START);
  comm.add("MOTOR",do_MOTOR);
  comm.add("SV",do_SV);
  comm.add("SA",do_SA);
  comm.add("TVQ",do_TVQ);
  comm.add("TVV",do_TVV);
  comm.add("VV",do_VV);
  comm.add("VQ",do_VQ);
  
  //速度环参数
  comm.add("VP1",do_vp1);
  comm.add("VI1",do_vi1);
  comm.add("VP2",do_vp2);
  comm.add("VI2",do_vi2);
  comm.add("TV",do_tv);
  comm.add("K31",do_K31);
  comm.add("K32",do_K32);
  comm.add("K33",do_K33);
  comm.add("K41",do_K41);
  comm.add("K42",do_K42);
  comm.add("K43",do_K43);

        ②在main文件中，通过void onPacketCallBack(AsyncUDPPacket packet)函数采集wifi传入数据，调用command.app中的void Command::run(char* str)函数，进而调用定义函数进行调参。