fix some bug
45coll
parent
5590208732
commit
17579c5e67
|
|
@ -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();
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
#include <Arduino.h>
|
||||
// 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
|
||||
|
||||
};
|
||||
|
|
@ -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; };
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
#include "Command.h"
|
||||
#include <Wire.h>
|
||||
#include <WiFi.h>
|
||||
#include <AsyncUDP.h> //引用以使用异步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;
|
||||
}
|
||||
|
|
@ -19,19 +19,19 @@
|
|||
<rect>
|
||||
<x>20</x>
|
||||
<y>460</y>
|
||||
<width>241</width>
|
||||
<width>271</width>
|
||||
<height>61</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="title">
|
||||
<string>wifi_IP</string>
|
||||
<string>本机Wifi_IP</string>
|
||||
</property>
|
||||
<widget class="QWidget" name="horizontalLayoutWidget">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>10</x>
|
||||
<y>20</y>
|
||||
<width>221</width>
|
||||
<width>251</width>
|
||||
<height>31</height>
|
||||
</rect>
|
||||
</property>
|
||||
|
|
@ -59,10 +59,10 @@
|
|||
<widget class="QGroupBox" name="groupBox_2">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>370</x>
|
||||
<x>310</x>
|
||||
<y>10</y>
|
||||
<width>811</width>
|
||||
<height>441</height>
|
||||
<height>481</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="title">
|
||||
|
|
@ -72,9 +72,9 @@
|
|||
<property name="geometry">
|
||||
<rect>
|
||||
<x>10</x>
|
||||
<y>20</y>
|
||||
<y>50</y>
|
||||
<width>791</width>
|
||||
<height>361</height>
|
||||
<height>371</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout"/>
|
||||
|
|
@ -83,13 +83,24 @@
|
|||
<property name="geometry">
|
||||
<rect>
|
||||
<x>10</x>
|
||||
<y>380</y>
|
||||
<y>420</y>
|
||||
<width>791</width>
|
||||
<height>51</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QHBoxLayout" name="tool_layout"/>
|
||||
</widget>
|
||||
<widget class="QWidget" name="horizontalLayoutWidget_6">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>10</x>
|
||||
<y>20</y>
|
||||
<width>791</width>
|
||||
<height>31</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QHBoxLayout" name="tool_layout_2"/>
|
||||
</widget>
|
||||
</widget>
|
||||
<widget class="QGroupBox" name="groupBox_3">
|
||||
<property name="geometry">
|
||||
|
|
@ -149,8 +160,8 @@
|
|||
<widget class="QGroupBox" name="groupBox_4">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>370</x>
|
||||
<y>460</y>
|
||||
<x>310</x>
|
||||
<y>500</y>
|
||||
<width>591</width>
|
||||
<height>131</height>
|
||||
</rect>
|
||||
|
|
|
|||
Loading…
Reference in New Issue