电压控制改为速度控制,已经完成自平衡

master
zrg 2021-10-18 22:51:50 +08:00
parent 3f8700eee3
commit 4d6025b854
3 changed files with 75 additions and 41 deletions

View File

@ -13,7 +13,6 @@ Deng's FOC 闭环速度控制例程 测试库SimpleFOC 2.1.1 测试硬件:
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter #include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
Kalman kalmanZ; Kalman kalmanZ;
#define gyroZ_OFF -0.19 #define gyroZ_OFF -0.19
#define swing_up_voltage 5 //V
#define balance_voltage 10 //V #define balance_voltage 10 //V
/* ----IMU Data---- */ /* ----IMU Data---- */
@ -49,16 +48,19 @@ float PID_D = 0; //
TwoWire I2Ctwo = TwoWire(1); TwoWire I2Ctwo = TwoWire(1);
PIDController angle_pid = PIDController(PID_P, PID_I, PID_D, balance_voltage * 0.7, 20000); PIDController angle_pid = PIDController(PID_P, PID_I, PID_D, balance_voltage * 0.7, 20000);
LowPassFilter lpf_throttle{0.00}; LowPassFilter lpf_throttle{0.00};
#define FLAG_V 1 #define FLAG_V 0
//倒立摆参数 //倒立摆参数
float LQR_K1 = 4; //摇摆到平衡 float LQR_K1_1 = 4; //摇摆到平衡
float LQR_K2 = 1.5; // float LQR_K1_2 = 1.5; //
float LQR_K3 = 0.30; // float LQR_K1_3 = 0.30; //
float LQR_K1_1 = 3.49; //平衡态 float LQR_K2_1 = 3.49; //平衡态
float LQR_K2_1 = 0.26; // float LQR_K2_2 = 0.26; //
float LQR_K3_1 = 0.15; // float LQR_K2_3 = 0.15; //
float LQR_K3_1 = 5.25; //平衡态
float LQR_K3_2 = 3.18; //
float LQR_K3_3 = 1.86; //
//电机参数 //电机参数
BLDCMotor motor = BLDCMotor(5); BLDCMotor motor = BLDCMotor(5);
@ -66,18 +68,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
float target_velocity = 0; float target_velocity = 0;
float target_angle = 90; float target_angle = 90;
float target_voltage = 0; float target_voltage = 0;
float swing_up_voltage = 2;
//命令设置 //命令设置
Command comm; Command comm;
bool Motor_enable_flag = 0; bool Motor_enable_flag = 0;
void do_K11(char* cmd) { comm.scalar(&LQR_K1, cmd); }
void do_K12(char* cmd) { comm.scalar(&LQR_K2, cmd); }
void do_K13(char* cmd) { comm.scalar(&LQR_K3, cmd); }
void do_K21(char* cmd) { comm.scalar(&LQR_K1_1, cmd); }
void do_K22(char* cmd) { comm.scalar(&LQR_K2_1, cmd); }
void do_K23(char* cmd) { comm.scalar(&LQR_K3_1, cmd); }
void do_TA(char* cmd) { comm.scalar(&target_angle, cmd); } void do_TA(char* cmd) { comm.scalar(&target_angle, cmd); }
void do_START(char* cmd) { wifi_flag = 1; } void do_START(char* cmd) { wifi_flag = !wifi_flag; }
void do_MOTOR(char* cmd) void do_MOTOR(char* cmd)
{ {
if(Motor_enable_flag) if(Motor_enable_flag)
@ -86,6 +82,24 @@ void do_MOTOR(char* cmd)
digitalWrite(22,LOW); digitalWrite(22,LOW);
Motor_enable_flag = !Motor_enable_flag; Motor_enable_flag = !Motor_enable_flag;
} }
void do_SW(char* cmd) { comm.scalar(&swing_up_voltage, cmd); }
#if FLAG_V
void do_K11(char* cmd) { comm.scalar(&LQR_K1_1, cmd); }
void do_K12(char* cmd) { comm.scalar(&LQR_K1_2, cmd); }
void do_K13(char* cmd) { comm.scalar(&LQR_K1_3, cmd); }
void do_K21(char* cmd) { comm.scalar(&LQR_K2_1, cmd); }
void do_K22(char* cmd) { comm.scalar(&LQR_K2_2, cmd); }
void do_K23(char* cmd) { comm.scalar(&LQR_K2_3, cmd); }
#else
void do_vp(char* cmd) { comm.scalar(&motor.PID_velocity.P, cmd); }
void do_vi(char* cmd) { comm.scalar(&motor.PID_velocity.I, cmd); }
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); }
#endif
void onPacketCallBack(AsyncUDPPacket packet) void onPacketCallBack(AsyncUDPPacket packet)
{ {
char* da; char* da;
@ -99,15 +113,25 @@ void onPacketCallBack(AsyncUDPPacket packet)
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
//命令设置 //命令设置
comm.add("TA",do_TA);
comm.add("START",do_START);
comm.add("MOTOR",do_MOTOR);
comm.add("SW",do_SW);
#if FLAG_V
comm.add("K11",do_K11); comm.add("K11",do_K11);
comm.add("K12",do_K12); comm.add("K12",do_K12);
comm.add("K13",do_K13); comm.add("K13",do_K13);
comm.add("K21",do_K21); comm.add("K21",do_K21);
comm.add("K22",do_K22); comm.add("K22",do_K22);
comm.add("K23",do_K23); comm.add("K23",do_K23);
comm.add("TA",do_TA); #else
comm.add("START",do_START); comm.add("VP",do_vp);
comm.add("MOTOR",do_MOTOR); comm.add("VI",do_vi);
comm.add("TV",do_tv);
comm.add("K31",do_K31);
comm.add("K32",do_K32);
comm.add("K33",do_K33);
#endif
// kalman mpu6050 init // kalman mpu6050 init
Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
@ -175,8 +199,8 @@ void setup() {
#else #else
motor.controller = MotionControlType::velocity; motor.controller = MotionControlType::velocity;
//速度PI环设置 //速度PI环设置
motor.PID_velocity.P = 20; motor.PID_velocity.P = 0.5;
motor.PID_velocity.I = 20; motor.PID_velocity.I = 10;
#endif #endif
@ -203,8 +227,6 @@ void setup() {
} }
char buf[255]; char buf[255];
int t_v;
int lim_v = 60;
long loop_count = 0; long loop_count = 0;
void loop() { void loop() {
motor.loopFOC(); motor.loopFOC();
@ -238,7 +260,7 @@ void loop() {
float pendulum_angle = constrainAngle(fmod(kalAngleZ,120)-target_angle); float pendulum_angle = constrainAngle(fmod(kalAngleZ,120)-target_angle);
// float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - target_angle) / 57.29578); // float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - target_angle) / 57.29578);
#if FLAG_V #if FLAG_V
if (abs(pendulum_angle) < 18) // if angle small enough stabilize 0.5~30°,1.5~90° if (abs(pendulum_angle) < 12) // if angle small enough stabilize 0.5~30°,1.5~90°
{ {
target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate, motor.shaftVelocity()); target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate, motor.shaftVelocity());
// limit the voltage set to the motor // limit the voltage set to the motor
@ -247,7 +269,7 @@ void loop() {
} }
else // else do swing-up else // else do swing-up
{ // sets 1.5V to the motor in order to swing up { // sets 1.5V to the motor in order to swing up
target_voltage = -_sign(gyroZrate) * 2; target_voltage = -_sign(gyroZrate) * 1.5;
} }
// set the target voltage to the motor // set the target voltage to the motor
@ -261,7 +283,21 @@ void loop() {
} }
#else #else
motor.move(0); if (abs(pendulum_angle) < 18) // if angle small enough stabilize 0.5~30°,1.5~90°
{
target_velocity = LQR_K3_1*pendulum_angle+LQR_K3_2*gyroZrate+LQR_K3_3*motor.shaftVelocity();
if (abs(target_velocity) > 140)
target_velocity = _sign(target_velocity) * 140;
motor.controller = MotionControlType::velocity;
motor.move(target_velocity);
}
else // else do swing-up
{ // sets 1.5V to the motor in order to swing up
motor.controller = MotionControlType::torque;
target_voltage = -_sign(gyroZrate) * swing_up_voltage;
motor.move(target_voltage);
}
#endif #endif
#if 0 #if 0
@ -281,16 +317,14 @@ Serial.print(kalAngleZ);Serial.print("\t");
if(wifi_flag) if(wifi_flag)
{ {
memset(buf, 0, strlen(buf)); memset(buf, 0, strlen(buf));
wifi_print("v", motor.shaftVelocity()); wifi_print("v", motor.shaftVelocity());
wifi_print("vq",motor.voltage.q);
wifi_print("p",pendulum_angle); wifi_print("p",pendulum_angle);
wifi_print("t",target_angle); wifi_print("t",target_angle);
wifi_print("k",kalAngleZ); wifi_print("k",kalAngleZ);
wifi_print("g",gyroZrate); wifi_print("g",gyroZrate);
// IPAddress broadcastAddr("192.168.4.255")
//IPAddress broadcastAddr(((uint32_t)"192.168.4.2")); //计算广播地址
// IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址
// Serial.println(buf);
//const char * udpAddress = "192.168.4.255";
udp.writeTo((const unsigned char*)buf, strlen(buf), IPAddress(192,168,4,2), localUdpPort); //广播数据 udp.writeTo((const unsigned char*)buf, strlen(buf), IPAddress(192,168,4,2), localUdpPort); //广播数据
} }
} }
@ -350,12 +384,12 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
float u; float u;
if (!stable) if (!stable)
{ {
u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel; u = LQR_K1_1 * p_angle + LQR_K1_2 * p_vel + LQR_K1_3 * m_vel;
} }
else else
{ {
//u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel; //u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel;
u = LQR_K1_1 * p_angle + LQR_K2_1 * p_vel + LQR_K3_1 * m_vel; u = LQR_K2_1 * p_angle + LQR_K2_2 * p_vel + LQR_K2_3 * m_vel;
} }

View File

@ -2,7 +2,7 @@
# Form implementation generated from reading ui file 'main_ui.ui' # Form implementation generated from reading ui file 'main_ui.ui'
# #
# Created by: PyQt5 UI code generator 5.15.2 # Created by: PyQt5 UI code generator 5.15.4
# #
# WARNING: Any manual changes made to this file will be lost when pyuic5 is # WARNING: Any manual changes made to this file will be lost when pyuic5 is
# run again. Do not edit this file unless you know what you are doing. # run again. Do not edit this file unless you know what you are doing.
@ -222,18 +222,18 @@ class Ui_MainWindow(object):
self.left_lineEdit_1.setText(_translate("MainWindow", "0")) self.left_lineEdit_1.setText(_translate("MainWindow", "0"))
self.right_lineEdit_1.setText(_translate("MainWindow", "15")) self.right_lineEdit_1.setText(_translate("MainWindow", "15"))
self.label.setText(_translate("MainWindow", "command:")) self.label.setText(_translate("MainWindow", "command:"))
self.command_lineEdit_1.setText(_translate("MainWindow", "K11")) self.command_lineEdit_1.setText(_translate("MainWindow", "K31"))
self.label_4.setText(_translate("MainWindow", "num:")) self.label_4.setText(_translate("MainWindow", "num:"))
self.num_lineEdit_1.setText(_translate("MainWindow", "0")) self.num_lineEdit_1.setText(_translate("MainWindow", "0"))
self.left_lineEdit_2.setText(_translate("MainWindow", "0")) self.left_lineEdit_2.setText(_translate("MainWindow", "0"))
self.right_lineEdit_2.setText(_translate("MainWindow", "3")) self.right_lineEdit_2.setText(_translate("MainWindow", "3"))
self.label_5.setText(_translate("MainWindow", "command:")) self.label_5.setText(_translate("MainWindow", "command:"))
self.command_lineEdit_2.setText(_translate("MainWindow", "K12")) self.command_lineEdit_2.setText(_translate("MainWindow", "K32"))
self.label_6.setText(_translate("MainWindow", "num:")) self.label_6.setText(_translate("MainWindow", "num:"))
self.num_lineEdit_2.setText(_translate("MainWindow", "0")) self.num_lineEdit_2.setText(_translate("MainWindow", "0"))
self.left_lineEdit_3.setText(_translate("MainWindow", "0")) self.left_lineEdit_3.setText(_translate("MainWindow", "0"))
self.right_lineEdit_3.setText(_translate("MainWindow", "1.5")) self.right_lineEdit_3.setText(_translate("MainWindow", "1.5"))
self.label_7.setText(_translate("MainWindow", "command:")) self.label_7.setText(_translate("MainWindow", "command:"))
self.command_lineEdit_3.setText(_translate("MainWindow", "K13")) self.command_lineEdit_3.setText(_translate("MainWindow", "K33"))
self.label_8.setText(_translate("MainWindow", "num:")) self.label_8.setText(_translate("MainWindow", "num:"))
self.num_lineEdit_3.setText(_translate("MainWindow", "0")) self.num_lineEdit_3.setText(_translate("MainWindow", "0"))

View File

@ -211,7 +211,7 @@
<item> <item>
<widget class="QLineEdit" name="command_lineEdit_1"> <widget class="QLineEdit" name="command_lineEdit_1">
<property name="text"> <property name="text">
<string>K11</string> <string>K31</string>
</property> </property>
</widget> </widget>
</item> </item>
@ -284,7 +284,7 @@
<item> <item>
<widget class="QLineEdit" name="command_lineEdit_2"> <widget class="QLineEdit" name="command_lineEdit_2">
<property name="text"> <property name="text">
<string>K12</string> <string>K32</string>
</property> </property>
</widget> </widget>
</item> </item>
@ -357,7 +357,7 @@
<item> <item>
<widget class="QLineEdit" name="command_lineEdit_3"> <widget class="QLineEdit" name="command_lineEdit_3">
<property name="text"> <property name="text">
<string>K13</string> <string>K33</string>
</property> </property>
</widget> </widget>
</item> </item>