diff --git a/arduino/main/main.ino b/arduino/main/main.ino index 5572627..0d7d577 100644 --- a/arduino/main/main.ino +++ b/arduino/main/main.ino @@ -51,13 +51,13 @@ 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_K2 = 80; // -float LQR_K3 = -0.30; // +float LQR_K1 = 0; //摇摆到平衡 +float LQR_K2 = 0; // +float LQR_K3 = 0; // -float LQR_K1_1 = 200; //平衡态 -float LQR_K2_1 = 15; // -float LQR_K3_1 = -0.15; // +float LQR_K1_1 = 0; //平衡态 +float LQR_K2_1 = 0; // +float LQR_K3_1 = 0; // //电机参数 @@ -67,19 +67,23 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22); //命令设置 Command comm; -double target_velocity = 0; -double target_angle = 89.9; -double target_voltage = 0; -void do_K1(char* cmd) { comm.scalar(&LQR_K1, cmd); } - +float target_velocity = 0; +float target_angle = 90; +float target_voltage = 0; +void do_K11(char* cmd) { comm.scalar(&LQR_K1, cmd); } +void do_K12(char* cmd) { comm.scalar(&LQR_K2, cmd); } +void do_K13(char* cmd) { comm.scalar(&LQR_K3, cmd); } +void do_K21(char* cmd) { comm.scalar(&LQR_K1_1, cmd); } +void do_K22(char* cmd) { comm.scalar(&LQR_K2_1, cmd); } +void do_K23(char* cmd) { comm.scalar(&LQR_K3_1, cmd); } +void do_TA(char* cmd) { comm.scalar(&target_angle, cmd); } void onPacketCallBack(AsyncUDPPacket packet) { char* da; da= (char*)(packet.data()); Serial.println(da); comm.run(da); - Serial.println(LQR_K1); - digitalWrite(22,LOW); + // target_velocity = atoi(); // Serial.print("数据内容: "); // Serial.println(target_velocity); @@ -90,8 +94,13 @@ void onPacketCallBack(AsyncUDPPacket packet) void setup() { Serial.begin(115200); //命令设置 - comm.add("K1",do_K1); - + comm.add("K11",do_K11); + comm.add("K12",do_K12); + comm.add("K13",do_K13); + comm.add("K21",do_K21); + comm.add("K22",do_K22); + comm.add("K23",do_K23); + comm.add("TA",do_TA); // 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 @@ -183,7 +192,7 @@ void setup() { Serial.println(F("Motor ready.")); Serial.println(F("Set the target velocity using serial terminal:")); - +// digitalWrite(22,LOW); } char buf[255]; @@ -218,12 +227,12 @@ void loop() { if (gyroZangle < -180 || gyroZangle > 180) gyroZangle = kalAngleZ; - - float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - target_angle) / 57.29578); + float pendulum_angle = constrainAngle(fmod(kalAngleZ,120)-target_angle); +// float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - target_angle) / 57.29578); #if FLAG_V - if (abs(pendulum_angle) < 0.6) // if angle small enough stabilize 0.5~30°,1.5~90° + if (abs(pendulum_angle) < 18) // if angle small enough stabilize 0.5~30°,1.5~90° { - target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate / 57.29578, motor.shaftVelocity()); + target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate, motor.shaftVelocity()); // limit the voltage set to the motor if (abs(target_voltage) > motor.voltage_limit * 0.7) target_voltage = _sign(target_voltage) * motor.voltage_limit * 0.7; @@ -240,13 +249,13 @@ void loop() { } else { - motor.move(lpf_throttle(-target_voltage)); + motor.move(lpf_throttle(target_voltage)); } #else motor.move(0); #endif -#if 1 +#if 0 //Serial.print(gyroZangle);Serial.print("\t"); Serial.print(kalAngleZ);Serial.print("\t"); @@ -255,7 +264,7 @@ Serial.print(kalAngleZ);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"); + Serial.print(pendulum_angle);Serial.print("\t"); Serial.print(gyroZrate);Serial.print("\t"); Serial.print("\r\n"); #endif @@ -264,7 +273,8 @@ Serial.print(kalAngleZ);Serial.print("\t"); if(wifi_flag) { memset(buf, 0, strlen(buf)); - wifi_print("p",pendulum_angle+target_angle); + wifi_print("v", motor.shaftVelocity()); + wifi_print("p",pendulum_angle); wifi_print("t",target_angle); wifi_print("k",kalAngleZ); wifi_print("g",gyroZrate); @@ -295,13 +305,18 @@ double acc2rotation(double x, double y) return (atan(x / y) / 1.570796 * 90); } } + // function constraining the angle in between -pi and pi, in degrees -180 and 180 float constrainAngle(float x) { - x = fmod(x + M_PI, _2PI); - if (x < 0) - x += _2PI; - return x - M_PI; + float a = 0; + if(x < 0) + { + a = 120+x; + if(a 0.05) + if (abs(p_angle) > 1.5) { last_unstable_time = millis(); stable = 0; @@ -346,6 +361,6 @@ void wifi_print(char * s,double num) strcpy(str,s); strcat(str, n); strcat(buf+strlen(buf), str); - strcat(buf, ","); + strcat(buf, ",\0"); } diff --git a/python_gui/gui/main.py b/python_gui/gui/main.py index a817ea4..e8748b3 100644 --- a/python_gui/gui/main.py +++ b/python_gui/gui/main.py @@ -28,7 +28,7 @@ class MyWindow(QMainWindow, Ui_MainWindow): def CreateSignalSlot(self): self.velocity_horizontalSlider.valueChanged.connect(self.velocity_horizontalSlider_valueChanged) self.wifi_config_pushButton.clicked.connect(self.wifi_config_pushButton_clicked) - + self.wifi_command_pushButton.clicked.connect(self.wifi_command_pushButton_clicked) # 设置实例 def CreateItems(self): # 定时器-绘图刷新 @@ -79,6 +79,9 @@ class MyWindow(QMainWindow, Ui_MainWindow): self.velocity_lineEdit.setText(str(self.target_velocity)) self.udp.send_message(str(self.target_velocity)) print(str(self.target_velocity)) + # command命令发送事件 + def wifi_command_pushButton_clicked(self): + self.udp.send_message(self.wifi_command_lineEdit.text()) def wifi_config_pushButton_clicked(self): try: # self.re_item = ['k','g','l','t'] @@ -113,11 +116,10 @@ class MyWindow(QMainWindow, Ui_MainWindow): # print(recv_data) for i, data in enumerate(recv_data): self.re_item.append(''.join(re.split(r'[^A-Za-z]', data))) - data = re.findall(r"\d+\.?\d*", data) - # print(i,data) + data = data.replace(self.re_item[i],'') self.signalDataArrays[i] = np.roll(self.signalDataArrays[i], -1) - self.signalDataArrays[i][-1] = data[0] + self.signalDataArrays[i][-1] = data pass def update_plot(self): if self.wifi_recv_flag: diff --git a/python_gui/gui/main_ui.py b/python_gui/gui/main_ui.py index f46ab51..4e32497 100644 --- a/python_gui/gui/main_ui.py +++ b/python_gui/gui/main_ui.py @@ -80,6 +80,21 @@ class Ui_MainWindow(object): self.tool_layout = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget_4) self.tool_layout.setContentsMargins(0, 0, 0, 0) self.tool_layout.setObjectName("tool_layout") + self.groupBox_3 = QtWidgets.QGroupBox(self.centralwidget) + self.groupBox_3.setGeometry(QtCore.QRect(10, 20, 291, 111)) + self.groupBox_3.setObjectName("groupBox_3") + self.horizontalLayoutWidget_5 = QtWidgets.QWidget(self.groupBox_3) + self.horizontalLayoutWidget_5.setGeometry(QtCore.QRect(10, 20, 271, 31)) + self.horizontalLayoutWidget_5.setObjectName("horizontalLayoutWidget_5") + self.horizontalLayout_4 = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget_5) + self.horizontalLayout_4.setContentsMargins(0, 0, 0, 0) + self.horizontalLayout_4.setObjectName("horizontalLayout_4") + self.wifi_command_lineEdit = QtWidgets.QLineEdit(self.horizontalLayoutWidget_5) + self.wifi_command_lineEdit.setObjectName("wifi_command_lineEdit") + self.horizontalLayout_4.addWidget(self.wifi_command_lineEdit) + self.wifi_command_pushButton = QtWidgets.QPushButton(self.horizontalLayoutWidget_5) + self.wifi_command_pushButton.setObjectName("wifi_command_pushButton") + self.horizontalLayout_4.addWidget(self.wifi_command_pushButton) MainWindow.setCentralWidget(self.centralwidget) self.menubar = QtWidgets.QMenuBar(MainWindow) self.menubar.setGeometry(QtCore.QRect(0, 0, 1187, 26)) @@ -103,3 +118,5 @@ class Ui_MainWindow(object): self.label_2.setText(_translate("MainWindow", "目标角度：")) self.angle_lineEdit.setText(_translate("MainWindow", "149")) self.groupBox_2.setTitle(_translate("MainWindow", "GroupBox")) + self.groupBox_3.setTitle(_translate("MainWindow", "command命令")) + self.wifi_command_pushButton.setText(_translate("MainWindow", "发送")) diff --git a/python_gui/gui/main_ui.ui b/python_gui/gui/main_ui.ui index 5a2a6c2..7626a06 100644 --- a/python_gui/gui/main_ui.ui +++ b/python_gui/gui/main_ui.ui @@ -168,6 +168,41 @@ + + + + 10 + 20 + 291 + 111 + + + + command命令 + + + + + 10 + 20 + 271 + 31 + + + + + + + + + + 发送 + + + + + + diff --git a/python_gui/gui/test2.py b/python_gui/gui/test2.py index 39db4ed..52f4c24 100644 --- a/python_gui/gui/test2.py +++ b/python_gui/gui/test2.py @@ -1,5 +1,6 @@ -a = [1,2] -b = [4,5,6,4] -c = [7,8,9,10] -for i,j,k in zip(a,b,c): - print(i,j,k) \ No newline at end of file +import re +a = 'v0.10' +b =''.join(re.split(r'[^A-Za-z]', a)) +print(b) +a = a.replace(b,'') +print(a) \ No newline at end of file diff --git a/python_gui/gui/udp_test1.py b/python_gui/gui/udp_test1.py index e598b78..af8aff5 100644 --- a/python_gui/gui/udp_test1.py +++ b/python_gui/gui/udp_test1.py @@ -16,7 +16,7 @@ def udp_recv(udp_socket): recv_data = udp_socket.recv(1024) recv_data = recv_data.decode('utf-8') udp_data = recv_data - # print('收到信息为:%s'%recv_data) + print('收到信息为:%s'%recv_data) def main(): udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) #创建套接字