Commit 985906a0 authored by Wouter Klop's avatar Wouter Klop

Some initial work on system identification in the frequency domain

parent ac14e145
This source diff could not be displayed because it is too large. You can view the blob instead.
c1p0.6500,c1i0.0000,c1d0.0750,c1n15.0000,c1m20.0000,c1o20.0000,c2p1.0000,c2i0.0000,c2d1.2000,c2n20.0000,c2m35.0000,c2o35.0000,c3p6.0000,c3i5.0000,c3d0.0000,c3n20.0000,c3m35.0000,c3o35.0000,h0.9000,i0.9000,v150,j1.0000,n0.9960,l3000.0000,wm1,wsFlopnet,192.899551,-111.630898,-112.226463,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
192.919403,-111.883141,-112.228622,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
192.939407,-111.777107,-112.228477,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
192.959412,-111.576424,-112.229446,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
192.979416,-111.611176,-112.227173,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
192.999420,-111.871773,-112.227959,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.019424,-111.252335,-112.226547,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.039444,-111.811935,-112.228279,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.059433,-111.249069,-112.226883,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.079453,-111.815804,-112.227623,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.099457,-111.826866,-112.225998,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.119461,-111.456139,-112.226334,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.139465,-111.900848,-112.225616,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.159485,-111.793083,-112.227318,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.179489,-111.443535,-112.226151,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.199493,-111.908592,-112.228210,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.219498,-111.627380,-112.228462,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.239517,-111.507462,-112.225601,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.259521,-111.881783,-112.224174,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.279526,-111.534561,-112.223282,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.299530,-111.453102,-112.219391,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.319534,-111.389114,-112.217232,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.339539,-111.809631,-112.216042,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.359543,-111.783127,-112.215729,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.379547,-111.722443,-112.218872,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.399567,-111.596008,-112.219116,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.419571,-112.017700,-112.218544,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.439575,-111.435730,-112.216545,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.459579,-111.656914,-112.216980,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.479584,-111.234985,-112.219315,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.499588,-111.726906,-112.218338,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.519608,-111.802704,-112.214958,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.539612,-111.323723,-112.210800,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.559631,-111.700905,-112.209167,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.579620,-111.864494,-112.206825,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.599625,-111.201202,-112.206703,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.619629,-111.733971,-112.205948,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.639648,-112.077400,-112.207962,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.659653,-111.689743,-112.209732,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.679657,-111.932961,-112.210487,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.699661,-111.974945,-112.211510,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.719666,-111.465584,-112.211609,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.739685,-111.685410,-112.212494,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.759689,-111.313751,-112.211479,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.779694,-111.794716,-112.211510,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.799698,-111.259766,-112.210060,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.819702,-111.337189,-112.208527,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.839706,-111.500008,-112.208092,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.859711,-111.598518,-112.207619,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.879715,-111.653511,-112.207611,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.899734,-111.480148,-112.207047,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.919739,-111.458397,-112.205971,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
193.939743,-111.882233,-112.206490,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
clear all; close all; clc;
set(0,'DefaultAxesXGrid','on')
dataPath = 'data/';
fieldNames = {'accAngle','filterAngle',...
'setpoint1','input1','output1',...
'setpoint2','input2','output2',...
'setpoint3','input3','output3'};
%% Find all .csv files
files = dir([dataPath, '*.csv']);
fileNames = {files.name}.';
%% Read .csv file
file = fileNames{1};
dRaw = csvread([dataPath, file], 1, 0);
t = dRaw(:,1) - dRaw(1,1);
d = cell2struct(num2cell(dRaw(:,2:end),1), fieldNames,2);
dT = mean(diff(t));
%% Plot some data
figure(1)
clf
subplot(221)
plot(t, d.accAngle)
hold on
plot(t, d.filterAngle)
plot(t, d.setpoint1, 'g--')
hold off
subplot(222)
plot(t, d.output1)
%% Calculate some FFTs
X = fft(d.filterAngle);
Y = fft(d.output1);
l = floor(length(X)/2)*2;
X = X(1:l/2+1);
Y = Y(1:l/2+1);
f = (1/dT)*(1:l/2+1)/l;
figure(2)
clf
subplot(211)
loglog(f, abs(X))
hold on
loglog(f, abs(Y))
hold off
subplot(212)
semilogx(f, angle(X)*180/pi)
hold on
semilogx(f, angle(Y)*180/pi)
hold off
xlabel('Frequency [Hz]')
%% Run FRF
[txy, f] = tfestimate(d.output1,d.filterAngle);
figure(2)
clf
subplot(211)
loglog(abs(txy))
\ No newline at end of file
......@@ -60,6 +60,7 @@ void readSensor();
void initSensor(uint8_t n);
void setMicroStep(uint8_t uStep);
void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length);
void sendConfigurationData(uint8_t num);
void IRAM_ATTR motLeftTimerFunction();
void IRAM_ATTR motRightTimerFunction();
......@@ -113,7 +114,7 @@ float accAngle = 0;
float filterAngle = 0;
float angleOffset = 2.0;
float gyroFilterConstant = 0.996;
float gyroGain = 1.1;
float gyroGain = 1.0;
// -- Others
#define ledPin 2
......@@ -446,7 +447,7 @@ void loop() {
} else {
pidAngle.input = filterAngle;
}
pidAngleOutput = pidAngle.calculate();
avgMotSpeedSum += pidAngleOutput/2;
......@@ -811,74 +812,12 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length
case WStype_CONNECTED: {
IPAddress ip = wsServer.remoteIP(num);
Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
// send message to client
char wBuf[63];
char buf[63];
sprintf(wBuf, "c%dp%5.2f", 1, pidAngle.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%5.2f", 1, pidAngle.Ti);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dd%5.2f", 1, pidAngle.Td);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dn%5.2f", 1, pidAngle.N);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dm%5.1f", 1, pidAngle.maxOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%do%5.1f", 1, -pidAngle.minOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dp%5.2f", 2, pidPos.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%5.2f", 2, pidPos.Ti);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dd%5.2f", 2, pidPos.Td);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dn%5.2f", 2, pidPos.N);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dm%5.1f", 2, pidPos.maxOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%do%5.1f", 2, -pidPos.minOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dp%5.2f", 3, pidSpeed.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%5.2f", 3, pidSpeed.Ti);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dd%5.2f", 3, pidSpeed.Td);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dn%5.2f", 3, pidSpeed.N);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dm%5.1f", 3, pidSpeed.maxOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%do%5.1f", 3, -pidSpeed.minOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "h%4.2f", speedFilterConstant);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "i%4.2f", steerFilterConstant);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "v%d", motorCurrent);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "j%4.2f", gyroGain);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "n%5.3f", gyroFilterConstant);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "l%5.0f", maxStepSpeed);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "wm%d", preferences.getUInt("wifi_mode", 0)); // 0=AP, 1=Client
wsServer.sendTXT(num, wBuf);
preferences.getBytes("wifi_ssid", buf, 63);
sprintf(wBuf, "ws%s", buf);
wsServer.sendTXT(num, wBuf);
sendConfigurationData(num);
}
break;
case WStype_TEXT:
Serial.printf("[%u] get Text: %s\n", num, payload);
parseCommand((char*) payload, length);
// send message to client
// webSocket.sendTXT(num, "message here");
// send data to all connected clients
// webSocket.broadcastTXT("message here");
break;
case WStype_BIN: {
// Serial.printf("[%u] get binary length: %u\n", num, length);
......@@ -923,3 +862,62 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length
}
}
void sendConfigurationData(uint8_t num) {
// send message to client
char wBuf[63];
char buf[63];
sprintf(wBuf, "c%dp%.4f", 1, pidAngle.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%.4f", 1, pidAngle.Ti);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dd%.4f", 1, pidAngle.Td);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dn%.4f", 1, pidAngle.N);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dm%.4f", 1, pidAngle.maxOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%do%.4f", 1, -pidAngle.minOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dp%.4f", 2, pidPos.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%.4f", 2, pidPos.Ti);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dd%.4f", 2, pidPos.Td);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dn%.4f", 2, pidPos.N);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dm%.4f", 2, pidPos.maxOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%do%.4f", 2, -pidPos.minOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dp%.4f", 3, pidSpeed.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%.4f", 3, pidSpeed.Ti);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dd%.4f", 3, pidSpeed.Td);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dn%.4f", 3, pidSpeed.N);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dm%.4f", 3, pidSpeed.maxOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%do%.4f", 3, -pidSpeed.minOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "h%.4f", speedFilterConstant);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "i%.4f", steerFilterConstant);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "v%d", motorCurrent);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "j%.4f", gyroGain);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "n%.4f", gyroFilterConstant);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "l%.4f", maxStepSpeed);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "wm%d", preferences.getUInt("wifi_mode", 0)); // 0=AP, 1=Client
wsServer.sendTXT(num, wBuf);
preferences.getBytes("wifi_ssid", buf, 63);
sprintf(wBuf, "ws%s", buf);
wsServer.sendTXT(num, wBuf);
}
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment