From c8ae6eecc03ec4519f276b788b54196a6db21665 Mon Sep 17 00:00:00 2001 From: 45coll <674148718@qq.com> Date: Wed, 25 Aug 2021 16:23:23 +0800 Subject: [PATCH] 8.25 --- arduino/main/i2c.ino | 63 ++++++ arduino/main/main.ino | 179 +++++++++++++++--- arduino/udp_kalman/i2c.ino | 63 ++++++ arduino/udp_kalman/kalman.ino | 98 ++++++++++ arduino/udp_kalman/udp_kalman.ino | 68 +++++++ .../udp_test1/{udp_test1 => }/udp_test1.ino | 0 .../wifi_test/{wifi_test => }/wifi_test.ino | 0 .../gui/__pycache__/main_ui.cpython-37.pyc | Bin 2708 -> 3213 bytes .../gui/__pycache__/wifi_udp.cpython-37.pyc | Bin 1159 -> 1168 bytes python_gui/gui/main.py | 58 ++++-- python_gui/gui/main_ui.py | 54 ++++-- python_gui/gui/main_ui.ui | 130 ++++++++----- 12 files changed, 608 insertions(+), 105 deletions(-) create mode 100644 arduino/main/i2c.ino create mode 100644 arduino/udp_kalman/i2c.ino create mode 100644 arduino/udp_kalman/kalman.ino create mode 100644 arduino/udp_kalman/udp_kalman.ino rename arduino/udp_test1/{udp_test1 => }/udp_test1.ino (100%) rename arduino/wifi_test/{wifi_test => }/wifi_test.ino (100%) diff --git a/arduino/main/i2c.ino b/arduino/main/i2c.ino new file mode 100644 index 0000000..23fa91c --- /dev/null +++ b/arduino/main/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/main/main.ino b/arduino/main/main.ino index 00e9496..bbcc3f6 100644 --- a/arduino/main/main.ino +++ b/arduino/main/main.ino @@ -9,17 +9,44 @@ Deng's FOC 闭环速度控制例程 测试库：SimpleFOC 2.1.1 测试硬件： #include #include #include //引用以使用异步UDP +#include // Source: https://github.com/TKJElectronics/KalmanFilter +Kalman kalmanZ; +#define gyroZ_OFF -0.72 +/* ----IMU Data---- */ +float PID_P = 8; // +float PID_I = 0; // +float PID_D = 0; // +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); const char *ssid = "esp32"; const char *password = "12345678"; + AsyncUDP udp; //创建UDP对象 unsigned int localUdpPort = 2333; //本地端口号 +unsigned int broadcastPort = localUdpPort; MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); -TwoWire I2Cone = TwoWire(0); +TwoWire I2Ctwo = TwoWire(1); +//倒立摆参数 +float LQR_K1 = 200; //摇摆到平衡 +float LQR_K2 = 15; // +float LQR_K3 = 0.15; // //电机参数 BLDCMotor motor = BLDCMotor(5); @@ -28,6 +55,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22); //命令设置 int target_velocity = 0; +int target_angle = 149; void onPacketCallBack(AsyncUDPPacket packet) { target_velocity = atoi((char*)(packet.data())); @@ -38,7 +66,45 @@ void onPacketCallBack(AsyncUDPPacket packet) void setup() { Serial.begin(115200); - //wifi初始化 + + + // 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"); + + //wifi初始化 WiFi.mode(WIFI_AP); while(!WiFi.softAP(ssid, password)){}; //启动AP Serial.println("AP启动成功"); @@ -46,50 +112,40 @@ void setup() { { } udp.onPacket(onPacketCallBack); //注册收到数据包事件 - I2Cone.begin(23, 5, 400000); //SDA,SCL - - sensor.init(&I2Cone); + + I2Ctwo.begin(23, 5, 400000); //SDA,SCL + sensor.init(&I2Ctwo); //连接motor对象与传感器对象 motor.linkSensor(&sensor); - //供电电压设置 [V] driver.voltage_power_supply = 12; driver.init(); - //连接电机和driver对象 motor.linkDriver(&driver); - //FOC模型选择 motor.foc_modulation = FOCModulationType::SpaceVectorPWM; //运动控制模式设置 motor.controller = MotionControlType::velocity; - - //速度PI环设置 motor.PID_velocity.P = 1.5; - motor.PID_velocity.I = 20; //最大电机限制电机 motor.voltage_limit = 12; - //速度低通滤波时间常数 motor.LPF_velocity.Tf = 0.01; - //设置最大速度限制 motor.velocity_limit = 40; - motor.useMonitoring(Serial); - //初始化电机 motor.init(); @@ -97,18 +153,95 @@ void setup() { //初始化 FOC motor.initFOC(); - Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); - + + } - - - +char s[255]; +int t_v; +int lim_v = 20; 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; + + sprintf(s, "%.2f",kalAngleZ); //将100转为16进制表示的字符串 +// target_velocity = -angle_pid(kalAngleZ); +// if (abs(target_velocity)>lim_v) +// target_velocity = -target_velocity; +// if (target_velocity >lim_v) +// target_velocity = lim_v; +// if (target_velocity<-lim_v) +// target_velocity = -lim_v; + + Serial.print(motor.shaft_velocity);Serial.print("\t"); + Serial.print(target_velocity);Serial.print("\t"); + Serial.print(target_angle);Serial.print("\t"); + Serial.print(kalAngleZ);Serial.print("\t"); + Serial.print("\r\n"); motor.loopFOC(); - - motor.move(target_velocity); - + //可以使用该方法广播信息 + IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址 + udp.writeTo((const unsigned char*)s, strlen(s), broadcastAddr, localUdpPort); //广播数据 +} +/* 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); + } +} + +unsigned long lastTime; +double errSum, lastErr; +int angle_pid(double now_angle) +{ + /*How long since we last calculated*/ + unsigned long now = millis(); + double timeChange = (double)(now - lastTime); + + /*Compute all the working error variables*/ + double error = target_angle-now_angle; + errSum += (error * timeChange); + double dErr = (error - lastErr) / timeChange; + + /*Compute PID Output*/ + int Output = PID_P * error + PID_I * errSum + PID_D * dErr; + + /*Remember some variables for next time*/ + lastErr = error; + lastTime = now; + return Output; } diff --git a/arduino/udp_kalman/i2c.ino b/arduino/udp_kalman/i2c.ino new file mode 100644 index 0000000..23fa91c --- /dev/null +++ b/arduino/udp_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/udp_kalman/kalman.ino b/arduino/udp_kalman/kalman.ino new file mode 100644 index 0000000..8c74f7d --- /dev/null +++ b/arduino/udp_kalman/kalman.ino @@ -0,0 +1,98 @@ +#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 kalman_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(); +} +double kalman_read(){ + 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; + return kalAngleZ; + } +/* 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_kalman/udp_kalman.ino b/arduino/udp_kalman/udp_kalman.ino new file mode 100644 index 0000000..2179b9b --- /dev/null +++ b/arduino/udp_kalman/udp_kalman.ino @@ -0,0 +1,68 @@ +#include +#include //引用以使用异步UDP +#include + +const char *ssid = "esp32"; +const char *password = "12345678"; + +double kalZ; +AsyncUDP udp; //创建UDP对象 +unsigned int localUdpPort = 2333; //本地端口号 + + +unsigned int broadcastPort = localUdpPort; +//const char *broadcastData = "broadcast data"; + const uint8_t broadcastData[] = {"broadcast data"}; + +void onPacketCallBack(AsyncUDPPacket packet) +{ + Serial.print("UDP数据包来源类型: "); + Serial.println(packet.isBroadcast() ? "广播数据" : (packet.isMulticast() ? "组播" : "单播")); + Serial.print("远端地址及端口号: "); + Serial.print(packet.remoteIP()); + Serial.print(":"); + Serial.println(packet.remotePort()); + Serial.print("目标地址及端口号: "); + Serial.print(packet.localIP()); + Serial.print(":"); + Serial.println(packet.localPort()); + Serial.print("数据长度: "); + Serial.println(packet.length()); + Serial.print("数据内容: "); + Serial.write(packet.data(), packet.length()); + Serial.println(); + +// packet.print("reply data"); +// broadcastPort = packet.remotePort(); +} + +void setup() +{ + Serial.begin(115200); + // kalman mpu6050 init + kalman_init(); + + //wifi初始化 + WiFi.mode(WIFI_AP); + while(!WiFi.softAP(ssid, password)){}; //启动AP + Serial.println("AP启动成功"); + while (!udp.listen(localUdpPort)) //等待udp监听设置成功 + { + } + udp.onPacket(onPacketCallBack); //注册收到数据包事件 +} +char s[255]; +void loop() +{ + kalZ = kalman_read(); + Serial.print(kalZ);Serial.print("\t"); + sprintf(s, "%.2f", kalZ); //将100转为16进制表示的字符串 +// Serial.println(strlen(s)); + + delay(10); + +// udp.broadcastTo(broadcastData, broadcastPort); //可以使用该方法广播信息 + + IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址 + udp.writeTo((const unsigned char*)s, strlen(s), broadcastAddr, localUdpPort); //广播数据 +} diff --git a/arduino/udp_test1/udp_test1/udp_test1.ino b/arduino/udp_test1/udp_test1.ino similarity index 100% rename from arduino/udp_test1/udp_test1/udp_test1.ino rename to arduino/udp_test1/udp_test1.ino diff --git a/arduino/wifi_test/wifi_test/wifi_test.ino b/arduino/wifi_test/wifi_test.ino similarity index 100% rename from arduino/wifi_test/wifi_test/wifi_test.ino rename to arduino/wifi_test/wifi_test.ino diff --git a/python_gui/gui/__pycache__/main_ui.cpython-37.pyc b/python_gui/gui/__pycache__/main_ui.cpython-37.pyc index ae55588b0448e971c6b7cb1076d7ecbe1ab50568..76b07cbc554a22b3c1ee87008c5bcf99eb72078a 100644 GIT binary patch literal 3213 zcmaJ@>uVcF5WhRUWlL5ZzaMd4PAof7{76C*+LGENPH5~{bwc29InLU%yz1#r-8(t4 zaUmt4rQbqZ0!=BEptKa)d}^SSw13D!p-}SBhf?~XpVOJW&B<~?b==y!eGdI0vsY1`UyjY)WNWmCK5%ltXGrmgXg*kkB28gpBa* zkQyQ3WhonJ09(q2ylCE?vzg&V^6q?9$l*Bjxh4XdNG3M@KM7V)IGMH(#9!Xg9l>>-_j zMH(#j3X2Zpaf=K{nU~_p)b@}rF;WKXx`kb5V7C{fy`bp{SY3z+UU$Ij0jURgy)8U$ z1(72!%;V?8&F>{z2Qi29hYz%_zVVgF@8%D%KlS>PTQTwEicI}Xw~XmTQkEMKE>!NWE3 zc@rQ_0PjQ#Z-V#a+A+|Eh0Sqs`f`BfMBI346f#-o9F9w53i)tyPhelzP72#8z@}id zX~CJsK9{B1402$NVx^O{Q@%_jC>%43Ub0y>=>Cu-(Z8T*c&UP6xlA{2@VeCa3F=3q z3mSSJw3Ny~$rKqSm-S7%DykO)RPvHdn0^CD;~%tr16A=Nv#XU^`(^_xYvXrpMo`hS zZZo=NTdr>IblBhyGrN^u<_2A(+NCAUq?U1>P}l3>NWr$&Xi=+Fo%PwO>)O@@x{VRM zivwX1_4+xF8HEkat{B!1Dm{x6`x)u$$n4uas$ZeCX|Hp)2C#^!uNtPeXTvb<0(Cco z$mo6WNF(UB2i(%q^XJbS_jo7)x9M-$q{LwKF48~xzbIy25ygxb<5563Y27NCMi4|} z0DZ;EhE-kFnU`=}-K{zxW>0%5$8fK#mW+bCsFw{j$_$5Y84Um6)L7o1G>buxVb0Vx zpwAn2*>Kq=!+#ewl79z`)wcY6upL+%DmsBW2RVg}xy+ExZm|PZFX5FCk5;`G%STN>NkZOj@%leUl*5@sK)iekjL=;o6 zM)Q|3M{F38eTc;J{(7?m*h8xYYSD7F?92L1E~f#%!YIr_cd2cuO5RnI7lZw(c7XmW zExfm6JCqZW*mK@mwKt4Fc?p(*ZRjRDgrgtEW)zzvIC2I&%=yPnOe~&XfLzV$jN$#l z561`8WPX`v50zO|wu5y5c_{{&(JjZ+UE>;s6&$LqzO#xK zb_{ckMZwkg5<3B(f2X0jaCYVC=MNwMc=y@EJ5TO?v4SLR%`U7|Hr;jG(u!5Of^Qk_ z99os;Dw|#m0;yK6(GD8}sC_~et%9zVKOO#s@s_4VUlzWw{reNUdOV(y-P`QSceVi>g1Vh+CS`>JlT-m zi))$%=RniEq^6Z^QiZaqdF`6^epNSp4Vwbre5f76<3V9O2HY`N%N#73aXJ1rLu4@< oRU=EAdG{1wIEH(R7gaB6^J>dkuX5)hs#D`Jp@y4zWLR06 zSs}G-7u2L?B)NKYIpA)Rtw#$nOA7VqHlyoq6gE(>`_TbhgeM$_!X=hQiKSDG>W_(K zP=jXRouOF>oJ-2Ln7!yQ|2Fe!aKC|8{NUS!=Q2ED)gWYB>%eGtGz}4iqQ;J{kx1_t z+rVxUJF`ui8PH@<1AG?iq9q3?*U3j_r$DnbNAuL&5h}qKX#pJqmuOMpp2%#Lo4u;u z7wOGxv!d$#w4^YhJ&2*#0oCguph4MdnfB5?TG`1+#~_TeUpj`AV;In|bPRxFkPa!w zFgS*#!%&VYpsI9K=>a-IN0p-rj)T&10Ci$6MgWa;M(J4k-N}ewJ_6ctsXeHoj{zE! z(GS499ikH|`WQG4O9zf;Ohg|CG%g(_7|$d@MyvW2>g7{?T#sm^hFQtGDEtWMJ! zJ+%WWF`6leH6yUrIgz?W-cQO^I1Svi^x_=rbXIw%!FyVIYp6pEqc|nKGr-M&W=?8m zu`V#_oJI|(&E{O^Oo}Oi4RmHHDei3_HVHe0eFU2)DK}g{;*NJ;>`(TK1`Xd}k%gjL zg>A*YdinBYmjdeId6A56xL(j;(WZSj;A|`KBgb3u7bqH#@6k29NwLeqbTs- zXWRHQKgN#m^h!pBj=PNwJ7~N9K56$Lw&z=}AFnz*DTI*|#Ua4j*`yr0(Fdzdw-GHl zE!WC%H)LBb$J5u!*V8kT=hIp`$Gn8K68IL}pyft&gkuV=Qhh;Xj^jG82B3$_uvlOS;iALWnIA%&1(`BFg4VIrdJS@z zW{V#}27lB_+i5U=olgLf6dX#^$$^fRRZ=*noF7F6TCV!es^?Oxf<33JT$;@KB4lIU z;e2X0z9g(-eMRI48;@gS4|cvwC1nm%?)ahSMD8sHy)U|vlV;EwfK}o{5>~t*3gB(?~rsqKK~-wXNY- zuH88~7Ipo?F+#HNi0k(=01;_wP5o0HE?;9C?{e*to*qqb>1G9^D<&ryLoxY|DKr_y z6r?7rn4)BIiYZAZpEBaWGnd4iC)(M#0ORUD{QAWY-~RdZODiw8_*UuMh4XXg-h6BB z^||vg=I4(mKmGpW69Iqw{EHV)AJ%%(y;Z5>pP5VTw#OP``y>Um2rS%L>7SCDEb?5j zAw<${x(}jsN8sAFOwgqYT*7-xwPx^BNZ<&pqOAgpwUQRI*j|I;8ayGMnM5-o=L71y zHTG(rURk9}wQOSAwh!ykwv(c5w*ne_h>(ab wreu9-e3+9%4hrKZxbWmjD0& diff --git a/python_gui/gui/__pycache__/wifi_udp.cpython-37.pyc b/python_gui/gui/__pycache__/wifi_udp.cpython-37.pyc index 6fe46e3e71113167b0ee9ea91597033bb398def9..2aa7eb23c9a65a641cc0b5c356cf0b1daddbc178 100644 GIT binary patch delta 167 zcmZqYoWRND#LLUY00atiH*&EsF-A@1XHu6F1PXjL1`=*oG0&%LeY$qSi>)osrp=5g zPR&V+pX|-FSBxJhdy6F{u_Unwq#8m9O_pQMml6i?#o&U-(#x3>gv3EQSu^trN=u4F S!Ad7fvgnHPVbu`Lq5}YYsU;Qw delta 158 zcmbQh+0M!3#LLUY00f5~Z{%WOV!Sn(pGjR*2q^GN*TpI(Ek8NFpeR2pHMt~aatPC2 zF#({=EtZtTlEfmAA_yToS)DmwN(96ghYKQ0Z(vRkk^t#s&CDw(Eh!QME1j&wqASXe JRYMAk4ge)QAg=%b diff --git a/python_gui/gui/main.py b/python_gui/gui/main.py index 895fd8d..0a76dd1 100644 --- a/python_gui/gui/main.py +++ b/python_gui/gui/main.py @@ -7,6 +7,14 @@ import numpy as np import pyqtgraph as pg +RED_COLOR = (255, 92, 92) +GREEN_COLOR = (57, 217, 138) +BLUE_COLOR = (91, 141, 236) +ORANGE_COLOR = (253, 172, 66) +YELLOW_COLOR = (255,255,51) +PURPLE_COLOR = (75,0,130) +MAROON_COLOR = (222,184,135) + class MyWindow(QMainWindow, Ui_MainWindow): def __init__(self, parent=None): super(MyWindow, self).__init__(parent) @@ -31,7 +39,7 @@ class MyWindow(QMainWindow, Ui_MainWindow): # 定时器-绘图刷新 self.timer = QtCore.QTimer() self.timer.timeout.connect(self.update_plot) - self.timer.start(50) + self.timer.start(20) # wifi udp self.udp = udp() # self.wifi_IP_lineEdit.setText(self.udp.user_ip) @@ -43,21 +51,39 @@ class MyWindow(QMainWindow, Ui_MainWindow): self.now_velocity = 0 self.close_flag = 1 def plot_init(self): - # 图表可视化数组 - self.v_list = [] - self.v_list.append(np.zeros(300)) - self.timeArray = np.arange(-300, 0, 1) # 绘图对象 - self.pw = pg.PlotWidget() - self.curve = self.pw.plot(pen='y') - self.gridLayout.addWidget(self.pw) + self.plotWidget = pg.PlotWidget() + self.plotWidget.showGrid(x=True, y=True, alpha=0.5) + # 图表可视化数组 + self.signals = ['now_Angle', 'tag_Angle'] + self.signal_tooltip = ['now_Angle ', 'tag_Angle'] + self.signalColors = [RED_COLOR, BLUE_COLOR] + self.signalIcons = ['reddot', 'bluedot'] + self.numberOfSamples = 300 + self.signalDataArrays = [] + self.signalPlots = [] + self.signalPlotFlags = [] + self.timeArray = np.arange(-self.numberOfSamples, 0, 1) + for (sig, sigColor, tooltip) in zip(self.signals, self.signalColors, self.signal_tooltip): + # define signal plot data array + self.signalDataArrays.append(np.zeros(self.numberOfSamples)) + # configure signal plot parameters + signalPen = pg.mkPen(color=sigColor, width=1.5) + self.signalPlots.append(pg.PlotDataItem(self.timeArray, + self.signalDataArrays[-1], + pen=signalPen, name=tooltip)) + self.plotWidget.addItem(self.signalPlots[-1]) + # is plotted flag + self.signalPlotFlags.append(True) + + self.gridLayout.addWidget(self.plotWidget) # 滑条绑定 def velocity_horizontalSlider_valueChanged(self): self.target_velocity = self.velocity_horizontalSlider.value() self.velocity_lineEdit.setText(str(self.target_velocity)) - self.udp.send_message(str(self.velocity_lineEdit)) - + self.udp.send_message(str(self.target_velocity)) + print(str(self.target_velocity)) def wifi_config_pushButton_clicked(self): try: print(self.wifi_IP_lineEdit.text(),type(self.wifi_IP_lineEdit.text())) @@ -82,9 +108,15 @@ class MyWindow(QMainWindow, Ui_MainWindow): self.udp_data = recv_data def update_plot(self): if self.wifi_recv_flag: - 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]) # 在绘图部件中绘制折线图 + self.signalDataArrays[0] = np.roll(self.signalDataArrays[0], -1) + self.signalDataArrays[0][-1] = self.udp_data + self.signalDataArrays[1] = np.roll(self.signalDataArrays[1], -1) + self.signalDataArrays[1][-1] = self.angle_lineEdit.text() + # self.curve.setData(self.timeArray, self.v_list[0]) # 在绘图部件中绘制折线图 + for i, plotFlag in enumerate(self.signalPlotFlags): + self.signalPlots[i].setData(self.timeArray, self.signalDataArrays[i]) + self.signalPlots[i].updateItems() + self.signalPlots[i].sigPlotChanged.emit(self.signalPlots[i]) def closeEvent(self, a0: QtGui.QCloseEvent) -> None: self.close_flag = 0 diff --git a/python_gui/gui/main_ui.py b/python_gui/gui/main_ui.py index dab51df..cbf40f1 100644 --- a/python_gui/gui/main_ui.py +++ b/python_gui/gui/main_ui.py @@ -14,23 +14,9 @@ from PyQt5 import QtCore, QtGui, QtWidgets class Ui_MainWindow(object): def setupUi(self, MainWindow): MainWindow.setObjectName("MainWindow") - MainWindow.resize(800, 600) + MainWindow.resize(800, 749) self.centralwidget = QtWidgets.QWidget(MainWindow) self.centralwidget.setObjectName("centralwidget") - self.velocity_horizontalSlider = QtWidgets.QSlider(self.centralwidget) - self.velocity_horizontalSlider.setGeometry(QtCore.QRect(470, 420, 261, 22)) - self.velocity_horizontalSlider.setMinimum(-100) - self.velocity_horizontalSlider.setMaximum(100) - self.velocity_horizontalSlider.setOrientation(QtCore.Qt.Horizontal) - self.velocity_horizontalSlider.setTickPosition(QtWidgets.QSlider.TicksAbove) - self.velocity_horizontalSlider.setTickInterval(10) - self.velocity_horizontalSlider.setObjectName("velocity_horizontalSlider") - self.velocity_lineEdit = QtWidgets.QLineEdit(self.centralwidget) - self.velocity_lineEdit.setGeometry(QtCore.QRect(580, 460, 113, 21)) - self.velocity_lineEdit.setObjectName("velocity_lineEdit") - self.label = QtWidgets.QLabel(self.centralwidget) - self.label.setGeometry(QtCore.QRect(520, 460, 51, 16)) - self.label.setObjectName("label") self.gridLayoutWidget = QtWidgets.QWidget(self.centralwidget) self.gridLayoutWidget.setGeometry(QtCore.QRect(180, 20, 581, 361)) self.gridLayoutWidget.setObjectName("gridLayoutWidget") @@ -57,6 +43,38 @@ class Ui_MainWindow(object): self.wifi_recv_open_pushButton.setEnabled(False) self.wifi_recv_open_pushButton.setGeometry(QtCore.QRect(180, 380, 93, 28)) self.wifi_recv_open_pushButton.setObjectName("wifi_recv_open_pushButton") + self.horizontalLayoutWidget_2 = QtWidgets.QWidget(self.centralwidget) + self.horizontalLayoutWidget_2.setGeometry(QtCore.QRect(570, 440, 104, 31)) + self.horizontalLayoutWidget_2.setObjectName("horizontalLayoutWidget_2") + self.horizontalLayout_2 = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget_2) + self.horizontalLayout_2.setContentsMargins(0, 0, 0, 0) + self.horizontalLayout_2.setObjectName("horizontalLayout_2") + self.label = QtWidgets.QLabel(self.horizontalLayoutWidget_2) + self.label.setObjectName("label") + self.horizontalLayout_2.addWidget(self.label) + self.velocity_lineEdit = QtWidgets.QLineEdit(self.horizontalLayoutWidget_2) + self.velocity_lineEdit.setObjectName("velocity_lineEdit") + self.horizontalLayout_2.addWidget(self.velocity_lineEdit) + self.velocity_horizontalSlider = QtWidgets.QSlider(self.centralwidget) + self.velocity_horizontalSlider.setGeometry(QtCore.QRect(410, 420, 261, 22)) + self.velocity_horizontalSlider.setMinimum(-150) + self.velocity_horizontalSlider.setMaximum(150) + self.velocity_horizontalSlider.setOrientation(QtCore.Qt.Horizontal) + self.velocity_horizontalSlider.setTickPosition(QtWidgets.QSlider.TicksAbove) + self.velocity_horizontalSlider.setTickInterval(10) + self.velocity_horizontalSlider.setObjectName("velocity_horizontalSlider") + self.horizontalLayoutWidget_3 = QtWidgets.QWidget(self.centralwidget) + self.horizontalLayoutWidget_3.setGeometry(QtCore.QRect(400, 440, 134, 31)) + self.horizontalLayoutWidget_3.setObjectName("horizontalLayoutWidget_3") + self.horizontalLayout_3 = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget_3) + self.horizontalLayout_3.setContentsMargins(0, 0, 0, 0) + self.horizontalLayout_3.setObjectName("horizontalLayout_3") + self.label_2 = QtWidgets.QLabel(self.horizontalLayoutWidget_3) + self.label_2.setObjectName("label_2") + self.horizontalLayout_3.addWidget(self.label_2) + self.angle_lineEdit = QtWidgets.QLineEdit(self.horizontalLayoutWidget_3) + self.angle_lineEdit.setObjectName("angle_lineEdit") + self.horizontalLayout_3.addWidget(self.angle_lineEdit) MainWindow.setCentralWidget(self.centralwidget) self.menubar = QtWidgets.QMenuBar(MainWindow) self.menubar.setGeometry(QtCore.QRect(0, 0, 800, 26)) @@ -72,9 +90,11 @@ class Ui_MainWindow(object): def retranslateUi(self, MainWindow): _translate = QtCore.QCoreApplication.translate MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow")) - self.velocity_lineEdit.setText(_translate("MainWindow", "0")) - self.label.setText(_translate("MainWindow", "速度：")) self.groupBox.setTitle(_translate("MainWindow", "wifi_IP")) self.wifi_IP_lineEdit.setText(_translate("MainWindow", "192.168.4.2")) self.wifi_config_pushButton.setText(_translate("MainWindow", "设置")) self.wifi_recv_open_pushButton.setText(_translate("MainWindow", "打开")) + self.label.setText(_translate("MainWindow", "速度：")) + self.velocity_lineEdit.setText(_translate("MainWindow", "0")) + self.label_2.setText(_translate("MainWindow", "目标角度：")) + self.angle_lineEdit.setText(_translate("MainWindow", "149")) diff --git a/python_gui/gui/main_ui.ui b/python_gui/gui/main_ui.ui index ffa1791..40b32aa 100644 --- a/python_gui/gui/main_ui.ui +++ b/python_gui/gui/main_ui.ui @@ -7,64 +7,13 @@ 0 0 800 - 600 + 749 MainWindow - - - - 470 - 420 - 261 - 22 - - - - -100 - - - 100 - - - Qt::Horizontal - - - QSlider::TicksAbove - - - 10 - - - - - - 580 - 460 - 113 - 21 - - - - 0 - - - - - - 520 - 460 - 51 - 16 - - - - 速度： - - @@ -134,6 +83,83 @@ 打开 + + + + 570 + 440 + 104 + 31 + + + + + + + 速度： + + + + + + + 0 + + + + + + + + + 410 + 420 + 261 + 22 + + + + -150 + + + 150 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 10 + + + + + + 400 + 440 + 134 + 31 + + + + + + + 目标角度： + + + + + + + 149 + + + + +