clear;          % Daten löschen
clf;            % Plot löschen

% Festlegunge der Geschwindigkeiten und der Bewegungsdauer
Testzeit= 500;  % Zeit in Schleifenperioden (sp)
vA =  45;       % Geschwindigkeit in Sensorschritten pro sp
vB = -20;       % mal 64, WB: -63 bis 63, ganzzahlig

s = serial('COM9','BAUD',9600);
set(s,'Timeout',1000); % Timeout 1000s (kein sofortiger Abbruch 
fopen(s);              % wenn der Prozessor im Debugger hält)
                       % SIO bei Absturz freigeben /Alternative
                       % Neustart von Matlab nach jedem Fehler
closeFID = onCleanup(@() fclose(s)); 
fwrite(s,[128]);       % Startkommando

%Geschwindigkeiten als 7Bit-Zweierkoplement * 2 (+1)
if vA<0 v = 128+vA;, else v = vA;, end;
fwrite(s, v*2);        % A erwartet geraden Wert
if vB<0 v = 128+vB;, else v = vB;, end;
fwrite(s, v*2+1);      % B erwartet ungeraden Wert


istA(1) = 0;           % Vektoren, zur Speicherung der Ist- und
sollA(1)= 0;           % Sollwerte der Motorpositionen in
istB(1) = 0;           % Anhängigkeit von der Zeit (in Mikro-
sollB(1)= 0;           % rechnerschleifendurchläufen)

fprintf('\n--------------------------------------------------------|\n');
fprintf('Zeit|       Motor A           |          Motor B        |\n');
fprintf('    | En Dir + - |  ist  soll | En Dir + - |  ist  soll |\n');
hold on;    % graphische Ausgabe weiterzeichnen
for t =2:Testzeit
    dat=fread(s,1);    % ein Datenbyte vom Mikrorechner lesen
    if length(dat)<1, return, end % Abbruch, falls keine Daten
    
    % Aufspalten des Empfangsbytes in seine einzelnen Bits
    pA  = bitget(dat, 8);
    nA  = bitget(dat, 7);
    DirA= bitget(dat, 6);
    EnA = bitget(dat, 5);
    pB  = bitget(dat, 4);
    nB  = bitget(dat, 3);
    DirB= bitget(dat, 2);
    EnB = bitget(dat, 1);
    istA(t) = istA(t-1) + pA - nA;
    sollA(t)= sollA(t-1)+ single(vA)/64;
    istB(t) = istB(t-1) + pB - nB;
    sollB(t)= sollB(t-1)+ single(vB)/64;
    
    % tabellarische Ausgabe
    fprintf('%4i|  %i  %i  %i %i |',t, EnA, DirA, pA, nA);
    fprintf('%4i %6.2f | ', istA(t), sollA(t)); 
    fprintf(' %i  %i  %i %i |', EnB, DirB, pB, nB);
    fprintf('%4i %6.2f |\n', istB(t), sollB(t)); 

    % graphische Ausgabe
    plot([t],[sollA(t)], 'r', [t],[sollB(t)], 'b');
    if EnA plot([t],[istA(t)], 'r');
    else   plot([t],[istA(t)], 'k'), end;
    if EnB plot([t],[istB(t)], 'b');
    else   plot([t],[istB(t)], 'k'), end;    
end;

hold off;        % Weiterzeichnen der Graphik abschließen
xlabel('Zeit in Mikrorechner-Hautschleifendurchläufen')
ylabel('Weg in Sensorschritten');
title('rot -- Motor A, blau Motor B, schwarz -- Motor aus');
grid on;

fwrite(s,[129]); % Motoren ausschalten
fclose(s);       % serielle Schnittstelle schließen