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

% Bewegungsdauer und Geschwindigkeiten
tz = uint16(400);% Bewegungsdauer mal 10 ms
% Geschwindigkeit in 256stel Sensorschritten pro 
vA =  500;      % 10 ms, der Motor schafft von
vB = -500;      % -2000 bis 2000 Sensorschr./10ms

s = serial('COM9','BAUD',9600);
set(s,'Timeout',1000); % Timeout 1000s  
fopen(s);              % serielle Verbindung herstellen
% wenn der Prozessor im Debugger hält SIO bei Absturz 
% freigeben. Alternative Neustart von Matlab nach 
closeFID = onCleanup(@() fclose(s)); % jedem Fehler

%Geschwindigkeits- und Zeitwert versenden
fwrite(s, 'a');         % Kommandobyte

if vA<0 vAu16=uint16(2^16+vA);  % 2er-Kompl.
else    vAu16=uint16(vA); end;  % Umformung
fwrite(s, bitshift(vAu16, -8)); % va Byte 1
fwrite(s, bitand(vAu16, 255));  % va Byte 0

if vB<0 vBu16=uint16(2^16+vB);  % 2er-Kompl.
else    vBu16=uint16(vB); end;  % Umformung
fwrite(s, bitshift(vBu16, -8)); % vb Byte 1
fwrite(s, bitand(vBu16, 255));  % vb Byte 0

fwrite(s, bitshift(tz, -8));    % tz Byte 1
fwrite(s, bitand(tz, 255));     % tz Byte 0

istA(1) = 0;           % Vektoren, zur Speicherung
sollA(1)= 0;           % der Ist- und Sollwerte
istB(1) = 0;           % der Motorpositionen
sollB(1)= 0;           % in Sensorschritten 
pwmMA(1)= 0;           % PWM-Werte der
pwmMB(1)= 0;           % Motoren

fprintf('\n--------------------------------------------------------|\n');
fprintf('Zeit| incA istA  sollA pwmA| incB istB  sollB pwmB|\n');
for t =2:tz
    dat=fread(s,4);   % 4 Byte vom Mikrorechner lesen
    if length(dat)<4, ... % Falls keine Daten
            return, end % Abbruch
  
    % Aufspalten der Bytesytes in seinen Bestandteile
    incA    = bitand(dat(2), 63);   % Ist-Schritte Motor A
    if bitget(dat(2),7) incA=incA-64; end;
    pwmA = dat(1)*2;                % PWM-Wert Motor A
    if bitget(dat(2),8) pwmA=pwmA+1; end;
    if bitget(dat(1),8) pwmA=pwmA-512; end;
    %fprintf('incA=%x, pwmA=%x\n', incA, pwmA);
    
    incB    = bitand(dat(4), 63);   % Ist-Schritte Motor B
    if bitget(dat(4),7) incB=incB-64; end;
    pwmB = dat(3)*2;                % PWM-Wert Motor B
    if bitget(dat(4),8) pwmB=pwmB+1; end;
    if bitget(dat(3),8) pwmB=pwmB-512; end;
    
    istA(t) = istA(t-1) + single(incA);     % Ist-Pos. A
    sollA(t)= sollA(t-1)+ single(vA)/256;   % Soll-Pos. A
    pwmMA(t)= pwmA;                         % PWM Motor A
    istB(t) = istB(t-1) + single(incB);     % Ist-Pos. B
    sollB(t)= sollB(t-1)+ single(vB)/256;   % Soll-Pos. B
    pwmMB(t)= pwmB;                         % PWM Motor B
    
    % tabellarische Ausgabe
    fprintf('%4i | %4i %4i %7.2f %4i ', t, ...
        incA, istA(t), sollA(t), pwmA); 
    fprintf('|%4i %4i %7.2f %4i|\n', incB, ...
        istB(t), sollB(t), pwmB);    
end;
tv = ((1:tz)-1)*10;         % Vektor der Zeitwerte in ms
% graphische Ausgabe
plot(tv, sollA, 'r', tv, istA, 'g', tv, pwmMA, 'b', ...
    tv,sollB, 'k', tv, istB, 'm', tv, pwmMB, 'c');


xlabel('Zeit in Millisekunden')
ylabel('Weg in Sensorschritten');
title('rot: SollA, grün: IstA, blau: pwmA, gelb: sollB, magneta: istB, cyan: pwmB');
grid on;

fclose(s);       % serielle Schnittstelle schließen