fix some bug

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_1 = 10;   //摇摆到平衡
-float LQR_K3_2 = 3.18;   //
+float LQR_K3_2 = 1.7;   //
-float LQR_K3_3 = 1.86; //
+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;
   }
 

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;
+}

arduino/test1/Command.h

@@ -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
+  
+}; 

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; };

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

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
+}

arduino/test1/test1.ino

@@ -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;
+}

python_gui/gui/main_ui.ui

@@ -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>
+      <x>310</x>
-      <y>460</y>
+      <y>500</y>
       <width>591</width>
       <height>131</height>
      </rect>