geoffrey/cdf2018-principal
geoffrey/cdf2018-principal
1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-11-14 12:26:06 +01:00
Code Issues Releases Wiki Activity

Added simulation

Browse source
This commit is contained in:
Geoffrey Frogeye 2018-05-06 18:34:56 +02:00
parent 6817aaf779
commit c9a7bee3b1
6 changed files with 459 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
raspberrypi/Makefile
View file

@ -109,6 +109,7 @@ upgrade-fpga:
# CHEF # CHEF
chef: chef:
../simu/mat2h.sh
make -C buildroot chef-rebuild make -C buildroot chef-rebuild
upgrade-chef: chef upgrade-chef: chef

2
simu/.gitignore vendored Normal file
View file

@ -0,0 +1,2 @@
*.mat
slprj/*

68
simu/mat2h.py Executable file
View file

@ -0,0 +1,68 @@
#!/usr/bin/env python3
import sys
inDimensions = False
for line in sys.stdin:
l = line.rstrip('\n')
if inDimensions:
if l == '%END DIMENSIONS':
inDimensions = False
continue
if l.startswith('global'):
continue
if l.startswith('%'):
print(l.replace('%', '//'))
continue
if l.strip() == "":
print()
continue
out = "#define "
step = 0;
wasUpper = l[0].isupper()
for c in l:
if step == 0:
if c.isalpha():
if c.isupper() and not wasUpper:
out += '_' + c
else:
out += c.upper()
wasUpper = c.isupper()
else:
step = 1
elif step == 1:
if c == '=':
step = 2
elif step == 2:
if c.isalpha():
if c.isupper() and not wasUpper:
out += '_' + c
else:
out += c.upper()
wasUpper = c.isupper()
else:
if c == ';':
step = 3
else:
out += c
elif step == 3:
if c == '%':
step = 4
out += '//'
else:
out += c
elif step == 4:
out += c
print(out.replace('PI', 'M_PI'))
else:
if l == '%BEGIN DIMENSIONS':
inDimensions = True
continue

15
simu/mat2h.sh Executable file
View file

@ -0,0 +1,15 @@
#!/usr/bin/env bash
cd "$( dirname "${BASH_SOURCE[0]}" )"
FILE="../chef/src/dimensions.h"
echo -e "#ifndef __DIMENSIONS_H__
#define __DIMENSIONS_H__
#include <math.h>" > $FILE
cat simu.m | ./mat2h.py >> $FILE
echo -e "#endif" >> $FILE

BIN
simu/modelisation.slx Normal file
View file

Binary file not shown.

373
simu/simu.m Normal file
View file

@ -0,0 +1,373 @@
global SIMULATION;
SIMULATION = 0;
% Paramètres de lecture
DIRNAME = "/home/geoffrey/CdF/cdf2018-principal/log/";
FILENAME = "000232.csv";
PATH = DIRNAME + FILENAME;
% Paramètres de simulation
global SIMULATION_TIME SIMULATION_DT;
SIMULATION_TIME = 10;
SIMULATION_DT = 1e-6;
%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.1; % V (from random)
motorSaturationMax = 12.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;
dDirEcartMin = 1.0; % mm
dDirEcartMax = 5.0; % mm
oDirEcartMin = (2.5 / 360.0 * 2.0 * pi); % rad
oDirEcartMax = (7.5 / 360.0 * 2.0 * pi); % rad
oGain = 1;
P = 2;
I = 0;
D = 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 = -300;
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;
%T.lVolt = T.lCodTot;
%T.rVolt = T.rCodTot;
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, motorNominalTension);
addLimitline(p, -motorNominalTension);
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"]);
addLimitline(p, dDirEcartMin);
addLimitline(p, dDirEcartMax);
title("Distance");
xlabel("Temps (s)");
ylabel("Distance (mm)");
legend("Ecart direction", "Err. retenue");
% 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]);
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', 'Blue', '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