master
45coll 2021-09-15 14:02:01 +08:00
parent 07742040a1
commit 73a0223a69
3 changed files with 57 additions and 41 deletions

View File

@ -36,10 +36,11 @@ float constrainAngle(float x);
const char *ssid = "esp32"; const char *ssid = "esp32";
const char *password = "12345678"; const char *password = "12345678";
#define WIFI_FLAG 1
AsyncUDP udp; //创建UDP对象 AsyncUDP udp; //创建UDP对象
unsigned int localUdpPort = 2333; //本地端口号 unsigned int localUdpPort = 2333; //本地端口号
unsigned int broadcastPort = localUdpPort; unsigned int broadcastPort = localUdpPort;
void wifi_print(char * s,double num);
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
float PID_P = 1; // 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}; LowPassFilter lpf_throttle{0.00};
#define FLAG_V 1 #define FLAG_V 1
//倒立摆参数 //倒立摆参数
float LQR_K1 = 600; //摇摆到平衡 float LQR_K1 = 400; //摇摆到平衡
float LQR_K2 = 40; // float LQR_K2 = 40; //
float LQR_K3 = 0.30; // float LQR_K3 = 0.30; //
@ -65,7 +66,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
//命令设置 //命令设置
double target_velocity = 0; double target_velocity = 0;
double target_angle = 31; double target_angle = 88;
double target_voltage = 0; double target_voltage = 0;
void onPacketCallBack(AsyncUDPPacket packet) void onPacketCallBack(AsyncUDPPacket packet)
{ {
@ -75,10 +76,6 @@ void onPacketCallBack(AsyncUDPPacket packet)
// packet.print("reply data"); // packet.print("reply data");
} }
// instantiate the commander // 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() { void setup() {
Serial.begin(115200); Serial.begin(115200);
@ -172,21 +169,18 @@ void setup() {
//初始化 FOC //初始化 FOC
motor.initFOC(); 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("Motor ready."));
Serial.println(F("Set the target velocity using serial terminal:")); Serial.println(F("Set the target velocity using serial terminal:"));
} }
char s[255]; char buf[255];
int t_v; int t_v;
int lim_v = 60; int lim_v = 60;
long loop_count = 0; long loop_count = 0;
void loop() { void loop() {
motor.loopFOC(); motor.loopFOC();
if (loop_count++ == 5) if (1)
{ {
while (i2cRead(0x3B, i2cData, 14)); while (i2cRead(0x3B, i2cData, 14));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
@ -211,19 +205,25 @@ void loop() {
if (gyroZangle < -180 || gyroZangle > 180) if (gyroZangle < -180 || gyroZangle > 180)
gyroZangle = kalAngleZ; gyroZangle = kalAngleZ;
sprintf(s, "%.2f",kalAngleZ); //将100转为16进制表示的字符串
float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - target_angle) / 57.29578); float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - target_angle) / 57.29578);
#if FLAG_V #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()); target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate / 57.29578, motor.shaftVelocity());
// limit the voltage set to the motor // limit the voltage set to the motor
if (abs(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.9; target_voltage = _sign(target_voltage) * motor.voltage_limit * 0.7;
} }
else // else do swing-up else // else do swing-up
{ // sets 1.5V to the motor in order to 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 // set the target voltage to the motor
@ -235,7 +235,7 @@ void loop() {
{ {
motor.move(lpf_throttle(target_voltage)); motor.move(lpf_throttle(target_voltage));
} }
Serial.print(target_voltage);Serial.print("\t");
#else #else
if (abs(pendulum_angle) < 0.6) // 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°
{ {
@ -258,9 +258,11 @@ void loop() {
{ {
motor.move(lpf_throttle(target_velocity)); motor.move(lpf_throttle(target_velocity));
} }
Serial.print(target_velocity);Serial.print("\t");
#endif #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(motor.shaft_velocity);Serial.print("\t");
Serial.print(target_angle);Serial.print("\t"); Serial.print(target_angle);Serial.print("\t");
Serial.print(pendulum_angle+target_angle);Serial.print("\t"); Serial.print(pendulum_angle+target_angle);Serial.print("\t");
@ -269,11 +271,17 @@ void loop() {
#endif #endif
// motor.move(target_velocity); // 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())); //计算广播地址 IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址
udp.writeTo((const unsigned char*)s, strlen(s), broadcastAddr, localUdpPort); //广播数据 udp.writeTo((const unsigned char*)buf, strlen(buf), broadcastAddr, localUdpPort); //广播数据
loop_count = 0;
} }
command.run(); #endif
} }
/* mpu6050加速度转换为角度 /* mpu6050加速度转换为角度
acc2rotation(ax, ay) acc2rotation(ax, ay)
@ -336,3 +344,13 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
return u; 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, ",");
}

View File

@ -105,18 +105,23 @@ class MyWindow(QMainWindow, Ui_MainWindow):
while self.close_flag: while self.close_flag:
recv_data = self.udp.udpClientSocket.recv(1024) recv_data = self.udp.udpClientSocket.recv(1024)
recv_data = recv_data.decode('utf-8') 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): def update_plot(self):
if self.wifi_recv_flag: pass
self.signalDataArrays[0] = np.roll(self.signalDataArrays[0], -1) # if self.wifi_recv_flag:
self.signalDataArrays[0][-1] = self.udp_data # for i, plotFlag in enumerate(self.signalPlotFlags):
self.signalDataArrays[1] = np.roll(self.signalDataArrays[1], -1) # self.signalPlots[i].setData(self.timeArray, self.signalDataArrays[i])
self.signalDataArrays[1][-1] = self.angle_lineEdit.text() # self.signalPlots[i].updateItems()
# self.curve.setData(self.timeArray, self.v_list[0]) # 在绘图部件中绘制折线图 # self.signalPlots[i].sigPlotChanged.emit(self.signalPlots[i])
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: def closeEvent(self, a0: QtGui.QCloseEvent) -> None:
self.close_flag = 0 self.close_flag = 0

View File

@ -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()解码收到的字节