Control with TrackCam Vision Feedback and MATLAB

From Mech
Jump to navigationJump to search

Stripping Down Windows

To make the new computer that runs the control system run as efficiently as possible, all extraneous functionalities were turned off. There is not anti-virus software, no firewall, no https for the internet, and so on. Only the bare minimum is left. Below is a list of the remaining functions and what they do. Another thing to do to make MATLAB run faster is to set it to go to the Task Manager, go to Processes, right-click on MATLAB.exe and set it to Realtime priority.

Explorer.exe

This is the user shell, which we see as the familiar taskbar, desktop, and other user interface features. This process isn't as vital to the running of Windows as you might expect, and can be stopped (and restarted) from Task Manager, usually with no negative side effects on other applications.

nvsvc32.exe

Part of the NVIDIA graphics card drivers (Detonator). It should be left running on any system using Nvidia graphics hardware to ensure proper function.

MATLAB.exe

MATLAB

svchost.exe

an integral part of Windows OS. It cannot be stopped or restarted manually. This process manages 32-bit DLLs and other services. At startup, Svchost.exe checks the services portion of the registry and constructs a list of services that it needs to load. Under normal conditions, multiple instances of Svchost.exe will be running simultaneously. Each Svchost.exe session can contain a grouping of services, so that many services can be run depending on how and where Svchost.exe is started. This allows for better control and debugging.

lsass.exe

the Local Security Authentication Server. It verifies the validity of user logons to your PC or server. Lsass generates the process responsible for authenticating users for the Winlogon service. This is performed by using authentication packages such as the default, Msgina.dll. If authentication is successful, Lsass generates the user's access token, which is used to launch the initial shell. Other processes that the user initiates then inherit this token.

services.exe

This is the Services Control Manager, which is responsible for running, ending, and interacting with system services. Use this program to start services, stop them, or change their default from automatic to manual startup.

winlogon.exe

runs in the background. Winlogon is a part of the Windows Login subsystem, and is necessary for user authorization and Windows XP activation checks.

csrss.exe

This is the user-mode portion of the Win32 subsystem; Win32.sys is the kernel-mode portion. Csrss stands for Client/Server Run-Time Subsystem, and is an essential subsystem that must be running at all times. Csrss is responsible for console windows, creating and/or deleting threads, and implementing some portions of the 16-bit virtual MS-DOS environment.

smss.exe

This is the session manager subsystem, which is responsible for starting the user session. This process is initiated by the main system thread and is responsible for various activities, including launching the Winlogon and Win32 (Csrss.exe) processes, and setting system variables. After it has launched these processes, it waits for either Winlogon or Csrss to end. If this happens normally, the system shuts down; if it happens unexpectedly, Smss.exe causes the system to stop responding (hang).

ctfmon.exe

When you run a Microsoft Office XP program, the file Ctfmon.exe (Ctfmon) runs in the background, even after you quit all Office programs. Ctfmon.exe monitors active windows and provides text input service support for speech recognition, handwriting recognition, keyboard translation, and other alternate user input forms.

rundll32.exe

This program is part of Windows, and is used to run program code in DLL files as if they were within the actual program.

RTHDCPL.EXE

a process belonging to the Realtek HD Audio Control Panel and is bundled alongside Realtek sound cards and audio hardware. This program is a non-essential process, but should not be terminated unless suspected to be causing problems.

ipoint.exe

a process installed alongside a Microsoft IntelliMouse and provides additional configuration options for these devices. "This program is a non-essential process, but should not be terminated unless suspected to be causing problems.

System

a process which shows up on the tasks on mainly Windows XP, Windows 2003 server and later version of Windows. This is a default system counter and cannot be removed.


Shared Memory

One of the methods explored to make the system run as quickly as possible was to have MATLAB and the camera running on separate cores of the computer, so both could run in parallel. This would require for them to communicate through shared memory. At the bottom of this page is a .cpp program that communicates through shared memory. We ultimately did not take this approach because Windows appears to be quick enough for now. Eventually, a real-time operating system such as Linux RTAI would be preferred, but for the time being, Windows suffices. I will therefore not explain further how the shared memory option works. However, if it is ever needed, everything to communicate through shared memory is in that file.


Test of Sample Rates

Overview

To see if using Windows instead of a real-time operating system would be a viable option, we had to test how fast the sampling rate could be set to without losing samples. We tried two methods: the first was to output data on one computer and collect it on the same computer, and the second was to output data on one computer and collect it on another computer.

Single Computer Tests

Pulses
To run these tests, we used Measurement Computing Cards (mcc). Both were installed in the same computer. The MATLAB code that is at the end of this document was then run (testInputOutput.m). It creates an output wave that is a wave of alternating pulses. These were output at a predetermined rate. The input card kept on taking input all the time. Then, the duration of the input pulses was measured to see how long each pulse actually was as opposed to how long we wanted it to be. Plots of this were made, along with the standard deviation and the extra duration of each pulse. Using this method, we were able to see how fast we could feasibly run the alternate pulses without losing accuracy. Figure 1 shows that this test started to break down around 2000 Hz.

Figure 1: Number of pulses with given duration.

Two Computers

Pulses
The next step was to output data on one computer and to input data on another. For output, we again used the mcc card. For input, however, we used a National Instruments (ni) card. Two programs were run for this test. The program run from the output computer was testOutput.m, and the one run on the input computer was testInputCard.m. With this simple modification to the setup, the test still ran fine at 10000 Hz, as can be seen in Figure 2.

Figure 2: Number of pulses with given duration.

Single Computer Again

Sine Wave
Following the tests with pulses, we decided to try to send a sine wave instead of a pulse, and to see at what output sample rate the frequency of the wave started to become inaccurate. This test is more practical, since in reality we would be sending sine waves and not pulses, and a little error in the output wave is acceptable in our applications. We used the same single computer setup as before, and ran the code inputOutputSineWave.m. A result of this test can be seen in Figure 3.

Figure 3: Frequency of sine wave with requested frequency of 50 Hz.

Sine Wave Sweep
Finally, we decided to try a sweep of sine wave frequencies to determine exactly where the input frequency breaks down. The code that was run is called sweepOfSampleRatesSineWave.m. The results of this can be seen in Figure 4. This breakdown occurred at 8100 Hz.

Figure 4: Sweep of requested sample rates.


Integration of Systems

To begin

Make sure the National Instruments input and output cards are hooked up and set up with the new computer. On line 39 of the program “integrationTestPutSample.m”, make sure the right device number is chosen. As the program runs now, only the bottom two speakers are controlled, and the left and right speakers don’t move. Make sure the camera is calibrated properly. Run the .m file, and the ball should be bounced to an average height of 20 mm. This is a test of the closed feedback loop from the camera.

Contents of programs

The program consists of two separate programs. There is the MATLAB .m file, “integrationTestPutSample.m”, and the MEX-file, “skeleton.cpp”. The analysis of the data collected is done in MATLAB, where it can be seen in the beginning that a lot of matrices are set up to hold data. Then, the MATLAB program calls the MEX-file, which returns a lot of data, and that data is analyzed in MATLAB. The MEX-file itself takes the pictures and does image processing, calculating the centroid of each picture of the bouncing ball. All the functions in both the .m file and the .cpp file are documented within the programs.

Overview of operation of control system

First, the camera and the framegrabber are initialized by allocating space in memory for the images that will be taken. Then, the camera starts taking pictures at a pre-specified rate (FRAME_TIME in the constants.h file). The three most recent pictures (real ball, reflected ball, and bar) are then taken from the image buffer and the coordinates of each centroid are calculated with some image processing algorithms. Then, these coordinates are used as input to another function in MATLAB to calculate x, y, and z coordinates for the ball and x and z coordinates for the bar. After that, the program checks if the ball is below 14.5 mm from the bar. If it is, half a period of sine wave is input into the speakers, and the ball is bounced back up. If the previous bounce of the ball was above 21 mm, the amplitude of the sine wave is reduced a little. If the previous bounce was below 19 mm, the amplitude of the sine wave is increased a little. This keeps the ball bouncing to approximately 20 mm. This loop then runs for as long as is specified, and afterwards the space allocated for the images is de-allocated.

Timing

The timing of the system is detailed on the timing diagrams (Fig 5 and 6). The idea, which is not fully implemented yet, is to have there be two pictures already taken when the MATLAB program calls the MEX-file, so that the program waits for only one more picture to be taken and then grabs all three and processes them all. To implement this, the frame time has to be just a little shorter than half of the total processing time, including the MEX-file processing and the MATLAB processing.

Figure 5: Sweep of requested sample rates.
Figure 6: Sweep of requested sample rates.


Code

Shared Memory

 
#include <windows.h>
#include <stdio.h>
#include <conio.h>
#include <tchar.h> 

#define BUF_SIZE 256
TCHAR szName[]=TEXT("Global\\MyFileMappingObject");
//TCHAR szMsg[]=TEXT("What's up (from the first process)?");
int szMsg;

int _tmain()
{
   HANDLE hMapFile;
   LPCTSTR pBuf;
   int ch,image;
   int coordinates[3];

   hMapFile = CreateFileMapping(
                 INVALID_HANDLE_VALUE,    // use paging file
                 NULL,                    // default security 
                 PAGE_READWRITE,          // read/write access
                 0,                       // max. object size 
                 BUF_SIZE,                // buffer size  
                 szName);                 // name of mapping object
 
   if (hMapFile == NULL) 
   { 
      _tprintf(TEXT("Could not create file mapping object (%d).\n"), 
             GetLastError());
      return 1;
   }
   pBuf = (LPTSTR) MapViewOfFile(hMapFile,   // handle to map object
                        FILE_MAP_ALL_ACCESS, // read/write permission
                        0,                   
                        0,                   
                        BUF_SIZE);           
 
   if (pBuf == NULL) 
   { 
      _tprintf(TEXT("Could not map view of file (%d).\n"), 
             GetLastError()); 

	  CloseHandle(hMapFile);

      return 1;
   }

   for (szMsg=0;szMsg<5;szMsg++) {
	   image = szMsg*2;
	   coordinates[0] = image * 2;
       coordinates[1] = image * 3;
       coordinates[2] = image * 4;

	   CopyMemory((PVOID)pBuf, coordinates, (3*sizeof(int)));

	   _cputs( "Type 'Y' to increment counter: " );
	   do
	   {
		  ch = _getch();
		  ch = toupper( ch );
	   } while( ch != 'Y' );

	   _putch( ch );
	   _putch( '\r' );    // Carriage return
	   _putch( '\n' );    // Line feed  
   }

   UnmapViewOfFile(pBuf);

   CloseHandle(hMapFile);

   return 0;
}



#include <windows.h>
#include <stdio.h>
#include <conio.h>
#include <tchar.h>
#pragma comment(lib, "user32.lib")

#define BUF_SIZE 256
TCHAR szName[]=TEXT("Global\\MyFileMappingObject");

int _tmain()
{
   HANDLE hMapFile;
   LPCTSTR pBuf;
   int i;
   double coordinates[3];
   int temp;
   const char *tempChar;

   hMapFile = OpenFileMapping(
                   FILE_MAP_ALL_ACCESS,   // read/write access
                   FALSE,                 // do not inherit the name
                   szName);               // name of mapping object 
 
   if (hMapFile == NULL) 
   { 
      _tprintf(TEXT("Could not open file mapping object (%d).\n"), 
             GetLastError());
      return 1;
   } 

   pBuf = (LPTSTR) MapViewOfFile(hMapFile, // handle to map object
               FILE_MAP_ALL_ACCESS,  // read/write permission
               0,                    
               0,                    
               BUF_SIZE);                   
 
   if (pBuf == NULL) 
   { 
      _tprintf(TEXT("Could not map view of file (%d).\n"), 
             GetLastError()); 

	  CloseHandle(hMapFile);

      return 1;
   }

   tempChar = (const char *) pBuf;
   temp = atoi(tempChar);
   do {
	for (i=0; i<3; i++){
		*(coordinates+i) = (double) (atoi((const char *) (&(*(pBuf+i)))));
	}
   } while (1);

   UnmapViewOfFile(pBuf);

   CloseHandle(hMapFile);
 
   return 0;
}

testInputOutput.m

clear all
close all
% set a desired sample rate of output
sampRate = 5000;
dt = 1/sampRate;
amp = 1;
% set input card's sample rate
sampRateIn = 50000;
% amount of time to acquire data
timeIn = 10;
trigger = timeIn*sampRateIn;

% creat input and output objects
AI = analoginput('mcc',0);
AO = analogoutput('mcc',1);

% add channels
addchannel(AI,[0:5],[1:6]);
addchannel(AO,[0:5],[1:6]);

% set up cards
set(AI,'SampleRate',sampRateIn)
set(AI,'SamplesPerTrigger',trigger)
set(AO,'SampleRate',ceil(1/500))

time = 1/sampRateIn:1/sampRateIn:timeIn;
t = 0:dt:timeIn;

% create an output wave that is just an alternating pulse
outputwave = zeros(length(t),1);
for jj = 1:floor(length(t)/2)
    outputwave(2*jj,1) = amp;
end
outputwave(:,2) = outputwave;
outputwave(:,[2 3 4 5 6]) = 0;

temp = rand(10);

% start acquisition
start(AI)
tic
a = 0;
% output data
for ii = 1:length(outputwave)
    while(toc<(dt*ii))
    end
    putsample(AO,outputwave(ii,:));
    %z = sampleMexFile(ii,ii,ii);
    %fft(outputwave);
    %fprintf('pause: %f\n',dt-time);
    %data(ii,:) = getsample(AI);
end

% collect data
data = getdata(AI);
stop(AI);
delete(AI)
clear AI

% get meaningful data from the channel that matters
meaningfulData = data(:,1);
plot(time,meaningfulData);

freqData = zeros(trigger,1);
counter = 0;
pulseCounter = 1;
% count number of pulses that have a specific duration
for ii = 2:trigger
    if (abs(meaningfulData(ii) - meaningfulData(ii-1)) < .05)
        counter = counter + 1;
    elseif ((counter ~= 0) && (ii-counter > 3))
        freqData(counter) = freqData(counter) + 1;
        pulseDuration(pulseCounter) = counter/sampRateIn;
        counter = 0;
        pulseCounter = pulseCounter + 1;
    elseif (ii-counter == 3)
        counter = 0;
    end
end

average = 1/sampRate;
figure
% create a graph of pulse durations
stem(time(1:2*average*sampRateIn),freqData(1:2*average*sampRateIn));
title(['Number of Pulses with Given Duration, Recorded for ',num2str(timeIn),' Seconds (',num2str(sampRate),' Hz)']);
xlabel('Pulse Duration (sec)');
ylabel('Number of Pulses');

averageDuration = 0;
for jj = 1:trigger
    if (freqData(jj) ~= 0)
        fprintf('%u pulses had duration of: %f \n',freqData(jj),jj/sampRateIn);
        averageDuration = averageDuration + (freqData(jj)*jj/sampRateIn);
    end
end
fprintf('\n');
averageDuration = averageDuration/sum(freqData);
fprintf('Average duration of pulses: %f \n\n',averageDuration);

standardDev = 0;
% find the standard deviation
for jj = 1:trigger
    if (freqData(jj) ~= 0)
        for kk = 1:freqData(jj)
            standardDev = standardDev + (averageDuration - (jj/sampRateIn))^2;
        end
    end
end
standardDev = sqrt(standardDev/(sum(freqData) - 1));
fprintf('Standard deviation of duration of pulses: %f \n\n\n',standardDev);

pulseDuration = pulseDuration - mode(pulseDuration);
figure
% plot how much longer or shorter pulses were than what was requested
stem(pulseDuration/dt);
title('Pulse Duration vs. Time')
xlabel('Time (sec)')
ylabel(['Extra length of pulse in terms of dt (',num2str(dt),' seconds)']);

testOutput.m

clear all
close all
sampRate = 5000;
dt = 1/sampRate;
freq = 30;
timeIn = 30;
amp = 1;

AO = analogoutput('mcc',1);
addchannel(AO,[0:5],[1:6]);
set(AO,'SampleRate',ceil(1/500))

t = 0:dt:timeIn;

outputwave = zeros(length(t),1);
for jj = 1:floor(length(t)/2)
    outputwave(2*jj,1) = amp;
end
outputwave(:,2) = outputwave;
outputwave(:,[2 3 4 5 6]) = 0;

tic
for ii = 1:length(outputwave)
    while(toc<(dt*ii))
    end
    putsample(AO,outputwave(ii,:));
    %z = sampleMexFile(ii,ii,ii);
    %fft(outputwave);
    %fprintf('pause: %f\n',dt-time);
    %data(ii,:) = getsample(AI);
end

testInputCard.m

clear all
close all
dt = 1/500;
freq = 30;
sampRateIn = 50000;
trigger = 100000;

time = 1/sampRateIn:1/sampRateIn:trigger/sampRateIn;

AI = analoginput('mcc',0);
addchannel(AI,[0:5],[1:6]);

set(AI,'SampleRate',sampRateIn)
set(AI,'SamplesPerTrigger',trigger)

start(AI)
data = getdata(AI);
meaningfulData = data(:,1);
plot(time,meaningfulData);

freqData = zeros(trigger,1);
counter = 0;
for ii = 2:trigger
    if (abs(meaningfulData(ii) - meaningfulData(ii-1)) < .05)
        counter = counter + 1;
    elseif ((counter ~= 0) && (ii-counter > 3))
        freqData(counter) = freqData(counter) + 1;
        counter = 0;
    elseif (ii-counter == 3)
        counter = 0;
    end
end

figure
stem(time,freqData);

for jj = 1:trigger
    if (freqData(jj) ~= 0)
        fprintf('%u pulses had duration of: %f \n',freqData(jj),jj/sampRateIn);
    end
end

inputOutputSineWave.m

clear all
close all
sampRate = 200;
dt = 1/sampRate;
freq = 50;
amp = 1;
sampRateIn = 50000;
timeIn = 10;
trigger = timeIn*sampRateIn;

AI = analoginput('mcc',0);
AO = analogoutput('mcc',1);

addchannel(AI,[0:5],[1:6]);
addchannel(AO,[0:5],[1:6]);

set(AI,'SampleRate',sampRateIn)
set(AI,'SamplesPerTrigger',trigger)
set(AO,'SampleRate',ceil(1/500))

time = 1/sampRateIn:1/sampRateIn:timeIn;
t = dt:dt:timeIn;

outputwave = amp*sin(2*pi*freq*t);
outputwave = outputwave';
outputwave(:,2) = amp*sin(2*pi*freq*t);
outputwave(:,[3 4 5 6]) = 0;

start(AI)
tic
a = 0;
for ii = 1:length(outputwave)
    while(toc<(dt*ii))
    end
    putsample(AO,outputwave(ii,:));
    z = sampleMexFile(ii,ii,ii);
end

data = getdata(AI);
stop(AI);
delete(AI)
clear AI

meaningfulData = data(:,1);
plot(time,meaningfulData);

[YfreqDomain,frequencyRange] = positiveFFT(meaningfulData,sampRateIn);
positiveFFT = figure;
stem(frequencyRange,abs(YfreqDomain));
xlabel('Freq (Hz)')
ylabel('Amplitude')
title('FFT')
grid
axis([0,2*freq,0,amp])

NFFT = 2^nextpow2(length(meaningfulData)); % Next power of 2 from length of y
Y = fft(meaningfulData,NFFT)/length(meaningfulData);
f = sampRateIn/2*linspace(0,1,NFFT/2+1);
figure
plot(f,2*abs(Y(1:NFFT/2+1)))
title(['Single-Sided Amplitude Spectrum of y(t) (',num2str(freq),' Hz) @ ',num2str(sampRate),' Hz'])
xlabel('Frequency (Hz)')
ylabel('|Y(f)|')
axis([0,2*freq,0,amp])

[a,b] = max(Y(1:NFFT/2+1));
actualFreq = f(b);
fprintf(['\nRequested frequency: ',num2str(freq), ' Hz\n']);
fprintf(['Actual frequency: ',num2str(actualFreq), ' Hz\n']);

sweepOfSampleRatesSineWave

clear all
close all

freq = 50;
amp = 1;
sampRateIn = 50000;
timeIn = 10;
trigger = timeIn*sampRateIn;

AI = analoginput('mcc',0);
AO = analogoutput('mcc',1);

addchannel(AI,[0:5],[1:6]);
addchannel(AO,[0:5],[1:6]);

set(AI,'SampleRate',sampRateIn)
set(AI,'SamplesPerTrigger',trigger)
set(AO,'SampleRate',ceil(1/500))

time = 1/sampRateIn:1/sampRateIn:timeIn;

sweep = zeros(100,3);
for sr = 500:100:10000
    sampRate = sr;
    dt = 1/sampRate;
    
    t = 0:dt:timeIn;

    outputwave = amp*sin(2*pi*freq*t);
    outputwave = outputwave';
    outputwave(:,2) = amp*sin(2*pi*freq*t);
    outputwave(:,[3 4 5 6]) = 0;

    start(AI)
    tic
    for ii = 1:length(outputwave)
        while(toc<(dt*ii))
        end
        putsample(AO,outputwave(ii,:));
        %fprintf('pause: %f\n',dt-time);
        %data(ii,:) = getsample(AI);
    end

    data = getdata(AI);
    stop(AI);

    meaningfulData = data(:,1);
    
    NFFT = 2^nextpow2(length(meaningfulData)); % Next power of 2 from length of y
    Y = fft(meaningfulData,NFFT)/length(meaningfulData);
    f = sampRateIn/2*linspace(0,1,NFFT/2+1);
%     figure
%     plot(f,2*abs(Y(1:NFFT/2+1)))
%     title(['Single-Sided Amplitude Spectrum of y(t) (',num2str(freq),' Hz) @ ',num2str(sampRate),' Hz'])
%     xlabel('Frequency (Hz)')
%     ylabel('|Y(f)|')
%     axis([0,2*freq,0,amp])

    [a,b] = max(Y(1:NFFT/2+1));
    actualFreq = f(b);
    
    sweep(sr/100,1) = sr;
    sweep(sr/100,2) = actualFreq;
end

figure
plot(sweep(:,1), sweep(:,2));

integrationTestPutSample.m

% This program is a simple control system to test the functionality of the
% camera and actuator closed-loop feedback system.  The camera takes
% pictures at a predetermined rate (set in the MEX-file).  This program
% accesses the MEX-file and takes the three most recent pictures from the
% image buffer.  The coordinates of the centroid of each image are then
% calculated.  These coordinates are passed back into this program, where
% the x, y, and z coordinates of the ball are computed and the x and z
% coordinates of the bar are computed.  After that, the program checks if
% the ball is below 14.5 mm from the bar.  If it is, half a period of a
% sine wave is input into the speakers, and the ball is bounced back up.
% If the previous bounce of the ball was above 21 mm, the amplitude of the
% sine wave is reduced a little.  If the pervious bounce was below 19 mm,
% the amplitude of the sine wave is increased a little.  This keeps the
% ball bouncing to approximately 20 mm.  This loop then runs for as long as
% is specified, and afterwards the space allocated for the images is
% de-allocated.

clear all
close all

% load calibration constants
calib = '8-17-09 Calibration Constants';
load(calib);

% compile the MEX-file
mex -L'C:\PROGRA~1\SiliconSoftware3.2\lib\visualc' -L'C:\Program Files\OpenCV\lib' -ldisplay_lib -lclserme3 -lclsersisome3 -lfastconfig -lfglib3.1 -lfglib3 -liolib -lmenable3 -lhighgui -lcv -lcvaux -lcvcam -lcvhaartraining -lcxcore -lcxts -lml skeleton.cpp cam.cpp imgproc.cpp roi.cpp tracking.cpp utils.cpp
% initialize camera and framegrabber
skeleton(0);

% set the sample rate of the data input into the speakers
sampRate = 2000;
dt = 1/sampRate;
% set the frequency of the sine wave
freq = 30;
amp = 2;
timeOnePeriod = 1/freq;

% create an analog output object
AO = analogoutput('nidaq','Dev3');

% add 2 channels for the two bottom speakers
addchannel(AO,2:3);

% set a sample rate for the output (only matters is the command "putdata"
% is used).
set(AO,'SampleRate',sampRate);

t = 0:dt:timeOnePeriod/2;

% create output wave
outputwave = amp*sin(2*pi*freq*t);
outputwave = outputwave';
outputwave(:,2) = amp*sin(2*pi*freq*t);

% set the length of the loop
lenLoop = 10000;

% initialize vectors to store useful data
height_ball = zeros(1,lenLoop);
height_bar = zeros(1,lenLoop);
proportional = zeros(1,lenLoop);
multArray = zeros(1,lenLoop);
timeArray = zeros(2,lenLoop);
eventTimer = zeros(1,3*lenLoop);
imageNumber = zeros(1,3*lenLoop);
times = zeros(5,lenLoop);

% initialize useful variables
previousHeight = 0;
goingDown = 0;
time_ball(1) = 0;
multiplier = 2;
P = 13.8;
ii = 1;

% start actual control system
while ii<(lenLoop+1)
    % access the MEX-file and store results in variable "temp"
    temp = skeleton(1);
    % use proper entries from "temp" to create inputs for the
    % calc_coords3ROI function
    XZ = [1 temp(3) temp(1) temp(2)];
    YZ = [1 temp(6) temp(4) temp(5)];
    bar_tracking = [1 temp(9) temp(7) temp(8)];
    
    %eventTimer((3*ii)-2:(3*ii)) = [temp(3) temp(6) temp(9)];
    %imageNumber((3*ii)-2:(3*ii)) = [temp(10) temp(11) temp(12)];
    
    % store the image number, image number modulo 3, time to get first
    % image number, time to get rest of images, and time to process images
    times(:,ii) = [temp(13); temp(14); temp(15); temp(16); temp(17)];
    % calculate coordinates
    [XYZ_BALL, XZ_BAR] = calc_coords3ROI(XZ,YZ,bar_tracking);
    % check if bar has to bounce
    if XYZ_BALL(5) > 13.9 && XYZ_BALL(5) < 14.5
        % check if ball bounced too high
        if P > 21
            % adjust amplitude of output wave
            multiplier = multiplier - .1;
        % check if ball bounced too low
        elseif P < 19 && multiplier < 4
            % adjust amplitude of output wave
            multiplier = multiplier + .1;
        end
        tic
        % load output wave into speakers
        for kk = 1:length(outputwave)
            while(toc<(dt*kk))
            end
            temp2 = toc;
            putsample(AO,multiplier*outputwave(kk,:));
            temp = skeleton(1);
            XZ = [1 temp(3) temp(1) temp(2)];
            YZ = [1 temp(6) temp(4) temp(5)];
            bar_tracking = [1 temp(9) temp(7) temp(8)];
            %eventTimer((3*ii)-2:(3*ii)) = [temp(3) temp(6) temp(9)];
            %imageNumber((3*ii)-2:(3*ii)) = [temp(10) temp(11) temp(12)];
            times(:,ii) = [temp(13); temp(14); temp(15); temp(16); temp(17)];
            [XYZ_BALL, XZ_BAR] = calc_coords3ROI(XZ,YZ,bar_tracking);
            timeArray(1,ii) = toc - temp2;
            height_bar(ii) = XZ_BAR(4);
            height_ball(ii) = XYZ_BALL(5);
            ii = ii + 1;
            if ii>(lenLoop-1)
                break
            end
        end
        timeArray(2,ii) = toc;
        multArray(ii) = multiplier;
    % if ball settles on lens, wait one second and then bounce it back up
    elseif XYZ_BALL(5) < 13.9
        tic
        while XYZ_BALL(5) < 13.9
            temp = skeleton(1);
            XZ = [1 temp(3) temp(1) temp(2)];
            YZ = [1 temp(6) temp(4) temp(5)];
            bar_tracking = [1 temp(9) temp(7) temp(8)];
            [XYZ_BALL, XZ_BAR] = calc_coords3ROI(XZ,YZ,bar_tracking);
            if toc > 1
                tic
                for kk = 1:length(outputwave)
                    while(toc<(dt*kk))
                    end
                    putsample(AO,4*outputwave(kk,:));
                    temp = skeleton(1);
                    XZ = [1 temp(3) temp(1) temp(2)];
                    YZ = [1 temp(6) temp(4) temp(5)];
                    bar_tracking = [1 temp(9) temp(7) temp(8)];
                    [XYZ_BALL, XZ_BAR] = calc_coords3ROI(XZ,YZ,bar_tracking);
                    height_bar(ii) = XZ_BAR(4);
                    height_ball(ii) = XYZ_BALL(5);
                    ii = ii + 1;
                end
            end
        end
    end
    % record the height of the previous bounce
    if XYZ_BALL(5) < previousHeight && goingDown == 0
        goingDown = 1;
        P = previousHeight;
        proportional(ii) = P;
    elseif XYZ_BALL(5) > previousHeight && goingDown == 1
        goingDown = 0;
    end
    previousHeight = XYZ_BALL(5);
    height_ball(ii) = XYZ_BALL(5);
    height_bar(ii) = XZ_BAR(4);
    ii = ii + 1;
end

% de-allocate memory
skeleton(2);

% make plots of data
figure
plot(height_ball);
title('Height of Ball');
figure
stem(proportional);
title('Max Height of Each Bounce');
figure
stem(multArray);
title('Multiplier');
figure
plot(height_bar);
title('Height of Bar');
% figure
% plot(timeArray(1,:));
% figure
% stem(imageNumber,eventTimer);
% title('Time for Event');

% find average height of bounces
j = 1;
for i = 1:length(proportional) 
    if proportional(i) ~= 0 
        temporary(j) = proportional(i); 
        j = j+1; 
    end
end
mean(temporary)

% j = 1;
% for i = 1:length(eventTimer)/3
%     temporary3(j) = mean(eventTimer((3*i)-2:(3*i)));
%     j = j+1;
% end
% figure
% stem(temporary3);
% mean(temporary3)

skeleton.cpp