From 17579c5e67f6e514a301a83e7c3b73e4e5805276 Mon Sep 17 00:00:00 2001 From: 45coll <674148718@qq.com> Date: Tue, 16 Nov 2021 14:35:36 +0800 Subject: [PATCH] fix some bug --- arduino/main/main.ino | 18 ++++-- arduino/test1/Command.cpp | 28 +++++++++ arduino/test1/Command.h | 17 ++++++ arduino/test1/Kalman.cpp | 93 ++++++++++++++++++++++++++++ arduino/test1/Kalman.h | 59 ++++++++++++++++++ arduino/test1/i2c.ino | 63 +++++++++++++++++++ arduino/test1/test1.ino | 124 ++++++++++++++++++++++++++++++++++++++ python_gui/gui/main_ui.ui | 31 +++++++--- 8 files changed, 417 insertions(+), 16 deletions(-) create mode 100644 arduino/test1/Command.cpp create mode 100644 arduino/test1/Command.h create mode 100644 arduino/test1/Kalman.cpp create mode 100644 arduino/test1/Kalman.h create mode 100644 arduino/test1/i2c.ino create mode 100644 arduino/test1/test1.ino diff --git a/arduino/main/main.ino b/arduino/main/main.ino index fa06798..cb17984 100644 --- a/arduino/main/main.ino +++ b/arduino/main/main.ino @@ -58,9 +58,9 @@ float LQR_K2_1 = 3.49; //平衡态 float LQR_K2_2 = 0.26; // float LQR_K2_3 = 0.15; // -float LQR_K3_1 = 5.25; //摇摆到平衡 -float LQR_K3_2 = 3.18; // -float LQR_K3_3 = 1.86; // +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; // @@ -73,7 +73,7 @@ float target_velocity = 0; float target_angle = 89.3; float target_voltage = 0; float swing_up_voltage = 1.8; -float swing_up_angle = 18; +float swing_up_angle = 20; float v_i_1 = 20; float v_p_1 = 0.5; float v_i_2 = 10; @@ -408,13 +408,19 @@ float controllerLQR(float p_angle, float p_vel, float m_vel) // - k = [40, 7, 0.3] // - k = [13.3, 21, 0.3] // - x = [pendulum angle, pendulum velocity, motor velocity]' + if (abs(p_angle) > 2.5) { last_unstable_time = millis(); - stable = 0; + if(stable) + { + target_angle = EEPROM.readFloat(0); + stable = 0; + } } - if ((millis() - last_unstable_time) > 1000) + if ((millis() - last_unstable_time) > 1000&&!stable) { + target_angle = target_angle+p_angle; stable = 1; } diff --git a/arduino/test1/Command.cpp b/arduino/test1/Command.cpp new file mode 100644 index 0000000..9c54a6b --- /dev/null +++ b/arduino/test1/Command.cpp @@ -0,0 +1,28 @@ +#include "Command.h" + +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::add(char* id, CommandCallback onCommand){ + call_list[call_count] = onCommand; + call_ids[call_count] = id; + call_count++; +} +void Command::scalar(float* value, char* user_cmd){ + *value = atof(user_cmd); +} +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; +} diff --git a/arduino/test1/Command.h b/arduino/test1/Command.h new file mode 100644 index 0000000..20e2fe5 --- /dev/null +++ b/arduino/test1/Command.h @@ -0,0 +1,17 @@ +#include +// callback function pointer definiton +typedef void (* CommandCallback)(char*); //!< command callback function pointer +class Command +{ + public: + void add(char* id , CommandCallback onCommand); + void run(char* str); + void scalar(float* value, char* user_cmd); + bool isSentinel(char* ch,char* str); + private: + // Subscribed command callback variables + CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number + char* call_ids[20]; //!< added callback commands + int call_count;//!< number callbacks that are subscribed + +}; diff --git a/arduino/test1/Kalman.cpp b/arduino/test1/Kalman.cpp new file mode 100644 index 0000000..80c7dec --- /dev/null +++ b/arduino/test1/Kalman.cpp @@ -0,0 +1,93 @@ +/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. + + This software may be distributed and modified under the terms of the GNU + General Public License version 2 (GPL2) as published by the Free Software + Foundation and appearing in the file GPL2.TXT included in the packaging of + this file. Please note that GPL2 Section 2[b] requires that all works based + on this software must also be made publicly available under the terms of + the GPL2 ("Copyleft"). + + Contact information + ------------------- + + Kristian Lauszus, TKJ Electronics + Web : http://www.tkjelectronics.com + e-mail : kristianl@tkjelectronics.com + */ + +#include "Kalman.h" + +Kalman::Kalman() { + /* We will set the variables like so, these can also be tuned by the user */ + Q_angle = 0.001f; + Q_bias = 0.003f; + R_measure = 0.03f; + + angle = 0.0f; // Reset the angle + bias = 0.0f; // Reset bias + + P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical + P[0][1] = 0.0f; + P[1][0] = 0.0f; + P[1][1] = 0.0f; +}; + +// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds +float Kalman::getAngle(float newAngle, float newRate, float dt) { + // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 + // Modified by Kristian Lauszus + // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it + + // Discrete Kalman filter time update equations - Time Update ("Predict") + // Update xhat - Project the state ahead + /* Step 1 */ + rate = newRate - bias; + angle += dt * rate; + + // Update estimation error covariance - Project the error covariance ahead + /* Step 2 */ + P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); + P[0][1] -= dt * P[1][1]; + P[1][0] -= dt * P[1][1]; + P[1][1] += Q_bias * dt; + + // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") + // Calculate Kalman gain - Compute the Kalman gain + /* Step 4 */ + float S = P[0][0] + R_measure; // Estimate error + /* Step 5 */ + float K[2]; // Kalman gain - This is a 2x1 vector + K[0] = P[0][0] / S; + K[1] = P[1][0] / S; + + // Calculate angle and bias - Update estimate with measurement zk (newAngle) + /* Step 3 */ + float y = newAngle - angle; // Angle difference + /* Step 6 */ + angle += K[0] * y; + bias += K[1] * y; + + // Calculate estimation error covariance - Update the error covariance + /* Step 7 */ + float P00_temp = P[0][0]; + float P01_temp = P[0][1]; + + P[0][0] -= K[0] * P00_temp; + P[0][1] -= K[0] * P01_temp; + P[1][0] -= K[1] * P00_temp; + P[1][1] -= K[1] * P01_temp; + + return angle; +}; + +void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting angle +float Kalman::getRate() { return this->rate; }; // Return the unbiased rate + +/* These are used to tune the Kalman filter */ +void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; }; +void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; }; +void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; }; + +float Kalman::getQangle() { return this->Q_angle; }; +float Kalman::getQbias() { return this->Q_bias; }; +float Kalman::getRmeasure() { return this->R_measure; }; diff --git a/arduino/test1/Kalman.h b/arduino/test1/Kalman.h new file mode 100644 index 0000000..7de545f --- /dev/null +++ b/arduino/test1/Kalman.h @@ -0,0 +1,59 @@ +/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. + + This software may be distributed and modified under the terms of the GNU + General Public License version 2 (GPL2) as published by the Free Software + Foundation and appearing in the file GPL2.TXT included in the packaging of + this file. Please note that GPL2 Section 2[b] requires that all works based + on this software must also be made publicly available under the terms of + the GPL2 ("Copyleft"). + + Contact information + ------------------- + + Kristian Lauszus, TKJ Electronics + Web : http://www.tkjelectronics.com + e-mail : kristianl@tkjelectronics.com + */ + +#ifndef _Kalman_h_ +#define _Kalman_h_ + +class Kalman { +public: + Kalman(); + + // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds + float getAngle(float newAngle, float newRate, float dt); + + void setAngle(float angle); // Used to set angle, this should be set as the starting angle + float getRate(); // Return the unbiased rate + + /* These are used to tune the Kalman filter */ + void setQangle(float Q_angle); + /** + * setQbias(float Q_bias) + * Default value (0.003f) is in Kalman.cpp. + * Raise this to follow input more closely, + * lower this to smooth result of kalman filter. + */ + void setQbias(float Q_bias); + void setRmeasure(float R_measure); + + float getQangle(); + float getQbias(); + float getRmeasure(); + +private: + /* Kalman filter variables */ + float Q_angle; // Process noise variance for the accelerometer + float Q_bias; // Process noise variance for the gyro bias + float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise + + float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector + float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector + float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate + + float P[2][2]; // Error covariance matrix - This is a 2x2 matrix +}; + +#endif diff --git a/arduino/test1/i2c.ino b/arduino/test1/i2c.ino new file mode 100644 index 0000000..23fa91c --- /dev/null +++ b/arduino/test1/i2c.ino @@ -0,0 +1,63 @@ +/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. + + This software may be distributed and modified under the terms of the GNU + General Public License version 2 (GPL2) as published by the Free Software + Foundation and appearing in the file GPL2.TXT included in the packaging of + this file. Please note that GPL2 Section 2[b] requires that all works based + on this software must also be made publicly available under the terms of + the GPL2 ("Copyleft"). + + Contact information + ------------------- + + Kristian Lauszus, TKJ Electronics + Web : http://www.tkjelectronics.com + e-mail : kristianl@tkjelectronics.com + */ + +const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB +const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication + +uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) { + return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success +} + +uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) { + Wire.beginTransmission(IMUAddress); + Wire.write(registerAddress); + Wire.write(data, length); + uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success + if (rcode) { + Serial.print(F("i2cWrite failed: ")); + Serial.println(rcode); + } + return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission +} + +uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) { + uint32_t timeOutTimer; + Wire.beginTransmission(IMUAddress); + Wire.write(registerAddress); + uint8_t rcode = Wire.endTransmission(false); // Don't release the bus + if (rcode) { + Serial.print(F("i2cRead failed: ")); + Serial.println(rcode); + return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission + } + Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading + for (uint8_t i = 0; i < nbytes; i++) { + if (Wire.available()) + data[i] = Wire.read(); + else { + timeOutTimer = micros(); + while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available()); + if (Wire.available()) + data[i] = Wire.read(); + else { + Serial.println(F("i2cRead timeout")); + return 5; // This error value is not already taken by endTransmission + } + } + } + return 0; // Success +} diff --git a/arduino/test1/test1.ino b/arduino/test1/test1.ino new file mode 100644 index 0000000..808aa96 --- /dev/null +++ b/arduino/test1/test1.ino @@ -0,0 +1,124 @@ +#include "Command.h" +#include +#include +#include //引用以使用异步UDP +#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter +#include "EEPROM.h" +Kalman kalmanZ; +#define gyroZ_OFF 0.9 +/* ----IMU Data---- */ + +double accX, accY, accZ; +double gyroX, gyroY, gyroZ; +int16_t tempRaw; +bool stable = 0; +uint32_t last_unstable_time; + +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 + +void setup() { + Serial.begin(115200); + // kalman mpu6050 init + Wire.begin(19, 18, 400000); // Set I2C frequency to 400kHz + 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]); + double pitch = acc2rotation(accX, accY); + + kalmanZ.setAngle(pitch); + gyroZangle = pitch; + + timer = micros(); + Serial.println("kalman mpu6050 init"); +} + +void loop() { + 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(); + + 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; + 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; + + Serial.print(pitch); Serial.print("\t"); +// Serial.print(gyroZrate); Serial.print("\t"); +// Serial.print(kalAngleZ); Serial.print("\t"); +// Serial.print(gyroZangle); Serial.print("\t"); + Serial.print(compAngleZ); Serial.print("\t"); + Serial.print("\r\n"); +} +/* 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); + } +} + +// function constraining the angle in between -60~60 +float constrainAngle(float x) +{ + float a = 0; + if (x < 0) + { + a = 120 + x; + if (a < abs(x)) + return a; + } + return x; +} diff --git a/python_gui/gui/main_ui.ui b/python_gui/gui/main_ui.ui index f701444..6cd980a 100644 --- a/python_gui/gui/main_ui.ui +++ b/python_gui/gui/main_ui.ui @@ -19,19 +19,19 @@ 20 460 - 241 + 271 61 - wifi_IP + 本机Wifi_IP 10 20 - 221 + 251 31 @@ -59,10 +59,10 @@ - 370 + 310 10 811 - 441 + 481 @@ -72,9 +72,9 @@ 10 - 20 + 50 791 - 361 + 371 @@ -83,13 +83,24 @@ 10 - 380 + 420 791 51 + + + + 10 + 20 + 791 + 31 + + + + @@ -149,8 +160,8 @@ - 370 - 460 + 310 + 500 591 131