Code for a Robotics + Computer Vision Competition using OpenCV and C++

Avalanche

/*											FINAL CODE FORMAT
        This code was written for a Computer Vision Competition using OpenCV and Visual Studio C++ about 4 years back.
        The purpose was to catch specific coloured balls rolling down a slope at the bottom of which are bot lay.
        Our thresholding functions didn't generalise to new environments very well.
*/

#include "stdafx.h"
#include <cv.h>
#include <highgui.h>

// Record the execution time of some code, in milliseconds.
#define DECLARE_TIMING(s)  int64 timeStart_##s; double timeDiff_##s; double timeTally_##s = 0; int countTally_##s = 0
#define START_TIMING(s)    timeStart_##s = cvGetTickCount()
#define STOP_TIMING(s) 	   timeDiff_##s = (double)(cvGetTickCount() - timeStart_##s); timeTally_##s += timeDiff_##s; countTally_##s++
#define GET_TIMING(s) 	   (double)(timeDiff_##s / (cvGetTickFrequency()*1000.0))
#define DEALLOC()			cvReleaseCapture(&Cap);	cvDestroyAllWindows()

class Communication_Module
{
	private:
		HANDLE hPort; 
	public:
		bool SerialOpen(LPCWSTR strPort);
		bool SerialWrite(BYTE theByte);
		bool SerialClose(void);
}Com;

class Image_Processing
{
	private:	
		IplImage* IY;		IplImage* IR;		IplImage* IB;		IplImage* IBk;
	public:
		IplImage* IF;
		CvSeq** IProcess(IplImage* IF);
		~Image_Processing(){cvDestroyAllWindows(); }
}IProc;

class Bot_Logic
{
	public:
		void Move(CvSeq*, CvSeq*, CvSeq*);
		void Deposit(CvSeq*, CvSeq*);
		bool Caught(CvSeq*, CvSeq*);
}Bot;


int main()
{
		//Initialisation
		DECLARE_TIMING(T);		CvSeq** C = 0;
		CvCapture* Cap = cvCreateCameraCapture(0);	
		IProc.IF = cvQueryFrame(Cap);
				
		//Parallel Port Opened
		if(!Com.SerialOpen(L"LPT1:"))	{ exit(111);	}
		
		while(1) 
		{
			START_TIMING(T);				//Starting Timer
			IProc.IF = cvQueryFrame(Cap);	//Obtain Image To Process
			if(!IProc.IF) break;
			
			C = IProc.IProcess(IProc.IF);	//Image Processed & Coordinates Of The Different Balls Obtained
			
			if(Bot.Caught(*C, *(C+1)))
				Bot.Deposit(*(C+1),*(C+3));
			else if(Bot.Caught(*C, *(C+2)))
				Bot.Deposit(*(C+2),*(C+3));

			if((*C)->total > 0)
			{
				if	    ((*(C+2))->total > 0)	{	Bot.Move(*C, *(C+2), *(C+3));	}
				
				else if ((*(C+1))->total > 0)	{	Bot.Move(*C, *(C+1), *(C+3));	}
			}

			cvShowImage("Video", IProc.IF);		
			STOP_TIMING(T);			printf("%fms\n",GET_TIMING(T));

			//Termination Condition
			if((char)cvWaitKey(33) == 27 ) break;
		}
		DEALLOC();			//Deallocation
		Com.SerialClose();	//Port Closed
		return 0;
}

CvSeq** Image_Processing::IProcess(IplImage* IF)
{
		IplImage* IY  = cvCreateImage(cvGetSize(IF), 8, 1);
		IplImage* IR  = cvCreateImage(cvGetSize(IF), 8, 1);
		IplImage* IB  = cvCreateImage(cvGetSize(IF), 8, 1);
		IplImage* IBk = cvCreateImage(cvGetSize(IF), 8, 1);

		cvCvtColor(IF, IF, CV_BGR2HSV);	//RGB To HSV Conversion
		cvCvtScale(IF, IF, 1, 0);		//Raw Data->Floating Point Image
		
		//Black Thresh
		cvInRangeS(IF, cvScalar(0, 0, 0), cvScalar(255, 255, 40), IBk);
		//Yellow Thresh
		cvInRangeS(IF, cvScalar(20, 110, 110), cvScalar(30, 255, 255), IY);
		//Red Thresh
		cvInRangeS(IF, cvScalar(170, 110, 90), cvScalar(180, 255, 255), IR);
		//Blue Thresh
		cvInRangeS(IF, cvScalar(105, 100, 70), cvScalar(115, 255, 255), IB);
		//White Thresh		
		cvInRangeS(IF, cvScalar(0, 5, 0), cvScalar(255, 70, 255), IW);

		//Morphological Closing And Noise Reduction in Thresholded Images	
		cvMorphologyEx(IY, IY, 0, 0, CV_MOP_CLOSE, 25);	

		cvErode(IR,IR,cvCreateStructuringElementEx(5,5,0,0,CV_SHAPE_ELLIPSE,0),1);
		cvMorphologyEx(IR, IR, 0, 0, CV_MOP_CLOSE, 15);
		
		cvErode(IB,IB,cvCreateStructuringElementEx(5,5,0,0,CV_SHAPE_ELLIPSE,0),1);
		cvMorphologyEx(IB, IB, 0, 0, CV_MOP_CLOSE, 25);
		
		cvDilate(IBk,IBk,cvCreateStructuringElementEx(5,5,0,0,CV_SHAPE_ELLIPSE,0),1);
		cvMorphologyEx(IBk, IBk, 0,0, CV_MOP_CLOSE, 1);
		
		//Obtaining Hough Circles Of The Thresholded Circluar Objects
		CvMemStorage* Store = cvCreateMemStorage(0);
		CvSeq* Y = cvHoughCircles( IY, Store, CV_HOUGH_GRADIENT,  2, IY->height/5, 120, 30, 30, 110);
		CvSeq* R = cvHoughCircles( IR, Store, CV_HOUGH_GRADIENT,  2, IR->height/5, 120, 30, 30, 110);
		CvSeq* B = cvHoughCircles( IB, Store, CV_HOUGH_GRADIENT,  2, IB->height/5, 120, 30, 40, 100);
		CvSeq* N = cvHoughCircles( IBk, Store, CV_HOUGH_GRADIENT, 2, IBk->height/2, 120, 30, 10, 20);
			
		CvSeq** C = (CvSeq**) malloc(sizeof(CvSeq)*4);
		* C		 = Y;		*(C + 1) = R;		*(C + 2) = B;		*(C + 3) = N;
		
		cvNamedWindow("Yellow" , CV_WINDOW_AUTOSIZE);
		cvNamedWindow("Red" , CV_WINDOW_AUTOSIZE);
		cvNamedWindow("Blue" , CV_WINDOW_AUTOSIZE);
		cvNamedWindow("Black" , CV_WINDOW_AUTOSIZE);

		cvShowImage("Yellow", IY);
		cvShowImage("Red", IR);
		cvShowImage("Blue", IB);
		cvShowImage("Black", IBk);
		
		return C;
}
void Bot_Logic::Move(CvSeq* Y, CvSeq* B, CvSeq* N)
{
}
void Bot_Logic::Deposit(CvSeq* B, CvSeq* Y)
{
}
bool Bot_Logic::Caught(CvSeq* B, CvSeq* Y)
{
	return 0;
}

void Bot_Logic::Move(CvSeq* Y, CvSeq* B, CvSeq* N)
{
		float *p = (float *)cvGetSeqElem( Y, 0 );
		float *q = (float *)cvGetSeqElem( B, 0 );
		float *r = (float *)cvGetSeqElem( N, 0 );
		float *d = Distance(p, q);

		if(d < Threshold)//Distance b/w Ball and Bot below Threshold
			return 0;

		if(Distance(p, r) > 30)//Distance b/w Black Ball And Bot Sufficient&& [Bot Not Moving in Black Balls Direction(?)]
		{
			SerialWrite(d*Ryt);
		}
}

void Bot_Logic::Deposit(cvSeq* B, cvSeq* N)
{
	while(Distance(B, N)>Threshold1 && Distance(B,Hole)>Threshold2)
	{
		//Move To Correct Hole
	}
	Deposit2(int Hole);//As Different Deposit Manuevores For Different Hole
		return 1;
}

bool Bot_Logic::Caught(cvSeq* Y, cvSeq* B)
{
	float *p = (float *)cvGetSeqElim( Y, 0 );
	float *q = (float *)cvGetSeqElim( B, 0 );
	
	if(Distance b/w Bot And Ball Being Caught Less than Threshold)
		return 1;
	else return 0;
}

bool Communication_Module::SerialOpen(LPCWSTR strPort)
{
	//Open The Port
	hPort= (HANDLE)CreateFile(strPort,// Pointer to the name of the port
	GENERIC_READ | GENERIC_WRITE,// Access (read-write) mode
	0, // Share mode
	NULL, // Pointer to the security attribute
	OPEN_EXISTING,// How to open the serial port
	0, // Port attributes
	(long)NULL); // Handle to port with attribute
	
	// To copy
	DCB PortDCB;	DWORD dwError;	

	// Initialize the DCBlengthmember.
	PortDCB.DCBlength= sizeof(DCB);

	// Get the default port setting information.
	GetCommState(hPort, &PortDCB);

	// Change the DCB structure settings.
	PortDCB.BaudRate= 9600; // Current baud
	PortDCB.fBinary= TRUE; // Binary mode; no EOF check
	PortDCB.fParity= TRUE; // Enable parity checking
	PortDCB.fOutxCtsFlow= FALSE; // No CTS output flow control
	PortDCB.fOutxDsrFlow= FALSE; // No DSR output flow control
	PortDCB.fDtrControl= DTR_CONTROL_ENABLE; // DTR flow control type
	PortDCB.fDsrSensitivity= FALSE; // DSR sensitivity
	PortDCB.fTXContinueOnXoff= TRUE; // XOFF continues Tx
	PortDCB.fOutX= FALSE; // No XON/XOFF out flow control
	PortDCB.fInX= FALSE; // No XON/XOFF in flow control
	PortDCB.fErrorChar= FALSE; // Disable error replacement
	PortDCB.fNull= FALSE; // Disable null stripping
	PortDCB.fRtsControl= RTS_CONTROL_ENABLE; // RTS flow control
	PortDCB.fAbortOnError= FALSE; // Do not abort reads/writes on error
	PortDCB.ByteSize= 8; // Number of bits/byte, 4-8
	PortDCB.Parity= NOPARITY; // 0-4=no,odd,even,mark,space
	PortDCB.StopBits= ONESTOPBIT; // 0,1,2 = 1, 1.5, 2

	// Configure the port according to the specifications of the DCB structure.
	if(!SetCommState(hPort, &PortDCB))
	{
		// Could not configure the serial port.
		dwError= GetLastError();
		printf("Serial port creation error: %d", dwError);
		MessageBox(NULL, L"Unable to configure the serial port", L"Error", MB_OK);
	
		return false;
	}

	return true;
}

bool Communication_Module::SerialWrite(BYTE theByte)
{
	//The port wasn't opened
	if(!hPort)
		return false;

	DWORD dwNumBytesWritten;
	WriteFile(	hPort,				//Port handle
				&theByte,			//Pointer to the data to write
				1,					//Number of bytes to write
				&dwNumBytesWritten, //Pointer to the number of bytes written
				NULL				//Must be NULL
				);
				return true;
}

bool Communication_Module::SerialClose()
{
	if(!hPort)
		return false;
	
	CloseHandle(hPort);
	return true;
}