diff --git a/arduino/main/main.ino b/arduino/main/main.ino index 28dca74..2cbf596 100644 --- a/arduino/main/main.ino +++ b/arduino/main/main.ino @@ -36,10 +36,11 @@ float constrainAngle(float x); const char *ssid = "esp32"; const char *password = "12345678"; - +#define WIFI_FLAG 1 AsyncUDP udp; //创建UDP对象 unsigned int localUdpPort = 2333; //本地端口号 unsigned int broadcastPort = localUdpPort; +void wifi_print(char * s,double num); MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); float PID_P = 1; // @@ -50,7 +51,7 @@ PIDController angle_pid = PIDController(PID_P, PID_I, PID_D, balance_voltage * 0 LowPassFilter lpf_throttle{0.00}; #define FLAG_V 1 //倒立摆参数 -float LQR_K1 = 600; //摇摆到平衡 +float LQR_K1 = 400; //摇摆到平衡 float LQR_K2 = 40; // float LQR_K3 = 0.30; // @@ -65,7 +66,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22); //命令设置 double target_velocity = 0; -double target_angle = 31; +double target_angle = 88; double target_voltage = 0; void onPacketCallBack(AsyncUDPPacket packet) { @@ -75,10 +76,6 @@ void onPacketCallBack(AsyncUDPPacket packet) // packet.print("reply data"); } // instantiate the commander -Commander command = Commander(Serial); -void onp(char* cmd) { command.scalar(&PID_P, cmd); } -void oni(char* cmd) { command.scalar(&PID_I, cmd); } -void ond(char* cmd) { command.scalar(&PID_D, cmd); } void setup() { Serial.begin(115200); @@ -172,21 +169,18 @@ void setup() { //初始化 FOC motor.initFOC(); - command.add('P', onp, "newKP"); - command.add('I', oni, "newKI"); - command.add('D', ond, "newKD"); Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); } -char s[255]; +char buf[255]; int t_v; int lim_v = 60; long loop_count = 0; void loop() { motor.loopFOC(); - if (loop_count++ == 5) + if (1) { while (i2cRead(0x3B, i2cData, 14)); accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); @@ -211,19 +205,25 @@ void loop() { if (gyroZangle < -180 || gyroZangle > 180) gyroZangle = kalAngleZ; - sprintf(s, "%.2f",kalAngleZ); //将100转为16进制表示的字符串 + float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - target_angle) / 57.29578); #if FLAG_V - if (abs(pendulum_angle) < 0.3) // if angle small enough stabilize 0.5~30°,1.5~90° + if (abs(pendulum_angle) < 0.6) // if angle small enough stabilize 0.5~30°,1.5~90° { target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate / 57.29578, motor.shaftVelocity()); // limit the voltage set to the motor - if (abs(target_voltage) > motor.voltage_limit * 0.9) - target_voltage = _sign(target_voltage) * motor.voltage_limit * 0.9; + if (abs(target_voltage) > motor.voltage_limit * 0.7) + target_voltage = _sign(target_voltage) * motor.voltage_limit * 0.7; } else // else do swing-up { // sets 1.5V to the motor in order to swing up - target_voltage = -_sign(gyroZrate) * swing_up_voltage; + if(loop_count == 0) + { + loop_count = 30; + target_voltage = -_sign(gyroZrate) * 3; + } + else + loop_count--; } // set the target voltage to the motor @@ -235,7 +235,7 @@ void loop() { { motor.move(lpf_throttle(target_voltage)); } - Serial.print(target_voltage);Serial.print("\t"); + #else if (abs(pendulum_angle) < 0.6) // if angle small enough stabilize 0.5~30°,1.5~90° { @@ -258,9 +258,11 @@ void loop() { { motor.move(lpf_throttle(target_velocity)); } - Serial.print(target_velocity);Serial.print("\t"); + #endif -#if 1 +#if 0 + Serial.print(target_voltage);Serial.print("\t"); + Serial.print(target_velocity);Serial.print("\t"); Serial.print(motor.shaft_velocity);Serial.print("\t"); Serial.print(target_angle);Serial.print("\t"); Serial.print(pendulum_angle+target_angle);Serial.print("\t"); @@ -269,11 +271,17 @@ void loop() { #endif // motor.move(target_velocity); //可以使用该方法广播信息 +#if WIFI_FLAG + + memset(buf, 0, strlen(buf)); + wifi_print("p",pendulum_angle+target_angle); + wifi_print("t",target_angle); + wifi_print("k",kalAngleZ); + wifi_print("g",gyroZrate); IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址 - udp.writeTo((const unsigned char*)s, strlen(s), broadcastAddr, localUdpPort); //广播数据 - loop_count = 0; + udp.writeTo((const unsigned char*)buf, strlen(buf), broadcastAddr, localUdpPort); //广播数据 } - command.run(); +#endif } /* mpu6050加速度转换为角度 acc2rotation(ax, ay) @@ -336,3 +344,13 @@ float controllerLQR(float p_angle, float p_vel, float m_vel) return u; } +void wifi_print(char * s,double num) +{ + char str[255]; + char n[255]; + sprintf(n, "%.2f",num); + strcpy(str,s); + strcat(str, n); + strcat(buf+strlen(buf), str); + strcat(buf, ","); +} diff --git a/python_gui/gui/main.py b/python_gui/gui/main.py index 0a76dd1..35d710c 100644 --- a/python_gui/gui/main.py +++ b/python_gui/gui/main.py @@ -105,18 +105,23 @@ class MyWindow(QMainWindow, Ui_MainWindow): while self.close_flag: recv_data = self.udp.udpClientSocket.recv(1024) recv_data = recv_data.decode('utf-8') - self.udp_data = recv_data + recv_data = recv_data[:-1] + self.udp_data = recv_data.split(',') + """处理接受的信息""" + print(len(self.udp_data)) + for i, data in enumerate(self.udp_data): + # print(i,data) + + # self.signalDataArrays[i] = np.roll(self.signalDataArrays[i], -1) + # self.signalDataArrays[i][-1] = data + pass def update_plot(self): - if self.wifi_recv_flag: - 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]) + pass + # if self.wifi_recv_flag: + # 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/test1.py b/python_gui/gui/test1.py index 21bb272..e69de29 100644 --- a/python_gui/gui/test1.py +++ b/python_gui/gui/test1.py @@ -1,7 +0,0 @@ -import socket -#创建socket对象 -#SOCK_DGRAM udp模式 -s=socket.socket(socket.AF_INET,socket.SOCK_DGRAM) -s.bind(("169.254.184.146",8000)) #绑定服务器的ip和端口 -data=s.recv(1024) #一次接收1024字节 -print(data.decode())# decode()解码收到的字节 \ No newline at end of file