利用FOC控制实现单摆平衡

硬件：

1. 电机：2804直流无刷电机
2. 编码器：AS5600
3. 驱动器：simplefoc mini板
4. MCU：esp32 lolin d32
5. 陀螺仪：mpu6050

以上硬件可以在M创动工坊买到。

因为要PID调整，所以用手机安装蓝牙串口调试程序，更方便在线调整参数。

直接上代码：

#include "Arduino.h"

#include <MPU6050_tockn.h>//磁罗盘库

#include <Wire.h>//I2C库

#include "SimpleFOC.h"//电机库

#include "BluetoothSerial.h"

 

float Balance_Angle_raw=0;//机械平衡角度

const int leftMotorOffset = 1,rightMotorOffset = 1; //左右轮的启动扭矩值，pm到一定电压马达才开的转动.  kp:5.76   ki:0.00   kd:0.94

int g_turn_apd = 60; //转弯的幅度，通过两轮的差值来实现转向

float ENERGY=6; //前进后退倾角，控制进后退速度

float kp=0.01,ki=0.01,kd=0.01; //根据调试设置kp ki kd的默认值

float angle_kp=2;//根据调试设置kp ki kd的默认值

float Keep_Angle,bias,integrate;//保持角度，角度偏差，偏差积分变量

float AngleX, GyroX, GyroZ; //mpu6050输出的角度值为浮点数，两位有效小数

int vertical_PWM, angle_PWM, PWM,L_PWM,R_PWM;//各种PWM计算值

MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);//初始化电机下的磁传感器

BLDCMotor motor_0 = BLDCMotor(7);//定义直流无刷电机，极对数为7

BLDCDriver3PWM driver_0 = BLDCDriver3PWM(25,32,33,12);//定义电机驱动PWM接的引脚和使能引脚12

 

//target variable

float target_angle = 0;//电机角度/速度/扭矩

char flag = 's';//控制左转右转的标签

 

BluetoothSerial SerialBT;//实例化蓝牙

MPU6050 mpu6050(Wire1);//实例化MPU对象

 

void serial_debug()//蓝牙串口调试函数，在线调试PID值

{

    if(SerialBT.available()>0)

    {

        char DATA = SerialBT.read();

        delay(5);

        switch(DATA)

        {

            case 'u':

              Keep_Angle +=0.1;

              break;

            case 'd':

              Keep_Angle -=0.01;

              break;

            case '0':

              kp -=0.01;

              break;

            case '1':

              kp +=0.01;

              break;

            case '2':

              ki -=0.01;

              break;

            case '3':

              ki +=0.01;

              break;

            case '4':

              kd -=0.01;

              break;

            case '5':

              kd +=0.01;

              break;

            case '6':

              angle_kp -=0.2;

              break;

            case '7':

              angle_kp +=0.2;

              break;

            // case 's':

            // // /*控制程序*/

            // //   flag = 's';

            // //   Keep_Angle = Balance_Angle_raw;

            // //   z_turn_spd = 0;

            // //   break; //调节物理平衡点为机械平衡角度值，原地平衡

            // // case 'a': //前进

            // //   flag = 'a';

            // //   Keep_Angle = Balance_Angle_raw + ENERGY;

            // //   z_turn_spd = 0;

            //   break;

        }

        if(kp<0) kp=0;

        if(ki<0) ki=0;

        if(kd<0) kd=0;

        SerialBT.print("Keep_Angle:");

        SerialBT.println(Keep_Angle);

        SerialBT.print("   kp:");

        SerialBT.print(kp);

        SerialBT.print("   ki:");

        SerialBT.print(ki);

        SerialBT.print("   kd:");

        SerialBT.println(kd);

        SerialBT.println("--------------------");

    }

}

void angle_pwm_clculation(){//转向的PWM

}

void verical_pwm_caculation(){

    AngleX = mpu6050.getAngleX();//陀螺仪获得X方向转动角度

    GyroX = mpu6050.getGyroX();//陀螺仪获得X方向角速度

    bias = AngleX - Keep_Angle;//计算角度偏差，bias为小车角度与结构静态平衡角度的差值

    integrate += bias;//偏差的积分，integrate为全局变量，一直积累

    integrate = constrain(integrate,-1000,1000);//限定误差积分的最大最小值

    vertical_PWM = kp*bias + ki*integrate + kd*GyroX;//得到PID调节后的值

    /*=-直立PID计算PWM--通过陀螺仪返回数据计算，前倾陀螺仪Y轴为正，后仰陀螺仪Y轴为负。前倾车前进，后仰车后退，保持直立。但可能为了直立，车会随时移动。*/

}

void setup() {

    Serial.begin(115200);

    SerialBT.begin("ESP32 car"); // Bluetooth device

    /*----陀螺仪初始化-------*/

    Wire1.begin(19,23,(uint32_t)400000);

    mpu6050.begin();

    mpu6050.calcGyroOffsets(true);

    //磁编码器

    Wire.setClock(400000);

    Wire.begin();//定义磁编码器连接引脚；

    sensor0.init();

    motor_0.linkSensor(&sensor0);

/*----电机初始化-----*/

    driver_0.voltage_power_supply = 12;

    driver_0.voltage_limit = 6;

    driver_0.init();

    motor_0.phase_resistance = 2.9; // 2.9 Ohms

    motor_0.torque_controller = TorqueControlType::voltage;

    motor_0.linkDriver(&driver_0);

    motor_0.foc_modulation = FOCModulationType::SpaceVectorPWM;

    motor_0.controller = MotionControlType::torque;//闭环扭矩控制

    motor_0.PID_velocity.P = 0.108;//速度P值，这个值不能填太大，否则容易抖动

    motor_0.PID_velocity.I = 0.01;

  // the lower the less filtered

    motor_0.LPF_velocity.Tf = 0.01f;  

  // angle P controller

    motor_0.P_angle.P = 30;  //位置PID的P值

    motor_0.velocity_limit = 30;

    motor_0.useMonitoring(Serial);

    motor_0.init();

    motor_0.initFOC();

 

/*--------------------------------------------------------*/

    Serial.begin(115200);

    Serial.println("Motor ready!");

    Keep_Angle = Balance_Angle_raw; //平衡度初始化为结构静态平衡时的陀螺仪角度。Keep Angle可以改变

    // motor(0,0);//机器启动时马达确保停止。

    delay(1000);

    Serial.println("BLE:ESP32 Car!");

}

void loop() {

    //串口PID调试

    serial_debug();

    //陀螺仪刷新

    mpu6050.update();

    //PWM计算

    // angle_pwm_clculation();

    verical_pwm_caculation();

    PWM = vertical_PWM;//电机扭矩值等于经PID计算后的值

    motor_0.loopFOC();

    // motor_1.loopFOC();

    if(PWM>0)

        PWM = PWM + leftMotorOffset;//加上电机死区扭矩

    if(PWM<0)

        PWM = PWM - leftMotorOffset;

    if(AngleX>60||AngleX<-60)//倾角过大，车子倒下，停止转动；

        motor_0.move(0);

    PWM = constrain(PWM,-60,60);//此时PWM还是角度值

//    float AngleX0 = mpu6050.getAngleX();

//    Serial.println(AngleX0);

    target_angle = static_cast<float>(PWM)/180*3.14;//把陀螺仪MPU6050获得的角度转换为弧度赋值给电机

    motor_0.move(target_angle);//实际这是扭矩值

    Serial.println(target_angle);

//    delay(5);

}

软件平台：arduino ide，或者 vscode+platform io