mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-04-28 10:16:44 +00:00
376 lines
10 KiB
Matlab
376 lines
10 KiB
Matlab
global SIMULATION;
|
|
SIMULATION = 0;
|
|
|
|
% Paramètres de lecture
|
|
DIRNAME = "/home/geoffrey/CdF/cdf2018-principal/log/";
|
|
FILENAME = "000655.csv";
|
|
PATH = DIRNAME + FILENAME;
|
|
|
|
% Paramètres de simulation
|
|
global SIMULATION_TIME SIMULATION_DT;
|
|
SIMULATION_TIME = 10;
|
|
SIMULATION_DT = 1e-15;
|
|
|
|
%BEGIN DIMENSIONS
|
|
|
|
% Dimensions pistes
|
|
global pisteWidth pisteHeight pisteOrigX pisteOrigY;
|
|
pisteWidth = 3000.0;
|
|
pisteHeight = 2000.0;
|
|
pisteOrigX = 0.0;
|
|
pisteOrigY = 0.0;
|
|
|
|
% Dimensions robot
|
|
global width height distanceBetweenWheels wheelDiameter wheelPerimeter motorSpeedGainRPMpV motorSpeedGain motorNominalTension motorControllerAlimentation motorControllerReference motorSaturationMin motorSaturationMax pwmMax coderResolution coderDataFactor coderDataResolution cranReducOut cranReducIn reducRatio coderFullResolution avPerCycle;
|
|
width = 250.0; % mm (from meca)
|
|
height = 100.0; % mm (from random)
|
|
distanceBetweenWheels = width; % mm (from meca)
|
|
wheelDiameter = 80.0; % mm (from meca)
|
|
wheelPerimeter = (wheelDiameter * pi); % mm
|
|
motorSpeedGainRPMpV = 233.0; % rpm/V (from datasheet)
|
|
motorSpeedGain = (motorSpeedGainRPMpV / 60.0); % motor rev/s/V
|
|
motorNominalTension = 24.0; % V (from datasheet)
|
|
motorControllerAlimentation = 24.0; % V (from elec)
|
|
motorControllerReference = 5; % V (from wiring)
|
|
motorSaturationMin = 0; % V (from random)
|
|
motorSaturationMax = 3.0; % V (from testing)
|
|
pwmMax = 3.3; % V (from FPGA datasheet)
|
|
coderResolution = 370.0; % cycles/motor rev
|
|
coderDataFactor = 4.0; % increments/motor cycles
|
|
coderDataResolution = (coderResolution * coderDataFactor); % cycles/motor rev
|
|
cranReducOut = 48.0; % nb crans (from meca)
|
|
cranReducIn = 12.0; % nb crans (from meca)
|
|
reducRatio = (cranReducIn / cranReducOut); % reduction ratio
|
|
coderFullResolution = (coderDataResolution / reducRatio); % cycles / wheel rev
|
|
avPerCycle = (wheelPerimeter / coderFullResolution); % mm
|
|
|
|
% Constantes asservissement
|
|
global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oGain oEcartMax;
|
|
dDirEcartMin = 7.0; % mm
|
|
dDirEcartMax = 10.0; % mm
|
|
oDirEcartMin = (5.0 / 360.0 * 2.0 * pi); % rad
|
|
oDirEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad
|
|
oEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad
|
|
oGain = 5.0;
|
|
P = 5.0;
|
|
I = 0.0;
|
|
D = 0.0;
|
|
|
|
%END DIMENSIONS
|
|
|
|
global s;
|
|
if SIMULATION == 1
|
|
% Génération de la consigne
|
|
xinit = 50;
|
|
yinit = 50;
|
|
oinit = 4 * pi;
|
|
d1t = 2;
|
|
d1x = -300;
|
|
d1y = 0;
|
|
d1o = 2 * pi;
|
|
dt = SIMULATION_DT;
|
|
|
|
global xcons ycons ocons;
|
|
xcons = timeseries([xinit, xinit, d1x, d1x], [0 d1t-dt d1t SIMULATION_TIME]);
|
|
ycons = timeseries([yinit, yinit, d1y, d1y], [0 d1t-dt d1t SIMULATION_TIME]);
|
|
ocons = timeseries([oinit, oinit, d1o, d1o], [0 d1t-dt d1t SIMULATION_TIME]);
|
|
|
|
% Simulation
|
|
disp("Lancement de la simulation");
|
|
s = sim("modelisation", "StopTime", string(SIMULATION_TIME), "MinStep", string(SIMULATION_DT));
|
|
fprintf("Simulation sampling rate: %f Hz\n", length(s.tout)/SIMULATION_TIME);
|
|
else
|
|
disp("Ouverture des données");
|
|
T = readtable(PATH);
|
|
|
|
% Données pratiques → données théoriques
|
|
T.time(1) = 0;
|
|
T.x = T.xConnu;
|
|
T.y = T.yConnu;
|
|
T.o = T.oConnu;
|
|
|
|
disp("Enregistrement des données");
|
|
s = containers.Map;
|
|
for name=T.Properties.VariableNames
|
|
nameChar = char(name);
|
|
s(nameChar) = timeseries(T.(nameChar), T.time);
|
|
end
|
|
|
|
% Modification pour faire passer comme une simu
|
|
td = getTimePoints();
|
|
SIMULATION_TIME = td(end);
|
|
|
|
end
|
|
|
|
|
|
% Graphes
|
|
|
|
clf
|
|
global p;
|
|
|
|
% Évolution spatiale
|
|
subplot(2, 2, 1);
|
|
initGraph
|
|
updateToTime(SIMULATION_DT);
|
|
|
|
% Roues
|
|
p = subplot(2, 2, 2);
|
|
hold on;
|
|
timeGraph(["lVolt", "rVolt", "lErr", "rErr"]);
|
|
addLimitline(p, -motorSaturationMin);
|
|
addLimitline(p, -motorSaturationMax);
|
|
addLimitline(p, motorSaturationMin);
|
|
addLimitline(p, motorSaturationMax);
|
|
addLimitline(p, 0);
|
|
title("Roues");
|
|
xlabel("Temps (s)");
|
|
ylabel("Tension (V)");
|
|
legend("Gauche", "Droite", "Err. gauche", "Err. droite");
|
|
|
|
% Distance
|
|
p = subplot(2, 2, 3);
|
|
hold on;
|
|
%timeGraph(["dDirEcart", "dErr"]);
|
|
timeGraph(["dDirEcart", "dErr", "secFront", "secBack"]);
|
|
addLimitline(p, dDirEcartMin);
|
|
addLimitline(p, dDirEcartMax);
|
|
title("Distance");
|
|
xlabel("Temps (s)");
|
|
ylabel("Distance (mm)");
|
|
%legend("Ecart direction", "Err. retenue");
|
|
legend("Ecart direction", "Err. retenue", "Distance avant", "Distance arrière");
|
|
|
|
% Rotation
|
|
p = subplot(2, 2, 4);
|
|
hold on;
|
|
timeGraph(["oDirEcart", "oEcart", "oErr"]);
|
|
addLimitline(p, oDirEcartMax);
|
|
addLimitline(p, oDirEcartMin);
|
|
addLimitline(p, -oDirEcartMax);
|
|
addLimitline(p, -oDirEcartMin);
|
|
title("Rotation");
|
|
xlabel("Temps (s)");
|
|
ylabel("Angle (rad)");
|
|
legend("Ecart direction", "Ecart orientation", "Err. retenue");
|
|
|
|
% Fonctions
|
|
|
|
function ts = getTS(name)
|
|
global SIMULATION s;
|
|
if SIMULATION == 1
|
|
ts = s.(name);
|
|
else
|
|
name = char(name);
|
|
if s.isKey(name)
|
|
ts = s(name);
|
|
else
|
|
fprintf("Données inconnues : %s\n", name);
|
|
ts = timeseries();
|
|
end
|
|
end
|
|
end
|
|
|
|
function pt = getTimePoints()
|
|
global SIMULATION s;
|
|
if SIMULATION == 1
|
|
pt = s.tout;
|
|
else
|
|
ts = getTS('time');
|
|
pt = ts.Time;
|
|
end
|
|
end
|
|
|
|
function timeGraph(series)
|
|
global SIMULATION_TIME p;
|
|
m = inf;
|
|
M = -inf;
|
|
for sname=series
|
|
serie = getTS(sname);
|
|
plot(serie);
|
|
if ~isempty(serie.Data)
|
|
m = min(m, min(serie));
|
|
end
|
|
if ~isempty(serie.Data)
|
|
M = max(M, max(serie));
|
|
end
|
|
end
|
|
xlim([0 SIMULATION_TIME]);
|
|
if (abs(m) ~= inf) && (abs(M) ~= inf)
|
|
ylim([m M+1E-9]);
|
|
end
|
|
addTimeline(p);
|
|
end
|
|
|
|
function addLimitline(p, x)
|
|
line(p.XLim, [x x], 'Color', [0.8 0.8 0.8]);
|
|
end
|
|
|
|
function addTimeline(p)
|
|
global t timelines;
|
|
timeline = line([t t], p.YLim, 'Color', [0 0 0]);
|
|
timelines = [timelines timeline];
|
|
end
|
|
|
|
function play()
|
|
global SIMULATION_TIME speed t playing;
|
|
if playing == 1
|
|
return
|
|
end
|
|
startCpu=cputime;
|
|
startT=t;
|
|
n=0;
|
|
playing=1;
|
|
while t<SIMULATION_TIME && playing == 1
|
|
updateToTime((cputime-startCpu)*speed + startT);
|
|
drawnow limitrate;
|
|
n = n + 1;
|
|
end
|
|
playing=0;
|
|
fprintf("Refresh rate : %f Hz\n", n/(cputime-startCpu));
|
|
end
|
|
|
|
function sliderCallback(hObject, ~)
|
|
updateToTime(get(hObject, 'Value'));
|
|
end
|
|
|
|
function playCallback(~, ~)
|
|
play();
|
|
end
|
|
|
|
function pauseCallback(~, ~)
|
|
global playing;
|
|
playing=0;
|
|
end
|
|
|
|
function [x, y] = pointArround(xC, yC, xD, yD, o)
|
|
D = xD + yD * 1i;
|
|
F = abs(D) .* exp(1i * (angle(D) + o - pi/2));
|
|
x = xC + real(F);
|
|
y = yC + imag(F);
|
|
end
|
|
|
|
function drawRect(p, x, y, o, w, h)
|
|
[x1, y1] = pointArround(x, y, - w/2, + h/2, o);
|
|
[x2, y2] = pointArround(x, y, + w/2, + h/2, o);
|
|
[x3, y3] = pointArround(x, y, + w/2, - h/2, o);
|
|
[x4, y4] = pointArround(x, y, - w/2, - h/2, o);
|
|
p.XData = [x1, x2, x3, x4, x1];
|
|
p.YData = [y1, y2, y3, y4, y1];
|
|
end
|
|
|
|
function initGraph()
|
|
cla;
|
|
global SIMULATION_TIME;
|
|
global t speed playing timelines;
|
|
t = 0;
|
|
speed = 1;
|
|
playing = 0;
|
|
timelines = [];
|
|
global timeSlider;
|
|
timeSlider = uicontrol('Style', 'slider', 'Callback', @sliderCallback, 'Min', 0, 'Max', SIMULATION_TIME, 'Position', [20 20 500 20], 'Value', t);
|
|
global timeText;
|
|
timeText = uicontrol('Style', 'text', 'Position', [520 20 120 20], 'String', sprintf("t = %f", t));
|
|
uicontrol('Style', 'pushbutton', 'String', 'Play', 'Position', [640 20 60 20], 'Callback', @playCallback);
|
|
uicontrol('Style', 'pushbutton', 'String', 'Pause', 'Position', [700 20 60 20], 'Callback', @pauseCallback);
|
|
|
|
hold on;
|
|
xTs = getTS('x');
|
|
yTs = getTS('y');
|
|
plot(xTs.Data, yTs.Data, 'b--');
|
|
global height;
|
|
global lQuiver;
|
|
lQuiver = quiver(0, 0, 0, 0, 'Color', 'Red', 'MaxHeadSize', height/4);
|
|
global rQuiver;
|
|
rQuiver = quiver(0, 0, 0, 0, 'Color', 'Red', 'MaxHeadSize', height/4);
|
|
global robotRect;
|
|
robotRect = plot(0, 0);
|
|
global robotPath;
|
|
robotPath = plot(0, 0, 'b');
|
|
global dirQuiver;
|
|
dirQuiver = quiver(0, 0, 0, 0, 'Color', 'Red', 'MaxHeadSize', height/4);
|
|
global consQuiver;
|
|
consQuiver = quiver(0, 0, 0, 0, 'Color', 'Green', 'MaxHeadSize', height/4);
|
|
|
|
% Draw track
|
|
global pisteWidth;
|
|
global pisteHeight;
|
|
global pisteOrigX;
|
|
global pisteOrigY;
|
|
rectangle('Position', [pisteOrigX pisteOrigY pisteWidth pisteHeight]);
|
|
|
|
% Set limits
|
|
margin = 300;
|
|
ampl = max(xTs.max - xTs.min, yTs.max - yTs.min);
|
|
xlim([xTs.min - margin, xTs.min + ampl + margin]);
|
|
ylim([yTs.min - margin, yTs.min + ampl + margin]);
|
|
title("Évolution spatiale");
|
|
xlabel("X (mm)");
|
|
ylabel("Y (mm)");
|
|
end
|
|
|
|
function d = getTSData(name, i)
|
|
ts = getTS(name);
|
|
if isempty(ts.Data)
|
|
d = 0;
|
|
else
|
|
d = ts.Data(i);
|
|
end
|
|
end
|
|
|
|
function updateToTime(newT)
|
|
% Update ui
|
|
global timeSlider timeText t SIMULATION_TIME;
|
|
t = max(min(SIMULATION_TIME, newT), 0);
|
|
timeSlider.Value = t;
|
|
timeText.String = sprintf("t = %f", t);
|
|
|
|
% Get values
|
|
i = find(getTimePoints() <= t);
|
|
i = i(end);
|
|
x = getTSData('x', i);
|
|
y = getTSData('y', i);
|
|
o = getTSData('o', i);
|
|
xCons = getTSData('xCons', i);
|
|
yCons = getTSData('yCons', i);
|
|
oCons = getTSData('oCons', i);
|
|
lVit = getTSData('lVit', i);
|
|
rVit = getTSData('rVit', i);
|
|
|
|
|
|
% Add event
|
|
tEvent = tsdata.event('tEvent', t);
|
|
xTs = getTS('x');
|
|
xS = xTs.addevent(tEvent);
|
|
yTs = getTS('y');
|
|
yS = yTs.addevent(tEvent);
|
|
|
|
% Draw path
|
|
global robotPath;
|
|
robotPath.XData = xS.gettsbeforeevent('tEvent').Data;
|
|
robotPath.YData = yS.gettsbeforeevent('tEvent').Data;
|
|
|
|
% Draw robot
|
|
global width height;
|
|
global robotRect dirQuiver;
|
|
drawRect(robotRect, x, y, o, width, height);
|
|
dirQuiver.XData = x; dirQuiver.YData = y;
|
|
dirQuiver.UData = cos(o) * height/2; dirQuiver.VData = sin(o) * height/2;
|
|
% Arrow for wheels
|
|
global lQuiver rQuiver;
|
|
[lQuiver.XData, lQuiver.YData] = pointArround(x, y, -width/2, 0, o);
|
|
lQuiver.UData = cos(o) * lVit; lQuiver.VData = sin(o) * lVit;
|
|
[rQuiver.XData, rQuiver.YData] = pointArround(x, y, +width/2, 0, o);
|
|
rQuiver.UData = cos(o) * rVit; rQuiver.VData = sin(o) * rVit;
|
|
|
|
% Draw cons
|
|
global consQuiver;
|
|
consQuiver.XData = xCons; consQuiver.YData = yCons;
|
|
consQuiver.UData = cos(oCons) * height/2 ; consQuiver.VData = sin(oCons) * height/2;
|
|
|
|
% Draw timelines
|
|
global timelines
|
|
for i = 1:length(timelines)
|
|
timelines(i).XData = [t t];
|
|
end
|
|
end
|