zcy
/
triangle
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完善

master
45coll 2021-10-27 15:46:59 +08:00
parent 4d6025b854
commit cde585d47e
8 changed files with 72 additions and 19417 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
arduino/main/main.ino
View File

@ -1,19 +1,22 @@
/** /**
Deng's FOC 闭环速度控制例程 测试库：SimpleFOC 2.1.1 测试硬件：灯哥开源FOC V1.0 arduino开发环境-灯哥开源FOChttps://gitee.com/ream_d/Deng-s-foc-controller，并安装Kalman。
在串口窗口中输入：T+速度，就可以使得两个电机闭环转动 FOC引脚32, 33, 25, 22 22为enable
比如让两个电机都以 10rad/s 的速度转动，则输入：T10 AS5600霍尔传感器 SDA-23 SCL-5 MPU6050六轴传感器 SDA-19 SCL-18
在使用自己的电机时，请一定记得修改默认极对数，即 BLDCMotor(7) 中的值，设置为自己的极对数数字 本程序有两种平衡方式， FLAG_V为1时使用电压控制，为0时候速度控制。电压控制时LQR参数使用K1和K2，速度控制时LQR参数使用K3和K4
程序默认设置的供电电压为 16.8V,用其他电压供电请记得修改 voltage_power_supply , voltage_limit 变量中的值 在wifi上位机窗口中输入：TA+角度，就可以修改平衡角度
默认PID针对的电机是 GB6010 ，使用自己的电机需要修改PID参数，才能实现更好效果 比如让平衡角度为90度，则输入：TA90，并且会存入eeprom的位置0中 注：wifi发送命令不能过快，因为每次都会保存进eeprom
在使用自己的电机时，请一定记得修改默认极对数，即 BLDCMotor(5) 中的值，设置为自己的极对数数字，磁铁数量/2
程序默认设置的供电电压为 12V,用其他电压供电请记得修改 voltage_power_supply , voltage_limit 变量中的值
默认PID针对的电机是 GB2204 ，使用自己的电机需要修改PID参数，才能实现更好效果
*/ */
#include <SimpleFOC.h> #include <SimpleFOC.h>
#include "Command.h" #include "Command.h"
#include <WiFi.h> #include <WiFi.h>
#include <AsyncUDP.h> //引用以使用异步UDP #include <AsyncUDP.h> //引用以使用异步UDP
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter #include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
#include "EEPROM.h"
Kalman kalmanZ; Kalman kalmanZ;
#define gyroZ_OFF -0.19 #define gyroZ_OFF -0.19
#define balance_voltage 10 //V
/* ----IMU Data---- */ /* ----IMU Data---- */
double accX, accY, accZ; double accX, accY, accZ;
@ -42,11 +45,7 @@ unsigned int localUdpPort = 2333; //本地端口号
void wifi_print(char * s,double num); void wifi_print(char * s,double num);
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
float PID_P = 1; //
float PID_I = 0; //
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);
LowPassFilter lpf_throttle{0.00}; LowPassFilter lpf_throttle{0.00};
#define FLAG_V 0 #define FLAG_V 0
//倒立摆参数 //倒立摆参数
@ -58,21 +57,29 @@ float LQR_K2_1 = 3.49; //平衡态
float LQR_K2_2 = 0.26; // float LQR_K2_2 = 0.26; //
float LQR_K2_3 = 0.15; // float LQR_K2_3 = 0.15; //
float LQR_K3_1 = 5.25; //平衡态 float LQR_K3_1 = 5.25; //摇摆到平衡
float LQR_K3_2 = 3.18; // float LQR_K3_2 = 3.18; //
float LQR_K3_3 = 1.86; // float LQR_K3_3 = 1.86; //
float LQR_K4_1 = 2.4; //摇摆到平衡
float LQR_K4_2 = 1.5; //
float LQR_K4_3 = 1.42; //
//电机参数 //电机参数
BLDCMotor motor = BLDCMotor(5); BLDCMotor motor = BLDCMotor(5);
BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22); BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
float target_velocity = 0; float target_velocity = 0;
float target_angle = 90; float target_angle = 89.3;
float target_voltage = 0; float target_voltage = 0;
float swing_up_voltage = 2; float swing_up_voltage = 1.8;
float swing_up_angle = 18;
//命令设置 //命令设置
Command comm; Command comm;
bool Motor_enable_flag = 0; bool Motor_enable_flag = 0;
void do_TA(char* cmd) { comm.scalar(&target_angle, 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; } void do_START(char* cmd) { wifi_flag = !wifi_flag; }
void do_MOTOR(char* cmd) void do_MOTOR(char* cmd)
{ {
@ -82,7 +89,6 @@ 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 #if FLAG_V
void do_K11(char* cmd) { comm.scalar(&LQR_K1_1, cmd); } void do_K11(char* cmd) { comm.scalar(&LQR_K1_1, cmd); }
void do_K12(char* cmd) { comm.scalar(&LQR_K1_2, cmd); } void do_K12(char* cmd) { comm.scalar(&LQR_K1_2, cmd); }
@ -91,12 +97,15 @@ void do_K21(char* cmd) { comm.scalar(&LQR_K2_1, cmd); }
void do_K22(char* cmd) { comm.scalar(&LQR_K2_2, cmd); } void do_K22(char* cmd) { comm.scalar(&LQR_K2_2, cmd); }
void do_K23(char* cmd) { comm.scalar(&LQR_K2_3, cmd); } void do_K23(char* cmd) { comm.scalar(&LQR_K2_3, cmd); }
#else #else
void do_vp(char* cmd) { comm.scalar(&motor.PID_velocity.P, cmd); } void do_vp(char* cmd) { comm.scalar(&motor.PID_velocity.P, cmd); EEPROM.writeFloat(12, motor.PID_velocity.P);}
void do_vi(char* cmd) { comm.scalar(&motor.PID_velocity.I, cmd); } void do_vi(char* cmd) { comm.scalar(&motor.PID_velocity.I, cmd);EEPROM.writeFloat(16, motor.PID_velocity.I); }
void do_tv(char* cmd) { comm.scalar(&target_velocity, cmd); } void do_tv(char* cmd) { comm.scalar(&target_velocity, cmd); }
void do_K31(char* cmd) { comm.scalar(&LQR_K3_1, 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_K32(char* cmd) { comm.scalar(&LQR_K3_2, cmd); }
void do_K33(char* cmd) { comm.scalar(&LQR_K3_3, 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); }
#endif #endif
@ -106,17 +115,28 @@ void onPacketCallBack(AsyncUDPPacket packet)
da= (char*)(packet.data()); da= (char*)(packet.data());
Serial.println(da); Serial.println(da);
comm.run(da); comm.run(da);
EEPROM.commit();
// packet.print("reply data"); // packet.print("reply data");
} }
// instantiate the commander // instantiate the commander
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
if (!EEPROM.begin(1000)) {
Serial.println("Failed to initialise EEPROM");
Serial.println("Restarting...");
delay(1000);
ESP.restart();
}
//命令设置 //命令设置
comm.add("TA",do_TA); comm.add("TA",do_TA);
comm.add("START",do_START); comm.add("START",do_START);
comm.add("MOTOR",do_MOTOR); comm.add("MOTOR",do_MOTOR);
comm.add("SW",do_SW); comm.add("SV",do_SV);
comm.add("SA",do_SA);
target_angle = EEPROM.readFloat(0);
swing_up_voltage = EEPROM.readFloat(4);
swing_up_angle = EEPROM.readFloat(8);
#if FLAG_V #if FLAG_V
comm.add("K11",do_K11); comm.add("K11",do_K11);
comm.add("K12",do_K12); comm.add("K12",do_K12);
@ -131,6 +151,11 @@ void setup() {
comm.add("K31",do_K31); comm.add("K31",do_K31);
comm.add("K32",do_K32); comm.add("K32",do_K32);
comm.add("K33",do_K33); comm.add("K33",do_K33);
comm.add("K41",do_K41);
comm.add("K42",do_K42);
comm.add("K43",do_K43);
motor.PID_velocity.P = EEPROM.readFloat(12);
motor.PID_velocity.I = EEPROM.readFloat(16);
#endif #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
@ -200,7 +225,7 @@ void setup() {
motor.controller = MotionControlType::velocity; motor.controller = MotionControlType::velocity;
//速度PI环设置 //速度PI环设置
motor.PID_velocity.P = 0.5; motor.PID_velocity.P = 0.5;
motor.PID_velocity.I = 10; motor.PID_velocity.I = 20;
#endif #endif
@ -258,18 +283,18 @@ void loop() {
gyroZangle = kalAngleZ; gyroZangle = kalAngleZ;
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); // FLAG_V为1时使用电压控制，为0时候速度控制
#if FLAG_V #if FLAG_V
if (abs(pendulum_angle) < 12) // if angle small enough stabilize 0.5~30°,1.5~90° if (abs(pendulum_angle) < swing_up_angle) // if angle small enough stabilize 0.5~30°,1.5~90°
{ {
target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate, motor.shaftVelocity()); target_voltage = controllerLQR(pendulum_angle, gyroZrate, motor.shaftVelocity());
// limit the voltage set to the motor // limit the voltage set to the motor
if (abs(target_voltage) > motor.voltage_limit * 0.7) if (abs(target_voltage) > motor.voltage_limit * 0.7)
target_voltage = _sign(target_voltage) * motor.voltage_limit * 0.7; target_voltage = _sign(target_voltage) * motor.voltage_limit * 0.7;
} }
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) * 1.5; target_voltage = -_sign(gyroZrate) * swing_up_voltage;
} }
// set the target voltage to the motor // set the target voltage to the motor
@ -283,9 +308,9 @@ void loop() {
} }
#else #else
if (abs(pendulum_angle) < 18) // if angle small enough stabilize 0.5~30°,1.5~90° if (abs(pendulum_angle) < swing_up_angle) // 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(); target_velocity = controllerLQR(pendulum_angle, gyroZrate, motor.shaftVelocity());
if (abs(target_velocity) > 140) if (abs(target_velocity) > 140)
target_velocity = _sign(target_velocity) * 140; target_velocity = _sign(target_velocity) * 140;
motor.controller = MotionControlType::velocity; motor.controller = MotionControlType::velocity;
@ -348,7 +373,7 @@ double acc2rotation(double x, double y)
} }
} }
// function constraining the angle in between -pi and pi, in degrees -180 and 180 // function constraining the angle in between -60~60
float constrainAngle(float x) float constrainAngle(float x)
{ {
float a = 0; float a = 0;
@ -370,7 +395,7 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
// - k = [40, 7, 0.3] // - k = [40, 7, 0.3]
// - k = [13.3, 21, 0.3] // - k = [13.3, 21, 0.3]
// - x = [pendulum angle, pendulum velocity, motor velocity]' // - x = [pendulum angle, pendulum velocity, motor velocity]'
if (abs(p_angle) > 1.5) if (abs(p_angle) > 2.5)
{ {
last_unstable_time = millis(); last_unstable_time = millis();
stable = 0; stable = 0;
@ -382,6 +407,7 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
//Serial.println(stable); //Serial.println(stable);
float u; float u;
#if FLAG_V
if (!stable) if (!stable)
{ {
u = LQR_K1_1 * p_angle + LQR_K1_2 * p_vel + LQR_K1_3 * m_vel; u = LQR_K1_1 * p_angle + LQR_K1_2 * p_vel + LQR_K1_3 * m_vel;
@ -391,8 +417,17 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
//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_K2_1 * p_angle + LQR_K2_2 * p_vel + LQR_K2_3 * m_vel; u = LQR_K2_1 * p_angle + LQR_K2_2 * p_vel + LQR_K2_3 * m_vel;
} }
#else
if (!stable)
{
u = LQR_K3_1 * p_angle + LQR_K3_2 * p_vel + LQR_K3_3 * m_vel;
}
else
{
//u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel;
u = LQR_K4_1 * p_angle + LQR_K4_2 * p_vel + LQR_K4_3 * m_vel;
}
#endif
return u; return u;
} }
void wifi_print(char * s,double num) void wifi_print(char * s,double num)

3
arduino/problems and reasons.txt
View File

@ -9,3 +9,6 @@
问题4：强制类型转换(const unsigned char*)不能直接使用print输出 问题4：强制类型转换(const unsigned char*)不能直接使用print输出
解决：atoi((char*)(packet.data())) //使用(char*) 进行强制类型转换 解决：atoi((char*)(packet.data())) //使用(char*) 进行强制类型转换
问题5: udp发送数据时会出现乱码
解决：字符数组末尾需要添加'\0'作为结束符

428
main.cpp
View File

@ -1,428 +0,0 @@
#include <Arduino.h>
#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
#define gyroZ_OFF -0.22
//#define stable_angle 178.2
//#define stable_angle 58.8
//#define stable_angle 301.75
#define stable_angle 60.0
Kalman kalmanZ;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroZangle; // Angle calculate using the gyro only
double compAngleZ; // Calculated angle using a complementary filter
double kalAngleZ; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
/********************************************************************************/
#include <SimpleFOC.h>
//#include "common/foc_utils.h"
#define swing_up_voltage 1.5 //V
#define balance_voltage 10 //V
#define min_voltage 9.5 //V
/*
#define PID_P 0 //
#define PID_I 0 //
#define PID_D 1 //
#define LQR_K1 1 //
#define LQR_K2 0 //
#define LQR_K3 0.0 //
*/
float PID_P = 1; //
float PID_I = 0; //
float PID_D = 0; //
/*
//稳定器参数
float LQR_K1 = 50; //摇摆到平衡
float LQR_K2 = 2; //
float LQR_K3 = 0.30; //
float LQR_K1_1 = 50; //平衡态
float LQR_K2_1 = 2; //
float LQR_K3_1 = 0.15; //
*/
//倒立摆参数
float LQR_K1 = 200; //摇摆到平衡
float LQR_K2 = 40; //
float LQR_K3 = 0.30; //
float LQR_K1_1 = 200; //平衡态
float LQR_K2_1 = 15; //
float LQR_K3_1 = 0.15; //
/*
float LQR_K1 = 200; //
float LQR_K2 = 40; //
float LQR_K3 = 0.30; //
*/
/*单角度稳定
float LQR_K1 = 80; //平衡完成
float LQR_K2 = 15; //
float LQR_K3 = 0.15; //
*/
float OFFSET = 0;
bool stable = 0, battery_low = 0;
uint32_t last_unstable_time;
//output=LQR_K1*PID+LQR_K2*p_vel + LQR_K3 * m_vel
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
PIDController angle_pid = PIDController(PID_P, PID_I, PID_D, balance_voltage * 0.7, 20000);
LowPassFilter lpf_throttle{0.00};
// BLDC motor init
BLDCMotor motor = BLDCMotor(5);
// driver instance
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8, 3);
double rotationshift(double origin, double theta, double shift, bool y);
double acc2rotation(double x, double y);
float controllerLQR(float p_angle, float p_vel, float m_vel);
float constrainAngle(float x);
// instantiate the commander
Commander command = Commander(Serial);
//void onp(char *cmd) { command.scalar(&PID_P, cmd); }
//void oni(char *cmd) { command.scalar(&PID_I, cmd); }
//void ond(char *cmd) { command.scalar(&PID_D, cmd); }
void onj(char *cmd) { command.scalar(&LQR_K1, cmd); }
void onk(char *cmd) { command.scalar(&LQR_K2, cmd); }
void onl(char *cmd) { command.scalar(&LQR_K3, cmd); }
/********************************************************************************/
void setup()
{
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
Serial.println(((analogRead(A3) / 41.5)));
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false))
; // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true))
; // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1))
;
if (i2cData[0] != 0x68)
{ // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1)
;
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6))
;
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
// Eq. 25 and 26
double pitch = acc2rotation(accX, accY);
kalmanZ.setAngle(pitch);
gyroZangle = pitch;
timer = micros();
pinMode(4, OUTPUT);
digitalWrite(4, 1);
sensor.init(&Wire);
motor.linkSensor(&sensor);
// driver
driver.voltage_power_supply = 12;
driver.init();
// link the driver and the motor
motor.linkDriver(&driver);
// aligning voltage
motor.voltage_sensor_align = 3;
// choose FOC modulation (optional)
//motor.foc_modulation = FOCModulationType::SinePWM;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// set control loop type to be used
motor.controller = MotionControlType::torque;
//motor.controller = TorqueControlType::voltage;
motor.voltage_limit = balance_voltage;
motor.useMonitoring(Serial);
// initialize motor
motor.init();
// align encoder and start FOC
//motor.initFOC(4.5,Direction::CW);
//motor.initFOC(4.05, Direction::CCW);
motor.initFOC();
//motor.initFOC(2.6492,Direction::CW);
//command.add('p', onp, "p");
//command.add('i', oni, "i");
//command.add('d', ond, "d");
command.add('j', onj, "newj:");
command.add('k', onk, "newk:");
command.add('l', onl, "newl:");
digitalWrite(4, 0);
}
long loop_count = 0;
float target_voltage;
void loop()
{
motor.loopFOC();
if (loop_count++ == 10)
{
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14))
;
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; // Calculate delta time
timer = micros();
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
// Eq. 25 and 26
double pitch = acc2rotation(accX, accY);
double gyroZrate = gyroZ / 131.0; // Convert to deg/s
kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);
gyroZangle += (gyroZrate + gyroZ_OFF) * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate() * dt;
compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroZangle < -180 || gyroZangle > 180)
gyroZangle = kalAngleZ;
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("\t");
Serial.print(accY); Serial.print("\t");
Serial.print(accZ); Serial.print("\t");
Serial.print(gyroX); Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ); Serial.print("\t");
Serial.print("\t");
#endif
#if 0
Serial.print(pitch);
Serial.print("\t");
Serial.print(gyroZangle);
Serial.print("\t");
Serial.print(compAngleZ);
Serial.print("\t");
Serial.print(kalAngleZ);
Serial.print("\t");
//Serial.print("\r\n");
#endif
// calculate the pendulum angle
//LQR_K1 = analogRead(A3) / 10.0;
digitalWrite(3, 1);
//float pendulum_angle = constrainAngle(rotationshift(kalAngleZ * 3, 180.0, -180.0+OFFSET, 0.0) / 57.29578 + M_PI);
//float pendulum_angle = constrainAngle((kalAngleZ - stable_angle ) / 57.29578);
float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - stable_angle) / 57.29578);
if (abs(pendulum_angle) < 0.6) // if angle small enough stabilize 0.5~30°,1.5~90°
{
//target_voltage = controllerLQR(pendulum_angle, g.gyro.z, motor.shaftVelocity());
target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate / 57.29578, motor.shaftVelocity());
//digitalWrite(4, 1);
}
else // else do swing-up
{ // sets 1.5V to the motor in order to swing up
target_voltage = -_sign(gyroZrate) * swing_up_voltage;
digitalWrite(4, 0);
}
// set the target voltage to the motor
if (accZ < -13000 && ((accX * accX + accY * accY) > (14000 * 14000)))
{
motor.move(0);
}
else
{
motor.move(lpf_throttle(target_voltage));
}
command.run();
// restart the counter
loop_count = 0;
//Serial.print("kangle:");
driver.voltage_power_supply = analogRead(A3) / 41.5;
//Serial.println(driver.voltage_power_supply);
if ((analogRead(A3) / 41.5) < min_voltage && !battery_low)
{
battery_low = 1;
Serial.println("battery_low!!");
while (battery_low)
{
motor.loopFOC();
motor.move(0);
if (millis() % 500 < 250)
digitalWrite(4, 1);
else
digitalWrite(4, 0);
}
}
//Serial.print(",fangle:");
//Serial.print(constrainAngle(rotationshift(kalAngleZ * 3, 180.0, -180.0+OFFSET, 0.0) / 57.29578 + M_PI));
//Serial.println(fmod(kalAngleZ * 3, 360.0) / 3.0);
//Serial.print(",pid:");
//Serial.println(accX);
//Serial.print(angle_pid(pendulum_angle));
//Serial.print(",voltage:");
//Serial.print(target_voltage);
//Serial.print(",lpf_throttle:");
//Serial.println(lpf_throttle(target_voltage));
//Serial.print(",E_gle:");
//Serial.print(sensor.getAngle());
//Serial.print(",vel:");
//Serial.println(sensor.getVelocity());
}
}
// function constraining the angle in between -pi and pi, in degrees -180 and 180
float constrainAngle(float x)
{
x = fmod(x + M_PI, _2PI);
if (x < 0)
x += _2PI;
return x - M_PI;
}
// LQR stabilization controller functions
// calculating the voltage that needs to be set to the motor in order to stabilize the pendulum
float controllerLQR(float p_angle, float p_vel, float m_vel)
{
// if angle controllable
// calculate the control law
// LQR controller u = k*x
// - k = [40, 7, 0.3]
// - k = [13.3, 21, 0.3]
// - x = [pendulum angle, pendulum velocity, motor velocity]'
if (abs(p_angle) > 0.05)
{
last_unstable_time = millis();
stable = 0;
digitalWrite(4, 0);
}
if ((millis() - last_unstable_time) > 1000)
{
stable = 1;
digitalWrite(4, 1);
}
//Serial.println(stable);
float u;
if (!stable)
{
u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel;
}
else
{
//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;
}
// limit the voltage set to the motor
if (abs(u) > motor.voltage_limit * 0.7)
u = _sign(u) * motor.voltage_limit * 0.7;
return u;
}
/* mpu6050加速度转换为角度
acc2rotation(ax, ay)
acc2rotation(az, ay) */
double acc2rotation(double x, double y)
{
if (y < 0)
{
return atan(x / y) / 1.570796 * 90 + 180;
}
else if (x < 0)
{
return (atan(x / y) / 1.570796 * 90 + 360);
}
else
{
return (atan(x / y) / 1.570796 * 90);
}
}
/* mpu6050加速度转换为角度
rotationshift(original angle,+θ,shiftθ,0 is normal,1 is reverse)
rotationshift(0,30)=30
rotationshift(20,30)=50
rotationshift(0,30,1)=330
rotationshift(20,30,1)=310
rotationshift(0,0,-180,0)=-180
*/
double rotationshift(double origin, double theta, double shift = 0, bool y = false)
{
static float origin_old;
if (abs(origin - origin_old) > 0.1)
origin_old += _sign(origin - origin_old) * 0.01;
else
origin_old = origin;
if (y == 0)
{
if (origin + theta > 360)
return origin + theta - 360 + shift;
else
{
return origin + theta + shift;
}
}
else
{
if (-(origin + theta) + 360 < 0)
return -(origin + theta) + 360 + 360 + shift;
else
{
return -(origin + theta) + 360 + shift;
}
}
}

1
python_gui/gui/main.py
View File

@ -118,6 +118,7 @@ class MyWindow(QMainWindow, Ui_MainWindow):
recv_data = recv_data[:-1] recv_data = recv_data[:-1]
recv_data = recv_data.split(',') recv_data = recv_data.split(',')
"""处理接受的信息""" """处理接受的信息"""
# recv_data = [40,50,60]
for i, data in enumerate(recv_data): for i, data in enumerate(recv_data):
self.re_item.append(''.join(re.split(r'[^A-Za-z]', data))) self.re_item.append(''.join(re.split(r'[^A-Za-z]', data)))
print(self.re_item) print(self.re_item)

9328
莱洛三角切割/triangle.dxf
View File

File diff suppressed because it is too large Load Diff

BIN
莱洛三角切割/triangle.lcp
View File

Binary file not shown.

9628
莱洛三角切割/wheel.dxf
View File

File diff suppressed because it is too large Load Diff

BIN
莱洛三角切割/wheel.lcp
View File

Binary file not shown.