[mjcoss]

Here's the code that we used. Worked fine for what we needed, and hopefully helps you.

#include "WPILib.h"
#include "Tracker.h"
#include "sockLib.h" 
#include "inetLib.h" 
#include "stdioLib.h" 
#include "strLib.h" 
#include "ioLib.h" 
#include "fioLib.h"

#define BAD_ANGLE			361.0
#define BAD_DISTANCE		-1.0

struct request { 
	char message[1023];     /* message buffer */ 
};

int
udpServer(Tracker *p) 
{ 
	struct sockaddr_in  serverAddr;    /* server's socket address */ 
	struct sockaddr_in  clientAddr;    /* client's socket address */ 
	struct request      clientRequest; /* request/Message from client */ 
	int                 sockAddrSize;  /* size of socket address structure */ 
	int                 sFd;           /* socket file descriptor */ 
	char                inetAddr[INET_ADDR_LEN];
	Tracker::targetInfo_t 		myinfo;
	

	sockAddrSize = sizeof(struct sockaddr_in); 
    bzero((char *)&serverAddr, sockAddrSize);
    serverAddr.sin_family = AF_INET; 
    serverAddr.sin_port = htons(p->getPort()); 
    serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
    
	if((sFd = socket(AF_INET, SOCK_DGRAM, 0)) == ERROR) { 
		perror("socket"); 
		return ERROR;
	}
	
    if(bind(sFd, (struct sockaddr *)&serverAddr, sockAddrSize) == ERROR) { 
    	perror("bind");
    	close(sFd);
    	return ERROR;
    }
    for(;;) {
       	char *ptr = clientRequest.message;
 
        if(recvfrom(sFd, (char *)&clientRequest, sizeof(clientRequest), 0, 
            (struct sockaddr *)&clientAddr, &sockAddrSize) == ERROR) {
            perror("recvfrom");
            close(sFd);
            return ERROR;
        }
        
        inet_ntoa_b(clientAddr.sin_addr, inetAddr);
        
        p->packetsReceived++;
       	memcpy((char *)&(myinfo.angle),    ptr, sizeof(myinfo.angle));    ptr += sizeof(myinfo.angle);
       	memcpy((char *)&(myinfo.distance), ptr, sizeof(myinfo.distance)); ptr += sizeof(myinfo.distance);
       	myinfo.isvalid = *ptr++;
       	p->setTarget(&myinfo);
	} 
 }

Tracker::Tracker(Solenoid *inner, Solenoid *outer, ushort port)
{
	m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
	m_tinfo.isvalid  = false;
	m_tinfo.angle    = BAD_ANGLE;
	m_tinfo.distance = BAD_DISTANCE;
	m_port  = port;
	m_inner = inner;
	m_outer = outer;
	m_needFree = false;
	m_task = new Task("udpDashInfo", (FUNCPTR)udpServer);
	m_task->Start((UINT32)this);
	packetsReceived = 0;
	return;
}

Tracker::Tracker(Solenoid &inner, Solenoid &outer, ushort port)
{
	m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
	m_tinfo.isvalid  = false;
	m_tinfo.angle    = BAD_ANGLE;
	m_tinfo.distance = BAD_DISTANCE;
	m_port  = port;
	m_inner = &inner;
	m_outer = &outer;
	m_needFree = false;
	m_task = new Task("udpDashInfo", (FUNCPTR)udpServer);
	m_task->Start((UINT32)this);
	packetsReceived = 0;
	return;
}

Tracker::Tracker(UINT8 inner, UINT8 outer, ushort port)
{
	m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
	m_tinfo.isvalid  = false;
	m_tinfo.angle    = BAD_ANGLE;
	m_tinfo.distance = BAD_DISTANCE;
	m_port  = port;
	m_inner = new Solenoid(inner);
	m_outer = new Solenoid(outer);
	m_needFree = true;
	m_task = new Task("udpDashInfo", (FUNCPTR)udpServer);
	m_task->Start((UINT32)this);
	packetsReceived = 0;
	return;
}

Tracker::~Tracker()
{
	semFlush(m_semaphore);
	if(m_task) {
		m_task->Stop();
		delete m_task;
	}
	if(m_needFree) {
		delete m_inner;
		delete m_outer;
	}
	return;
}

bool
Tracker::targetAquired()
{
	return m_tinfo.isvalid;
}

float
Tracker::getAngle()
{
	targetInfo_t t;
	
	getTarget(&t);
	return t.angle;
}

float
Tracker::getDistance()
{
	targetInfo_t t;
	
	getTarget(&t);
	return t.distance;
}

ushort
Tracker::getPort()
{
	return m_port;
}

void
Tracker::getTarget(targetInfo_t *t)
{
	CRITICAL_REGION(m_semaphore)
	{
		memcpy((char *)t, (char *)&m_tinfo, sizeof(*t));
		if(!m_tinfo.isvalid) {
			t->angle    = BAD_ANGLE;
			t->distance = BAD_DISTANCE;
		}
	}
	END_REGION;
	return;
}

void
Tracker::setTarget(targetInfo_t *t)
{
	CRITICAL_REGION(m_semaphore)
	{
		m_tinfo = *t;
		if(!m_tinfo.isvalid) {
			t->angle    = BAD_ANGLE;
			t->distance = BAD_DISTANCE;
		}
	}
	END_REGION;
}

void
Tracker::illuminateTarget(bool on)
{
	m_inner->Set(on);
	m_outer->Set(on);
	return;
}