硬件:
- 电机:2804直流无刷电机
- 编码器:AS5600
- 驱动器:simplefoc mini板
- MCU:esp32 lolin d32
- 陀螺仪: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