diff --git a/arduino/kalman/i2c.ino b/arduino/kalman/i2c.ino new file mode 100644 index 0000000..23fa91c --- /dev/null +++ b/arduino/kalman/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/kalman/kalman.ino b/arduino/kalman/kalman.ino new file mode 100644 index 0000000..b50a566 --- /dev/null +++ b/arduino/kalman/kalman.ino @@ -0,0 +1,111 @@ +#include +#include // Source: https://github.com/TKJElectronics/KalmanFilter +#define gyroZ_OFF -0.72 +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 + +/* ----FOC Data---- */ + +// driver instance +double acc2rotation(double x, double y); +void setup() { + Serial.begin(115200); + /* ----IMU 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(); + /* ----FOC init---- */ +} + +void loop() { + /*----IMU 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; + +#if 1 // Set to 1 to activate + Serial.print(pitch); Serial.print("\t"); + Serial.print(gyroZangle); Serial.print("\t"); + Serial.print(compAngleZ); Serial.print("\t"); + Serial.print(kalAngleZ); Serial.print("\t"); +#endif +Serial.print("\r\n"); +delay(2); +} +/* 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); + } +} diff --git a/arduino/udp_test1/udp_test1/udp_test1.ino b/arduino/udp_test1/udp_test1/udp_test1.ino index 31928f3..a9187c3 100644 --- a/arduino/udp_test1/udp_test1/udp_test1.ino +++ b/arduino/udp_test1/udp_test1/udp_test1.ino @@ -83,7 +83,7 @@ void loop() // Serial.print(" | GyX = "); Serial.print(GyX); // Serial.print(" | GyY = "); Serial.print(GyY); // Serial.print(" | GyZ = "); Serial.println(GyZ); - delay(100); + delay(10); // udp.broadcastTo(broadcastData, broadcastPort); //可以使用该方法广播信息 diff --git a/python_gui/gui/__pycache__/main_ui.cpython-37.pyc b/python_gui/gui/__pycache__/main_ui.cpython-37.pyc index 7e3c881..ae55588 100644 Binary files a/python_gui/gui/__pycache__/main_ui.cpython-37.pyc and b/python_gui/gui/__pycache__/main_ui.cpython-37.pyc differ diff --git a/python_gui/gui/__pycache__/wifi_udp.cpython-37.pyc b/python_gui/gui/__pycache__/wifi_udp.cpython-37.pyc index e099e09..6fe46e3 100644 Binary files a/python_gui/gui/__pycache__/wifi_udp.cpython-37.pyc and b/python_gui/gui/__pycache__/wifi_udp.cpython-37.pyc differ diff --git a/python_gui/gui/main.py b/python_gui/gui/main.py index 78f965d..895fd8d 100644 --- a/python_gui/gui/main.py +++ b/python_gui/gui/main.py @@ -34,14 +34,14 @@ class MyWindow(QMainWindow, Ui_MainWindow): self.timer.start(50) # wifi udp self.udp = udp() - self.wifi_IP_lineEdit.setText(self.udp.user_ip) + # self.wifi_IP_lineEdit.setText(self.udp.user_ip) def variable_init(self): # 图表数据变量 self.wifi_recv_flag = 0 self.udp_data = 0 self.target_velocity = 0 self.now_velocity = 0 - + self.close_flag = 1 def plot_init(self): # 图表可视化数组 self.v_list = [] @@ -60,6 +60,7 @@ class MyWindow(QMainWindow, Ui_MainWindow): def wifi_config_pushButton_clicked(self): try: + print(self.wifi_IP_lineEdit.text(),type(self.wifi_IP_lineEdit.text())) self.udp.udpClientSocket.bind((self.wifi_IP_lineEdit.text(), 2333)) t1 = threading.Thread(target=self.udp_recv) t1.start() @@ -75,7 +76,7 @@ class MyWindow(QMainWindow, Ui_MainWindow): self.wifi_recv_flag = 0 self.wifi_recv_open_pushButton.setText('打开') def udp_recv(self): - while self.wifi_recv_flag: + while self.close_flag: recv_data = self.udp.udpClientSocket.recv(1024) recv_data = recv_data.decode('utf-8') self.udp_data = recv_data @@ -84,6 +85,9 @@ class MyWindow(QMainWindow, Ui_MainWindow): self.v_list[0] = np.roll(self.v_list[0], -1) self.v_list[0][-1] = self.udp_data self.curve.setData(self.timeArray, self.v_list[0]) # 在绘图部件中绘制折线图 + + def closeEvent(self, a0: QtGui.QCloseEvent) -> None: + self.close_flag = 0 if __name__ == '__main__': app = QApplication(sys.argv) myWin = MyWindow()