// This method takes new data from the camera and inserts it, with proper timing, into the queue to be processed by the consumerd
// the data consumer delegate that processes and displays the data
public void onNewData(ushort[] data)
{
int i;
int curFrame;
Frameco++;
if (quittingBool)
{
return;
}
curFrame = currentData.AddData(data, Frameco); // Returns the number of frames in the RawData object
if (curFrame == bufferSize)
{
try
{
for (i = 0; i < consumerd.Length; i++)
{
lock (((ICollection)consumerd[i].myQueue).SyncRoot)
{
consumerd[i].myQueue.Enqueue(currentData);
}
consumerd[i].mySyncEvent.NewItemEvent.Set();
}
}
catch (System.Threading.ThreadAbortException) { }
catch (Exception ex)
{
EmailError.emailAlert(ex);
throw (ex);
}
currentData = new RawData(bufferSize);
}
}
//Camera initialization
public void initCamera()
{
try
{
myCamera = new Camera();
myCamera.cameraInit(cameraType);
Camera.dataDelegate dd = new Camera.dataDelegate(onNewData);
myCamera.startFrameGrab(0x8888, 0, dd, cameraType);
cameraStatus = 1;
}
catch (System.Threading.ThreadAbortException) { }
catch (Exception ex)
{
cameraStatus = 0;
//Camera status bit
ds = new AdsStream(4);
BinaryWriter bw = new BinaryWriter(ds);
bw.Write(cameraStatus);
if (twinCatBool)
{
tcAds.Write(0x4020, 40, ds);
}
EmailError.emailAlert(ex);
throw (ex);
}
}
public Form1()
{
try
{
initTime = DateTime.Now;
//Calls calculation method for filters
highCoeff = filterCoeff(double.Parse(ConfigurationManager.AppSettings.Get("angleHighPass")), sampFreq / bufferSize, "High");
lowCoeff = filterCoeff(double.Parse(ConfigurationManager.AppSettings.Get("angleLowPass")), sampFreq / bufferSize, "Low");
bandHighCoeff = filterCoeff(double.Parse(ConfigurationManager.AppSettings.Get("velocityHighPass")), sampFreq / bufferSize, "High");
bandLowCoeff = filterCoeff(double.Parse(ConfigurationManager.AppSettings.Get("velocityLowPass")), sampFreq / bufferSize, "Low"); //2*10^-2
InitializeComponent(); // Initializes the visual components
SetSize(); // Sets up the size of the window and the corresponding location of the components
Frameco = 0;
dayFrameCo0 = DayNr.GetDayNr(DateTime.Now);
//Initialization of TwinCAT connection. 851 is the port for PLC runtime 1
if (twinCatBool)
{
tcAds.Connect(851);
}
myStopwatch = new Stopwatch();
dataWritingSyncEvent = new SyncEvents();
consumerd = new DataConsumerDelegate[2];
consumerd[0] = new DataConsumerDelegate(new DataConsumerDelegate.outputDelegate(showStatistics), true);
consumerd[1] = new DataConsumerDelegate(new DataConsumerDelegate.outputDelegate(Pattern), true);
consumerd[1].myThread.Priority = ThreadPriority.Highest;
for (int i = 0; i < consumerd.Length; i++)
{
consumerd[i].myThread.Start();
}
dataWritingQueue = new Queue <PeakQueueItem>();
dataWritingThreadBool = true;
dataWritingThread = new Thread(dataWrite);
dataWritingThread.Priority = ThreadPriority.Highest;
dataWritingThread.Start();
Thread.Sleep(10);
cameraThread = new Thread(initCamera);
cameraThread.Priority = ThreadPriority.Highest;
cameraThread.Start();
curDirec = System.IO.Path.GetDirectoryName(Application.ExecutablePath);
System.Diagnostics.Process.Start(curDirec + "\\AffinitySet.bat");
}
catch (System.Threading.ThreadAbortException) { }
catch (Exception ex)
{
EmailError.emailAlert(ex);
throw (ex);
}
}
// This functions processes a pattern, obtains the data and send it out to be written
private void Pattern(RawData data)
{
PeakQueueItem quI;
int ql;
long[] timestamps;
double fitLength = 15; //Amount of pixels to be used in fit of correlation
int halflength = (int)Math.Floor(fitLength / 2) + 1; // increased by 1 pixel to allow two fits
int length = patternLength; //Length of patterns
double[] crossCor = new double[(int)fitLength + 2]; // increased by 2 pixels to allow two fits
int startIndexRight = splitPixel; //Beginning of left pattern
int startIndexLeft = 0; //Beginning of left pattern
int endIndexRight = camWidth;
int pixshift = 1; // Direction of shift required to estimate slope of fit correction
float sum = 0;
double[] offset = new double[1];
double[] fit = new double[4];
double mu = 0;
double mu1 = 0;
double mu2 = 0;
double N = fitLength;
double y = 0;
double xy = 0;
double xxy = 0;
double b, c, D, Db, Dc;// a,b, and c are the values we solve for then derive mu,sigma, and A from them.
if (quittingBool)
{
return;
}
timestamps = new long[bufferSize];
newdata = new double[bufferSize, 2];
for (int frameNo = 0; frameNo < bufferSize; frameNo++)
{
try
{
frame = data.getData(frameNo);
if (firstFrame == true)
{
refFrame = frame;
firstFrame = false;
for (int j = splitPixel; j < frame.Length; j++)
{
if (frame[j] > threshold)
{
startIndexRightRef = j - pixelMargin;
break;
}
}
for (int j = 0; j < frame.Length; j++)
{
if (frame[j] > threshold)
{
startIndexLeftRef = j - pixelMargin;
break;
}
}
x = 0;
xSquar = 0;
xCube = 0;
xFourth = 0;
for (int j = 0; j < fitLength; j++)
{
x += j;
xSquar += j * j;
xCube += j * j * j;
xFourth += j * j * j * j;
}
}
//Finds beginning of pattern using a threshold
for (int j = splitPixel; j < frame.Length; j++)
{
if (frame[j] > threshold)
{
startIndexRight = j - pixelMargin;
break;
}
if (j == frame.Length - 1)
{
startIndexRight = 0;
}
}
Array.Copy(frame, flipFrame, frame.Length);
Array.Reverse(flipFrame);
for (int j = 0; j < flipFrame.Length; j++)
{
if (flipFrame[j] > threshold)
{
endIndexRight = flipFrame.Length - j + pixelMargin;
break;
}
if (j == frame.Length - 1)
{
endIndexRight = flipFrame.Length;
}
}
if (startIndexRight >= frame.Length || startIndexRight <= splitPixel || (endIndexRight - startIndexRight) < length)
{
timestamps[frameNo] = data.TimeStamp(frameNo);
newdata[frameNo, 0] = angleLastValue;
newdata[frameNo, 1] = refLastValue;
}
else
{
//Cuts length of pattern down if the pattern extends beyond the frame
while (length + startIndexRight + halflength + 1 >= frame.Length)
{
length = (int)Math.Round(length / 1.1);
}
//Calcualtes the crosscorrelation between the two patterns at shifts ; first time
for (int k = -halflength; k <= halflength; k++)
{
sum = 0;
for (int m = 0; m < length; m++)
{
if ((m + startIndexRight + k) > 0 && (m + startIndexRightRef) > 0)
{
if ((m + startIndexRight + k) < frame.Length && (m + startIndexRightRef) < refFrame.Length)
{
sum += frame[m + startIndexRight + k] * refFrame[m + startIndexRightRef];
}
}
}
crossCor[k + halflength] = sum;
}
//Sums x,x^2,x^3,x^4,ln(y),x ln(y),x^2 ln(y)
y = 0;
xy = 0;
xxy = 0;
for (int j = 0; j < fitLength; j++)
{
y += Math.Log(crossCor[j + 1]);
xy += j * Math.Log(crossCor[j + 1]);
xxy += j * j * Math.Log(crossCor[j + 1]);
}
//Solves system of equations using Cramer's rule
D = N * (xSquar * xFourth - xCube * xCube) - x * (x * xFourth - xCube * xSquar) + xSquar * (x * xCube - xSquar * xSquar);
//Da = y * (xSquar * xFourth - xCube * xCube) - x * (xy * xFourth - xCube * xxy) + xSquar * (xy * xCube - xSquar * xxy);
Db = N * (xy * xFourth - xCube * xxy) - y * (x * xFourth - xCube * xSquar) + xSquar * (x * xxy - xy * xSquar);
Dc = N * (xSquar * xxy - xy * xCube) - x * (x * xxy - xy * xSquar) + y * (x * xCube - xSquar * xSquar);
//a = Da / D;
b = Db / D;
c = Dc / D;
mu1 = -b / (2 * c);
// If fit-center is to left of center of crosscor pattern, shift cross-cor pattern by 1 pixel to right or vice versa
if (mu1 < (halflength - 1))
{
pixshift = -1;
}
else
{
pixshift = 1;
}
//Redo the fit
y = 0;
xy = 0;
xxy = 0;
for (int j = 0; j < fitLength; j++)
{
y += Math.Log(crossCor[j + 1 + pixshift]);
xy += j * Math.Log(crossCor[j + 1 + pixshift]);
xxy += j * j * Math.Log(crossCor[j + 1 + pixshift]);
}
D = N * (xSquar * xFourth - xCube * xCube) - x * (x * xFourth - xCube * xSquar) + xSquar * (x * xCube - xSquar * xSquar);
Db = N * (xy * xFourth - xCube * xxy) - y * (x * xFourth - xCube * xSquar) + xSquar * (x * xxy - xy * xSquar);
Dc = N * (xSquar * xxy - xy * xCube) - x * (x * xxy - xy * xSquar) + y * (x * xCube - xSquar * xSquar);
b = Db / D;
c = Dc / D;
mu2 = -b / (2 * c);
mu = (halflength - 1) - (mu1 - (halflength - 1)) * pixshift / (mu2 - mu1);
newdata[frameNo, 0] = mu + startIndexRight;
timestamps[frameNo] = data.TimeStamp(frameNo);
angleLastValue = mu + startIndexRight;
y = 0;
xy = 0;
xxy = 0;
//Finds beginning of pattern using a threshold
if (frameNo == 0)
{
for (int j = 0; j < frame.Length; j++)
{
if (frame[j] > threshold)
{
startIndexLeft = j - pixelMargin;
lightSourceStatus = 1;
break;
}
if (j == frame.Length - 1)
{
lightSourceStatus = 0;
}
}
if (startIndexLeft < 0)
{
startIndexLeft = 0;
}
}
//Calcualtes the crosscorrelation between the two patterns at shifts
for (int k = -halflength; k <= halflength; k++)
{
sum = 0;
for (int m = 0; m < length; m++)
{
if ((m + startIndexLeft + k) > 0 && (m + startIndexLeftRef) > 0)
{
sum += frame[m + startIndexLeft + k] * refFrame[m + startIndexLeftRef];
}
}
crossCor[k + halflength] = sum;
}
//Sums x,x^2,x^3,x^4,ln(y),x ln(y),x^2 ln(y)
for (int j = 0; j < fitLength; j++)
{
y += Math.Log(crossCor[j + 1]);
xy += j * Math.Log(crossCor[j + 1]);
xxy += j * j * Math.Log(crossCor[j + 1]);
}
//Solves system of equations using Cramer's rule
D = N * (xSquar * xFourth - xCube * xCube) - x * (x * xFourth - xCube * xSquar) + xSquar * (x * xCube - xSquar * xSquar);
//Da = y * (xSquar * xFourth - xCube * xCube) - x * (xy * xFourth - xCube * xxy) + xSquar * (xy * xCube - xSquar * xxy);
Db = N * (xy * xFourth - xCube * xxy) - y * (x * xFourth - xCube * xSquar) + xSquar * (x * xxy - xy * xSquar);
Dc = N * (xSquar * xxy - xy * xCube) - x * (x * xxy - xy * xSquar) + y * (x * xCube - xSquar * xSquar);
//a = Da / D;
b = Db / D;
c = Dc / D;
mu1 = -b / (2 * c);
// If fit-center is to left of center of crosscor pattern, shift cross-cor pattern by 1 pixel to right or vice versa
if (mu1 < (halflength - 1))
{
pixshift = -1;
}
else
{
pixshift = 1;
}
//Redo the fit
y = 0;
xy = 0;
xxy = 0;
for (int j = 0; j < fitLength; j++)
{
y += Math.Log(crossCor[j + 1 + pixshift]);
xy += j * Math.Log(crossCor[j + 1 + pixshift]);
xxy += j * j * Math.Log(crossCor[j + 1 + pixshift]);
}
D = N * (xSquar * xFourth - xCube * xCube) - x * (x * xFourth - xCube * xSquar) + xSquar * (x * xCube - xSquar * xSquar);
Db = N * (xy * xFourth - xCube * xxy) - y * (x * xFourth - xCube * xSquar) + xSquar * (x * xxy - xy * xSquar);
Dc = N * (xSquar * xxy - xy * xCube) - x * (x * xxy - xy * xSquar) + y * (x * xCube - xSquar * xSquar);
b = Db / D;
c = Dc / D;
mu2 = -b / (2 * c);
mu = (halflength - 1) - (mu1 - (halflength - 1)) * pixshift / (mu2 - mu1);
newdata[frameNo, 1] = mu + startIndexLeft;
refValue = mu + startIndexLeft;
newdata[frameNo, 0] = newdata[frameNo, 0] - refValue;
refLastValue = refValue;
}
}
catch (System.Threading.ThreadAbortException) { }
catch (Exception ex)
{
EmailError.emailAlert(ex);
timestamps[frameNo] = data.TimeStamp(frameNo);
newdata[frameNo, 0] = angleLastValue;
newdata[frameNo, 1] = refLastValue;
throw ex;
}
sum = frame.Select(x => (int)x).Sum();
Debug.WriteLine(sum);
if (sum == 0)
{
lightSourceStatus = 0;
}
else
{
lightSourceStatus = 1;
}
}
quI = new PeakQueueItem(timestamps, newdata);
lock (((ICollection)dataWritingQueue).SyncRoot)
{
while (dataWritingQueue.Count > 10)
{
dataWritingQueue.Dequeue();
}
dataWritingQueue.Enqueue(quI);
ql = dataWritingQueue.Count;
}
dataWritingSyncEvent.NewItemEvent.Set();
setTextBox2(data.QueueLen.ToString());
setTextBox3(ql.ToString());
if (quittingBool)
{
return;
}
}