//***********************************************************
// Author: Mike Konicki
// Date: 2-26-2006
// Project Name: CMU.DLL
//
// Description: Full dll for Com and CMU functions using Karl Reihl's 2005
// Senior Capstone as starter for the comport code
//
//
//**********************************************************
#include <windows.h>
#include <stdio.h>
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//TYPEDEF STRUCTURES
// -These structures exist to facilitate the
// easy sending of data to and from the
// dll to the main program
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//\/\/\/\/\/\/\/\/\
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
typedef struct
{
//M-Packet consists of the following values: Mx,My,X1,Y1,X2,Y2,pixels,confidence
int MX; //x coordinate of the centroid
int MY; //y coordinate of the centroid
int X1; //upper left x coordinate
int Y1; //upper left y coordinate
int X2; //lower right x coordinate
int Y2; //lower right y coordinate
int Num_Pix; //number of matching pixels in the box
int Confid; //confidence that it is the object according to the CMUcam
}M_PACKET;
typedef struct
{
//C-Packet consists of the following values: X1,Y1,X2,Y2,pixels,confidence
int X1; //upper left x coordinate
int Y1; //upper left y coordinate
int X2; //lower right x coordinate
int Y2; //lower right y coordinate
int Num_Pix; //number of matching pixels in the box
int Confid; //confidence that it is the object according to the CMUcam
}C_PACKET;
typedef struct
{
//S-Packet consists of the following values: Rmean,Gmean,Bmean,Rdev,Gdev,Bdev;
int Rmean; //Red Mean Color Value in the Image
int Gmean; //Green Mean Color Value in the Image
int Bmean; //Blue Mean Color Value in the Image
int Rdev; //Red Deviation Color Value in the Image
int Gdev; //Green Mean Color Value in the Image
int Bdev; //Blue Mean Color Value in the Image
}S_PACKET;
typedef struct
{
//M-Packet consists of the following values: Spos,Mx,My,X1,Y1,X2,Y2,pixels,confidence
int Spos; //Servo Position
int MX; //x coordinate of the centroid
int MY; //y coordinate of the centroid
int X1; //upper left x coordinate
int Y1; //upper left y coordinate
int X2; //lower right x coordinate
int Y2; //lower right y coordinate
int Num_Pix; //number of matching pixels in the box
int Confid; //confidence that it is the object according to the CMUcam
}N_PACKET;
typedef struct
{
//COL_PACKET consists of upper and lower bound RGB values for color tracking purposes
//Used with the TC (trackcustomcolor) command
int Rmax; //Upper red bound
int Rmin; //Lower red bound
int Gmax; //Upper green bound
int Gmin; //Lower green bound
int Bmax; //Upper blue bound
int Bmin; //Lower blue bound
}COL_PACKET;
//GLOBAL VARIABLES
static HANDLE ComPort; //Handle to our comport
static OVERLAPPED o; //Overlapped type whcih is used for the com_events
static int IN_PACKET; //bool to keep track of whether or not we are in a packet stream
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//KARL RIEHL'S COMPORT CODE (Modified by Mike Konicki)
// -Modified versions of karl's comport api which
// was modified to allow for ease of use
// and stealth (user doesnt have to touch)
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//\/\/\/\/\/\/\/\/\
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
int _stdcall CMU_SetComSettings(int BaudRate, int ByteSize, DWORD Parity, DWORD StopBits)
//***********************************************************
// Purpose: Sets the commport settings so that communication can occur between the program and the CMUcam
//
// Input: ComPort handle, BaudRate, ByteSize, Parity, StopBits are all communication settings for the comport
//
// Output: 0 on failure, 1 on success
//**********************************************************
{
DCB dcb; //Struct that holds communication settings
//Check Comstate to ensure we can change the settings
if(!GetCommState(ComPort, &dcb))
return 0;
//Fill in the struct with the new communication protocal
dcb.Parity = Parity;
dcb.BaudRate = BaudRate;
dcb.ByteSize = ByteSize;
dcb.StopBits = StopBits;
//Set the protocal on the comport, check for a failure
if(!SetCommState(ComPort, &dcb))
return 0;
return 1; //return 1 on success
}
int _stdcall CMU_SetTimeouts(DWORD ReadInterval, int ReadMultiplier, int ReadConstant, int WriteMultiplier, int WriteConstant)
//**********************************************************
// Purpose: The function sets the length in milliseconds that a function will
// wait before it continues on with the following code.
//
// Input: Timeout Settings and ComPort handle
//**********************************************************
{
COMMTIMEOUTS timeouts;
// Fills the COMMTIMEOUTS structure
if (!GetCommTimeouts(ComPort, &timeouts))
return 0;
// Sets the fields in the structure
timeouts.ReadIntervalTimeout = ReadInterval;
timeouts.ReadTotalTimeoutConstant = ReadConstant;
timeouts.ReadTotalTimeoutMultiplier = ReadMultiplier;
timeouts.WriteTotalTimeoutConstant = WriteConstant;
timeouts.WriteTotalTimeoutMultiplier = WriteMultiplier;
// Sets the new timeouts
if (!SetCommTimeouts(ComPort, &timeouts))
return 0;
return 1;
}
int _stdcall CMU_ClearOutputBuffer()
//**********************************************************
// Purpose: Clears the output buffer of the comport
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
if (!PurgeComm(ComPort, PURGE_TXCLEAR))
return 0;
return 1;
}
int _stdcall CMU_ClearInputBuffer()
//**********************************************************
// Purpose: Clears the input buffer of the comport
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
if (!PurgeComm(ComPort, PURGE_RXCLEAR))
return 0;
return 1;
}
int _stdcall CMU_ClearBuffers()
//**********************************************************
// Purpose: Clears the input & output buffers of the comport (calls the in and out clears)
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
// Clears the output buffer
if (!CMU_ClearOutputBuffer())
return 0;
// Clears the input buffer
if (!CMU_ClearInputBuffer())
return 0;
return 1;
}
int _stdcall CMU_SetBuffer(int Incoming, int Outgoing)
//***********************************************************
// Purpose: Sets the read and write buffers for the com port
//
// Input: Handle to ComPort, incoming, outgoing rates
//
// Output: 0 of fail, 1 on success
//**********************************************************
{
// Sets the new buffer size
if (!SetupComm(ComPort, Incoming, Outgoing))
return 0;
return 1;
}
int _stdcall CMU_ClearWrite()
//**********************************************************
// Purpose: Clears any waiting write commands from the buffer
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
if (!PurgeComm(ComPort, PURGE_TXABORT))
return 0;
return 1;
}
int _stdcall CMU_ClearRead()
//**********************************************************
// Purpose: Clears any waiting reads from the buffer
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
if (!PurgeComm(ComPort, PURGE_RXABORT))
return 0;
return 1;
}
int _stdcall CMU_ClearReadWrite()
//**********************************************************
// Purpose: Clears any waiting reads and/or writes from the buffer
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
// Clears the reads yet to be made
if(!CMU_ClearRead())
return 0;
// Clears the writes yet to be made
if(!CMU_ClearWrite())
return 0;
return 1;
}
int _stdcall CMU_ClearSignal()
//**********************************************************
// Purpose: Lets the port know it is ok to send/recieve data
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
if(!EscapeCommFunction(ComPort, CLRDTR))
return 0;
if(!EscapeCommFunction(ComPort, CLRRTS))
return 0;
return 1;
}
int _stdcall CMU_SetSignal()
//**********************************************************
// Purpose: Lets the port know it is ok to send/recieve data
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
if(!EscapeCommFunction(ComPort, SETDTR))
return 0;
if(!EscapeCommFunction(ComPort, SETRTS))
return 0;
return 1;
}
int _stdcall CMU_PortOpen(char *Port)
//***********************************************************
// Purpose: Opens the port and initializes it to work with the default CMUcam settings
//
// Input: ComPort
//
// Output: 0 on failure, HANDLE(int) on Success
//**********************************************************
{
//Opens the port and sets the handle
ComPort = CreateFile(Port, GENERIC_WRITE | GENERIC_READ, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
//Check to make sure handle is good
if(ComPort == INVALID_HANDLE_VALUE)
return 0;
//Clear any possible signals on the comport
if(!CMU_ClearSignal())
return 0;
//Sets the signal DTR and RTS signals on the comport
if(!CMU_SetSignal())
return 0;
//Clear out the read write buffers
if(!CMU_ClearReadWrite())
return 0;
//Possibly need a sleep(2) here
//Set CMUcam Default communication Rates
if(!CMU_SetComSettings(115200, 8, NOPARITY, ONESTOPBIT))
return 0;
//Set the read and write timeouts
if(!CMU_SetTimeouts( MAXWORD, 0 ,0 ,0 ,0 ))
return 0;
//Set the buffer sizes to max for CMUcam
if(!CMU_SetBuffer( 1028, 1028))
return 0;
//Port Successfully Opened Return the com HANDLE
return 1;
}
void _stdcall CMU_ClosePort()
//***********************************************************
// Purpose: Closes the HANDLE to the comport
//
// Input: Handle to ComPort
//
// Output: None
//**********************************************************
{
CloseHandle(ComPort);
return;
}
int _stdcall CMU_Read(unsigned char* ch, int NumBytesToRead)
//**********************************************************
// Purpose: The function reads in from the input buffer and places the first
// byte at the address that is pointed to by the unsigned char pointer.
//
// Input: ComPort Handle, and read data (blank)
//
// Output: 0 on fail, pointer on success
//**********************************************************
{
DWORD NumBytesRead;
if (!ReadFile(ComPort, ch, NumBytesToRead, &NumBytesRead, 0))
return 0;
return (int)NumBytesRead;
}
int _stdcall CMU_Write(unsigned char* ch, int NumBytesToWrite)
//**********************************************************
// Purpose: The function writes the number of bytes starting at the memory
// location specified by the unsigned char pointer to the port.
//
// Input: ComPort Handle, and data
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
DWORD write;
if (!WriteFile(ComPort, ch, NumBytesToWrite, &write, 0))
return 0;
return 1;
}
int _stdcall CMU_InputBufferSize()
//**********************************************************
// Purpose: The function returns the number of bytes that are currently in the
// input buffer.
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
COMSTAT cs;
DWORD err;
ClearCommError(ComPort, &err, &cs);
//return the size of the buffer
return cs.cbInQue;
}
int _stdcall CMU_OutputBufferSize()
//**********************************************************
// Purpose: The function returns the number of bytes that are
// currently in the output buffer.
//
// Input: ComPort Handle
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
COMSTAT cs;
DWORD err;
ClearCommError(ComPort, &err, &cs);
//return the size of the buffer
return cs.cbOutQue;
}
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//INTERNAL CODE MODULES
// -These two modules handle almost all of the reading
// of the comport one is for streams the other for
// non-stream functions
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//\/\/\/\/\/\/\/\/\
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
int _stdcall INTERNAL_ACKNCK(unsigned char * str, int buffer)
//***************************************************************************
// Purpose: Simple reads in data after a transmission and determines if
// the cmucam got it (ack) or didnt get it (nck)
//
// Input: str --> pointer to string, some functions need to draw info out of it
//
// Output: 0 on nck, 1 on ack
//***************************************************************************
{
DWORD Mask; //Mask for COMEvent
unsigned char test[750]; //buffer to hold data beign read in
int i; //loop counter
int start = 0; //used as a flag when processing acks
int cnt = 0; //used with start when processing
// Needed to set mask for WaitCommEvent
SetCommMask(ComPort, EV_RXCHAR);
//Wait a bit for the camera to process the command
WaitCommEvent(ComPort, &Mask, &o);
//Read from the buffer, for the entire buffer size
if(!CMU_Read(&test[0],buffer)){
return 0;
}
//in case of a framedump we dont want to remove
//the chr 13's since that will hurt our data
if(buffer > 500)
{
str = test;
str[buffer +1] = '\0';
return 1;
}
//Compare the imcoming data to see if we have an ACK or NCKm
//only need to compare the first letter to save time
if((int)'A' == (int)test[0])
{
//need to go through and remove the chr(13) or carraige return
//if it isnt removed passed data comes out funky. We are just
//going to replace it with a Chr(32) or space
for(i=0;i<buffer - 1;i++)
{
if((int)test[i] != 13) //if not enter copy it into our return string
{
//We dont want to save the ACK_
if(start == 1)
{
sprintf(&str[cnt],"%c\0",test[i]);
cnt = cnt + 1;
}//end if
}//end if
else //got a CR need to replace it
{
//we dont want to save until after the ACK which will be marked
//with a Chr(13) we are replacing
if(start == 1)
{
sprintf(&str[cnt],"%c\0",32);
cnt = cnt + 1;
}//end if
else
start = 1;
}//end else
}//end for
return 1;
}
//NCK recieved from CMUcamera something went wrong somewhere
else
{
str = NULL;
return 0;
}
}
int _stdcall INTERNAL_PACKET_PROCESS(int* slot1, int* slot2, int* slot3, int* slot4, int* slot5, int* slot6, int* slot7, int* slot8, int* slot9, int buffer)
//***************************************************************************
// Purpose: Reads data from the packet streams (M,N,C,S) and splits it up
// into (upto) 9 different int* which will get sent back to the
// function that called it. That function knows what numbers are
// what and actually fills in teh data structs for the main program
//
// Input: int* slot(1-9) --> holds the various numbers returned from the packet stream
// buffer --> how much to read out of the packet stream before processing
//
// Output: 0 on nck, 1 on ack
//***************************************************************************
{
DWORD Mask; //Mask for COMEvent
unsigned char test[150]; //input of packet
int startofpacket; //mark start of a packet string
int i; //Int for loop counter
char* tok1; //Used for token ripping
char* tok2; //Used for token ripping
char* tok3; //Used for token ripping
char* tok4; //Used for token ripping
char* tok5; //Used for token ripping
char* tok6; //Used for token ripping
char* tok7; //Used for token ripping
char* tok8; //Used for token ripping
char* tok9; //Used for token ripping
// Needed to set mask for WaitCommEvent
SetCommMask(ComPort, EV_RXCHAR);
//Wait a bit for the camera to process the command
WaitCommEvent(ComPort, &Mask, &o);
//Read from the buffer, for the entire buffer size
if(!CMU_Read(&test[0],buffer)){
return 0;
}
//Removes all Chr(13)'s from the string
//if strtok hits a Chr(13) it will generate
//a nasty exception error
for(i=0;i<buffer;i++)
{
if((int)test[i] == 13)
{
sprintf(&test[i],"%c\0",32);
}
}
//Go through the loop and find start of the packet
for(i=0;i<buffer;i++)
{
if((int)test[i] == 0)
{
//Check to see if it is a start of a packet in which
//case we need to add 1 to the start for strtok
if((int)test[i+1] == (int)'S' || (int)test[i+1] == (int)'C' || (int)test[i+1] == (int)'M' || (int)test[i+1] == (int)'N'){
startofpacket = i + 3;
break;
}
//if it is not the start of a packet then we
//dont have to worry about the extra char in the packet
else{
startofpacket = i + 2;
break;
}
}//end big if
}//end for
//Signal that we are inside a packet already
//so ping knows this in order to clear buffers
//after a call or not
IN_PACKET = 1;
//******************************************
//use strtok to rip apart the packets
//******************************************
//See if the first token is ok, if it fails error
tok1 = strtok(&test[startofpacket]," ");
if(tok1 == NULL)
return 0;
else
*slot1 = atoi(tok1);
//See if 2nd token is ok, if it fails error
tok2 = strtok(NULL," ");
if(tok2 == NULL)
return 0;
else
*slot2 = atoi(tok2);
//See if 3rd token is ok, if it fails error
tok3 = strtok(NULL," ");
if(tok3 == NULL)
return 0;
else
*slot3 = atoi(tok3);
//See if 4th token is ok, if it fails error
tok4 = strtok(NULL," ");
if(tok4 == NULL)
return 0;
else
*slot4 = atoi(tok4);
//See if 5th token is ok, if it fails error out
tok5 = strtok(NULL," ");
if(tok5 == NULL)
return 0;
else
*slot5 = atoi(tok5);
//See if 6th token is ok, if it fails error out
tok6 = strtok(NULL," ");
if(tok6 == NULL)
return 0;
else
*slot6 = atoi(tok6);
//See if 7th token is ok, if it fails its ok since packet size
//can be 6 for 'S' or 'C' packets
tok7 = strtok(NULL," ");
if(tok7 == NULL)
return 1;
else
*slot7 = atoi(tok7);
//See if token 8 is ok, if it fails error out no token size of 7
tok8 = strtok(NULL," ");
if(tok8 == NULL)
return 0;
else
*slot8 = atoi(tok8);
//check token 9 if it fails return success 'M' packet is
//size 8
tok9 = strtok(NULL," ");
if(tok9 == NULL)
return 1;
else
*slot9 = atoi(tok9);
return 1;
}
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//CAMERA FUNCTIONS (CMU_Cam routines)
// -These fucntions prepare and send messages to
// the CMUcam. The internal functions do the
// actual reading though
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//\/\/\/\/\/\/\/\/\
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
int _stdcall CAM_Ping()
//**********************************************************
// Purpose: Pings the Camera to check for activity
//
// Input: None
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
unsigned char msg[1];
unsigned char * ret;
unsigned char store[20];
//Clear the buffers since a ping also serves as the stream killer for packets
if(!CMU_ClearBuffers())
return 0;
//Give our pointer something to point to
ret = store;
//Send the Command to ping CMUcam
msg[0] = '\r';
//****************************
//This is if we are currently in a packet stream.
//EndPacket calls ping which is the safest way to
//to destroy a packet stream since it will not
//reset any of the registers currently set by the user
//****************************
if(IN_PACKET == 1)
{
//Send it once, to stop packets from coming in
if(!CMU_Write(&msg[0],1))
return 0; //Failed return 0
//Delay so the packet stream can be terminated
Sleep(100);
//Clear the buffers since a ping also serves as the stream killer for packets
//and clean out the packet data in the buffer. YES the ack/nck on the ping is lost
//but all we are concerned with here is stopping the packet stream
if(!CMU_ClearBuffers())
return 0;
Sleep(100);
IN_PACKET = 0;
}
//Send it
if(!CMU_Write(&msg[0],1))
return 0; //Failed return 0
//Make sure the packet went through
if(!INTERNAL_ACKNCK(ret, 5))
return 0;
//Return Success
return 1;
}//End CAM_Ping
int _stdcall CAM_Init(char * port)
//**********************************************************
// Purpose: Calls the routine to setup up the port for the CMUcam,
//
// Input: Port Address
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
//Open the port to the CMUcam
if(!CMU_PortOpen(port))
return 0;
// Creates an event for the OVERLAPPED structure
o.hEvent = CreateEvent(
NULL, // default security attributes
FALSE, // auto reset event
FALSE, // not signaled
NULL // no name
);
//Intialize the rest of the OVERLAPPED structure to zero. which
//is used for the com_event functions
o.Internal = 0;
o.InternalHigh = 0;
o.Offset = 0;
o.OffsetHigh = 0;
//Delay to let the camera finish connecting
Sleep(100);
//Return we got a good port
return 1;
}//End CAM_Init
int _stdcall CAM_TrackLight(unsigned char* mode)
//**********************************************************
// Purpose: Sets the tracking light to on, off, or auto mode
//
// Input: mode (0,1,2)
//
// Output: 0 on fail, 1 on success
//**********************************************************
{
unsigned char msg[20]; //Message being sent to the CMUcam
unsigned char * ret;
unsigned char store[20];
//Give our pointer something to point to else we get warnings when compiling
ret = store;
//Check to make sure our input is valid (0,1,2 only)
if (mode != '0' && mode != '1' && mode != '2')
return 0;
//Build the command string for the CMUcam (Tracking Light)
msg[0] = 'L';
msg[1] = '1';
msg[2] = ' ';
msg[3] = mode;
msg[4] = '\r';
//send the command
if(!CMU_Write(&msg[0],5))
return 0;
//Check to see the command went through
if(!INTERNAL_ACKNCK(ret, 5))
return 0;
return 1;
}//End CAM_TrackLight
int _stdcall CAM_ClockSpeed(unsigned char speed)
//**********************************************************
// Purpose: Sets the clockspeed to the user selected amout
//
// Input: speed(2,3,4,5,6,7,8)
//
// Output: 0 on fail, 1 on success
//**********************************************************
{