Exemple #1
0
        // 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);
            }
        }
Exemple #2
0
 //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);
     }
 }
Exemple #3
0
        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);
            }
        }
Exemple #4
0
        // 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;
            }
        }