%
% 1.  Complile and flash the project "Acceleration"
% 2.  Close the CodeWarrior Debug window, flip the RUN/LOAD switch to RUN
% 3.  Reset the mDRGN board to start the program
%   Press and hold "A" to collect acceleration data
%   Press and hold "C" to watch the acceleration data
%   Run the MATLAB file DRGN_2_MLAB.m, press "B"  to send collected data
%
clear;
s = serial('COM1','BaudRate',9600,'DataBits',8);
fopen(s);  %opens the serial port
count = fread(s,1,'int16')   %get 16-bit integer
for i = 1:count
    ax(i)=fread(s,1,'int16');
    ay(i)=fread(s,1,'int16');
    az(i)=fread(s,1,'int16');
end
fclose(s);
dt=0.01024;
g = 9.81; % m/s^2
scale=1.5;
%maxval = (3/8)*(3.3/5.0)*1023;
maxval = (3.3/5.0)*1023;
zeroval= maxval/2.0;
%{
avgx=0.0;
avgy=0.0;
avgz=0.0;

for i=1:count
    t(i)=(i-1)*dt;
    ax(i)=((ax(i)-zeroval)/zeroval)*scale;  % *g;
    ay(i)=((ay(i)-zeroval)/zeroval)*scale;  % *g;
    az(i)=((az(i)-zeroval)/zeroval)*scale;  % *g;
    if (i<51)
        avgx=avgx+ax(i);
        avgy=avgy+ay(i);
        avgz=avgz+az(i);
    end
end
avgx=avgx/50;
avgy=avgy/50;
avgz=avgz/50;
for i=1:count
    ax(i)=ax(i)-avgx;
    ay(i)=ay(i)-avgy;
    az(i)=az(i)-avgz;
end
//%}
avgpts=100;
for i=1:count
    t(i)=(i-1)*dt;
    if (i<=avgpts)
        avgx=avgx+ax(i);
        avgy=avgy+ay(i);
        avgz=avgz+az(i);
    end
end
avgx=avgx/avgpts;
avgy=avgy/avgpts;
avgz=avgz/avgpts;
for i=1:count
    ax(i)=(ax(i)-avgx)/zeroval*scale*g;
    ay(i)=(ay(i)-avgy)/zeroval*scale*g;
end


%plot(t,ax,'-r', t, ay, '-g', t, az, '-b')
% integrate the x and y values of acceleration
vx(1)=0;
vy(1)=0;
sx(1)=0;
sy(1)=0;
for i=2:length(ax)
    vx(i)=vx(i-1)+0.5*(ax(i-1)+ax(i))*dt;
    vy(i)=vy(i-1)+0.5*(ay(i-1)+ay(i))*dt;    
    sx(i)=sx(i-1)+0.5*(vx(i-1)+vx(i))*dt;
    sy(i)=sy(i-1)+0.5*(vy(i-1)+vy(i))*dt;
end
% ENTER to see the following plots in turn
plot(t,ax);
xlabel('time');
ylabel('x-acceleration');
pause;
plot(t,ay);
xlabel('time');
ylabel('y-acceleration');
pause;
plot(t,vx);
xlabel('time');
ylabel('x-velocity');
pause;
plot(t,vy);
xlabel('time');
ylabel('y-velocity');
pause;
plot(t,sx);
xlabel('time');
ylabel('x-position');
pause;
plot(t,sy);
xlabel('time');
ylabel('y-position');
pause;

plot(sx,sy);
xlabel('x position');
ylabel('y position');
%}

