void ArmSpecial(void) {
/* ARM-SPECIAL
Format: BType $42
BData0 BurstSize (1-15)
BData1 Ammo (1-15)
BData2 TrigRate (13-255)
BSum $cs
BurstSize is in the range 1 to 15, 15 meaning unlimited firing.
Ammo is the amount of ready ammo to indicate in the Ammo Power Bar and should normally be in the range 1-10.
TrigRate is the number of milliseconds between shots in increments of 10msec.
*/
//BType
queueByte(0x42);
//BData0
queueByte(BURSTSIZE);
//BData1
queueByte(AMMO);
//BData2
queueByte(TRIGRATE);
//BSum
queueChecksum();
//Send it!
sendQueue();
//We're expecting another ACK from this.
weAreReloading = 2;
}
void IRAM_ATTR QueuedInterrupt::delegatedInterrupt()
{
int interruptMicros = micros();
int interruptPinValue = digitalRead(interruptPin);
QueuedInterruptMessage queuedInterruptMessage;
queuedInterruptMessage.interruptPin = interruptPin;
queuedInterruptMessage.interruptMicros = interruptMicros;
queuedInterruptMessage.interruptPinValue = interruptPinValue;
sendQueue(queuedInterruptMessage, true);
}
void BarrelReply(void) {
/* BARREL-REPLY
Format: BType $40
BData0 Barrel Type
BSum $cs
Barrels send this block in response to $00 ROLL-CALL blocks.
BData0 should be 0x01 for a shotgun, 0x02 for a machinegun, 0x03 for a sniper, 0x04 for a grenade launcher.
Other values are undefined, but can be used.
*/
//BType
queueByte(0x40);
//BData0
queueByte(BARRELTYPE);
//BSum
queueChecksum();
//Send it!
sendQueue();
}
void LoadSpecial(void) {
/* LOAD-SPECIAL
Format: BType $41
BData0 Ammo Needed (1-10)
BData1 DoubleTaps
BData2 LoadSFX
BData3 FireSFX
BData4 DisplayMode
BSum $cs
Unloads any ready ammo and loads special shot.
Ammo Needed is how much ammo will be deducted from the available total.
DoubleTaps is (UNKNOWN).
LoadSFX is what sound effect will be played when this block is RX'd by the tagger.
FireSFX is what sound effect will be played when the trigger is pulled after this
special ammo is loaded/armed.
DisplayMode is what will be shown on the ammo display of the tagger.
*/
//BType
queueByte(0x41);
//BData0
queueByte(AMMONEEDED);
//BData1
queueByte(DOUBLETAPS);
//BData2
queueByte(LOADSFX);
//BData3
queueByte(FIRESFX);
//BData4
queueByte(DISPLAYMODE);
//BSum
queueChecksum();
//We also need to mark that we're trying to load something and are expecting a response from the tagger.
weAreReloading = 1;
//Send it!
sendQueue();
}
int run(const char *serverAddress, const int serverPort, char headless)
{
int i, sockfd, show = ~0;
int frames = 0;
int returnValue = EXIT_SUCCESS;
CvCapture *capture;
CvMemStorage *storage;
IplImage *grabbedImage;
IplImage *imgThreshold;
CvSeq *seq;
CvFont font;
SendQueue *queue;
char strbuf[255];
struct timeval oldTime, time, diff;
float lastKnownFPS = 0;
sockfd = initNetwork(serverAddress, serverPort);
if (sockfd == -1) {
fprintf(stderr, "ERROR: initNetwork returned -1\n");
return EXIT_FAILURE;
}
queue = initSendQueue();
capture = cvCaptureFromCAM(CV_CAP_ANY);
if (capture == NULL) {
fprintf( stderr, "ERROR: capture is NULL \n" );
getchar();
return EXIT_FAILURE;
}
// Create a window in which the captured images will be presented
cvNamedWindow("mywindow", CV_WINDOW_AUTOSIZE);
storage = cvCreateMemStorage(0);
// void cvInitFont(font, font_face, hscale, vscale, shear=0, thickness=1, line_type=8 )
cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1, 1, 0, 1, 8);
gettimeofday(&oldTime, NULL);
// Show the image captured from the camera in the window and repeat
while (1) {
cvClearMemStorage(storage);
grabbedImage = cvQueryFrame(capture);
if (grabbedImage == NULL) {
fprintf( stderr, "ERROR: frame is null...\n" );
getchar();
returnValue = EXIT_FAILURE;
break;
}
//Create detection image
imgThreshold = cvCreateImage(cvGetSize(grabbedImage), 8, 1);
cvInRangeS(grabbedImage, min, max, imgThreshold);
//Flip images to act as a mirror.
//TODO remove when camera faces screen
if (show) {
cvFlip(grabbedImage, grabbedImage, 1);
cvFlip(imgThreshold, imgThreshold, 1);
}
//Find all dots in the image. This is where any calibration of dot detection is done, if needed, though it
//should be fine as it is right now.
/*
* image, circleStorage, method, double dp, double minDist, double param1, double param2, int minRadius, int maxRadius
*/
seq = cvHoughCircles(imgThreshold, storage, CV_HOUGH_GRADIENT, 2, 20, 20, 2, 0, 10);
for (i = 0; i < seq->total; i++){
// Get point
float *p = (float*)cvGetSeqElem(seq, i);
//Draw current circle to the original image
if (show) paintCircle(p, grabbedImage);
//Buffer current circle to be sent to the server
addPointToSendQueue(p, queue);
}
//Print some statistics to the image
if (show) {
snprintf(strbuf, sizeof(strbuf), "Dots: %i", seq->total);
cvPutText(grabbedImage, strbuf, cvPoint(10, 20), &font, cvScalar(WHITE));
snprintf(strbuf, sizeof(strbuf), "FPS: %.1f", lastKnownFPS);
cvPutText(grabbedImage, strbuf, cvPoint(10, 200), &font, cvScalar(WHITE));
}
//Show images
//TODO Comment these out will probably improve performance quite a bit
if (show) {
cvShowImage("mywindow", imgThreshold);
cvShowImage("mywindow", grabbedImage);
}
gettimeofday(&time, NULL);
timeval_subtract(&diff, &time, &oldTime);
// printf("Frames = %i\n", diff.tv_sec);
if (diff.tv_sec >= 2) {
lastKnownFPS = (float)frames / diff.tv_sec;
oldTime = time;
frames = 0;
}
//Add one to the frame rate counter
frames++;
//Send to dots detected this frame to the server
sendQueue(sockfd, queue);
clearSendQueue(queue);
//
//If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
//remove higher bits using AND operator
i = (cvWaitKey(10) & 0xff);
if (i == 'v') show = ~show;
if (i == 27) break;
}
// Release the capture device housekeeping
cvReleaseCapture( &capture );
cvDestroyWindow( "mywindow" );
destroySendQueue(queue);
close(sockfd);
return returnValue;
}
// Runs the dot detector and sends detected dots to server on port TODO Implement headless. Needs more config options and/or possibly a config file first though
int run( const char *serverAddress, const int serverPort, char headless ) {
char calibrate_exposure = 0, show = ~0, flip = 0, vflip = 0, done = 0, warp = 0; //"Boolean" values used in this loop
char noiceReduction = 2; //Small counter, so char is still ok.
int i, sockfd; //Generic counter
int dp = 0, minDist = 29, param1 = 0, param2 = 5; // Configuration variables for circle detection
int minDotRadius = 1;
int detected_dots; //Detected dot counter
int returnValue = EXIT_SUCCESS;
int captureControl; //File descriptor for low-level camera controls
int currentExposure = 150;
int maxExposure = 1250; //Maximum exposure supported by the camera TODO Get this from the actual camera
Color min = { 0, 70, 0, 0 }; //Minimum color to detect
Color max = { 255, 255, 255, 0 }; //Maximum color to detect
CvScalar colorWhite = cvScalar( WHITE ); //Color to draw detected dots on black and white surface
BoundingBox DD_mask; //The box indicating what should and what should not be considered for dot search
BoundingBox DD_transform; //The box indicating the plane we are looking at( and as such is the plane we would transform from )
BoundingBox DD_transform_to; //The plane we are transforming to
CvCapture *capture = NULL; //The camera
CvMemStorage *storage; //Low level memory area used for dynamic structures in OpenCV
CvSeq *seq; //Sequence to store detected dots in
IplImage *grabbedImage = NULL; //Raw image from camera( plus some overlay in the end )
IplImage *imgThreshold = NULL; //Image with detected dots
IplImage *mask = NULL; //Mask to be able to remove uninteresting areas
IplImage *coloredMask = NULL; //Mask to be able to indicate above mask on output image
CvFont font; //Font for drawing text on images
SendQueue *queue; //Head of the linked list that is the send queue
char strbuf[255]; //Generic buffer for text formatting( with sprintf())
struct timeval oldTime, time, diff; //Structs for measuring FPS
float lastKnownFPS = 0; //Calculated FPS
CvMat* pointRealMat = cvCreateMat( 1,1,CV_32FC2 ); //Single point matrix for point transformation
CvMat* pointTransMat = cvCreateMat( 1,1,CV_32FC2 ); //Single point matrix for point transformation
CvMat* transMat = cvCreateMat( 3,3,CV_32FC1 ); //Translation matrix for transforming input to a straight rectangle
ClickParams clickParams = { TOP_LEFT, NULL, &DD_transform_to, transMat }; //Struct holding data needed by mouse-click callback function
// Set up network
sockfd = initNetwork( serverAddress, serverPort );
if( sockfd == -1 ) {
fprintf( stderr, "ERROR: initNetwork returned -1\n");
return EXIT_FAILURE;
}
queue = initSendQueue();
if( openCamera( &capture, &captureControl ) == 0 ) {
fprintf( stderr, "ERROR: capture is NULL \n" );
return EXIT_FAILURE;
}
if( ( disableAutoExposure( captureControl ) ) == -1 ) {
fprintf( stderr, "ERROR: Cannot disable auto exposure \n" );
//return EXIT_FAILURE;
}
if( ( updateAbsoluteExposure( captureControl, currentExposure ) ) == 0 ) {
fprintf( stderr, "ERROR: Cannot set exposure\n");
}
// Create a window in which the captured images will be presented
cvNamedWindow( imagewindowname, CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_NORMAL );
// Create a window to hold the configuration sliders and the detection frame TODO This is kind of a hack. Make a better solution
cvNamedWindow( configwindowname, CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_NORMAL );
// Create a window to hold the transformed image. Handy to see how the dots are translated, but not needed for functionality
if( warp ) cvNamedWindow( warpwindowname, CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_NORMAL );
// Create sliders to adjust the lower color boundry
cvCreateTrackbar( red_lable , configwindowname, &min.red, 255, NULL );
cvCreateTrackbar( green_lable, configwindowname, &min.green, 255, NULL );
cvCreateTrackbar( blue_lable , configwindowname, &min.blue, 255, NULL );
//Create sliters for the contour based dot detection
cvCreateTrackbar( min_area_lable, configwindowname, &minDotRadius,255, NULL );
/* Slider for manual exposure setting */
cvCreateTrackbar( exposure_lable, configwindowname, ¤tExposure, maxExposure, NULL );
//Create the memory storage
storage = cvCreateMemStorage( 0 );
// void cvInitFont( font, font_face, hscale, vscale, shear=0, thickness=1, line_type=8 )
cvInitFont( &font, CV_FONT_HERSHEY_PLAIN, 1, 1, 0, 1, 8 );
// Grab an initial image to be able to fetch image size before the main loop.
grabbedImage = cvQueryFrame( capture );
//Move the two windows so both are visible at the same time
cvMoveWindow( imagewindowname, 0, 10 );
cvMoveWindow( configwindowname, grabbedImage->width+2, 10 );
//TODO Move these three inits to a function
// Set masking defaults TODO load from file? Specify file for this file loading?
DD_mask.topLeft.x = 0;
DD_mask.topLeft.y = 0;
DD_mask.topRight.x = grabbedImage->width-1;
DD_mask.topRight.y = 0;
DD_mask.bottomLeft.x = 0;
DD_mask.bottomLeft.y = grabbedImage->height-1;
DD_mask.bottomRight.x = grabbedImage->width-1;
DD_mask.bottomRight.y = grabbedImage->height-1;
// Set transformation defaults TODO load from file? Specify file for this file loading?
DD_transform.topLeft.x = 0;
DD_transform.topLeft.y = 0;
DD_transform.topRight.x = grabbedImage->width-1;
DD_transform.topRight.y = 0;
DD_transform.bottomLeft.x = 0;
DD_transform.bottomLeft.y = grabbedImage->height-1;
DD_transform.bottomRight.x = grabbedImage->width-1;
DD_transform.bottomRight.y = grabbedImage->height-1;
// Set the transformation destination
DD_transform_to.topLeft.x = 0;
DD_transform_to.topLeft.y = 0;
DD_transform_to.topRight.x = grabbedImage->width-1;
DD_transform_to.topRight.y = 0;
DD_transform_to.bottomLeft.x = 0;
DD_transform_to.bottomLeft.y = grabbedImage->height-1;
DD_transform_to.bottomRight.x = grabbedImage->width-1;
DD_transform_to.bottomRight.y = grabbedImage->height-1;
calculateTransformationMatrix( &DD_transform, &DD_transform_to, transMat );
// Set callback function for mouse clicks
cvSetMouseCallback( imagewindowname, calibrateClick, ( void* ) &clickParams );
gettimeofday( &oldTime, NULL );
// Main loop. Grabbs an image from cam, detects dots, sends dots,and prints dots to images and shows to user
while( !done ) {
//PROFILING_PRO_STAMP(); //Uncomment this and the one in the end of the while-loop, and comment all other PROFILING_* to profile main-loop
// ------ Common actions
cvClearMemStorage( storage );
detected_dots = 0;
//Grab a fram from the camera
PROFILING_PRO_STAMP();
grabbedImage = cvQueryFrame( capture );
PROFILING_POST_STAMP( "cvQueryFrame");
if( grabbedImage == NULL ) {
fprintf( stderr, "ERROR: frame is null...\n" );
getchar();
returnValue = EXIT_FAILURE;
break;
}
//Flip images to act as a mirror.
if( show && flip ) {
cvFlip( grabbedImage, grabbedImage, 1 );
}
if( show && vflip ) {
cvFlip( grabbedImage, grabbedImage, 0 );
}
// ------ State based actions
switch( state ) {
case GRAB_DOTS:
//Create detection image
imgThreshold = cvCreateImage( cvGetSize( grabbedImage ), 8, 1 );
cvInRangeS( grabbedImage, cvScalar( DD_COLOR( min )), cvScalar( DD_COLOR( max )), imgThreshold );
//Mask away anything not in our calibration area
mask = cvCreateImage( cvGetSize( grabbedImage ), 8, 1 );
cvZero( mask );
cvFillConvexPoly( mask, ( CvPoint* ) &DD_mask, 4, cvScalar( WHITE ), 1, 0 );
cvAnd( imgThreshold, mask, imgThreshold, NULL );
// Invert mask, increase the number of channels in it and overlay on grabbedImage //TODO Tint the mask red before overlaying
cvNot( mask, mask );
coloredMask = cvCreateImage( cvGetSize( grabbedImage ), grabbedImage->depth, grabbedImage->nChannels );
cvCvtColor( mask, coloredMask, CV_GRAY2BGR );
cvAddWeighted( grabbedImage, 0.95, coloredMask, 0.05, 0.0, grabbedImage );
// Reduce noise.
// Erode is kind of floor() of pixels, dilate is kind of ceil()
// I'm not sure which gives the best result.
switch( noiceReduction ) {
case 0: break; //No noice reduction at all
case 1: cvErode( imgThreshold, imgThreshold, NULL, 2 ); break;
case 2: cvDilate( imgThreshold, imgThreshold, NULL, 2 ); break;
}
// Warp the warp-image. We are reusing the coloredMask variable to save some space
PROFILING_PRO_STAMP();
if( show && warp ) cvWarpPerspective( grabbedImage, coloredMask, transMat, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll( 0 ));
PROFILING_POST_STAMP( "Warping perspective" );
// Find all dots in the image
PROFILING_PRO_STAMP();
// Clear old data from seq
seq = 0;
// Find the dots
cvFindContours(
imgThreshold,
storage,
&seq,
sizeof( CvContour ),
CV_RETR_LIST,
CV_CHAIN_APPROX_SIMPLE,
cvPoint( 0,0 )
);
// cvFindContours destroys the original image, so we wipe it here
// and then repaints the detected dots later
cvZero( imgThreshold );
PROFILING_POST_STAMP( "Dot detection" );
//Process all detected dots
PROFILING_PRO_STAMP();
for( ; seq != 0; seq = seq->h_next ) {
// Calculate radius of the detected contour
CvRect rect =( ( CvContour * )seq )->rect;
float relCenterX = rect.width / 2;
float relCenterY = rect.height / 2;
// Make sure the dot is big enough
if( relCenterX < minDotRadius || relCenterY < minDotRadius ) {
continue;
}
// Note that we have found another dot
++detected_dots;
// Transform the detected dot according to transformation matrix.
float absCenter[] = { rect.x + relCenterX, rect.y + relCenterY };
pointRealMat->data.fl = absCenter;
cvPerspectiveTransform( pointRealMat, pointTransMat, transMat );
// Draw the detected contour back to imgThreshold
// Draw the detected dot both to real image and to warped( if warp is active )
if( show ) {
cvDrawContours( imgThreshold, seq, colorWhite, colorWhite, -1, CV_FILLED, 8, cvPoint( 0,0 ) );
drawCircle( absCenter[0], absCenter[1], ( relCenterX + relCenterY ) / 2, grabbedImage );
if( warp ) {
drawCircle( pointTransMat->data.fl[0], pointTransMat->data.fl[1], ( relCenterX + relCenterY ) / 2, coloredMask );
}
}
// Add detected dot to to send queue
addPointToSendQueue( pointTransMat->data.fl, queue );
}
PROFILING_POST_STAMP("Painting dots");
//Calculate framerate
gettimeofday( &time, NULL );
timeval_subtract( &diff, &time, &oldTime );
lastKnownFPS = lastKnownFPS * 0.7 + ( 1000000.0 / diff.tv_usec ) * 0.3; //We naïvly assume we have more then 1 fps
oldTime = time;
//Send the dots detected this frame to the server
PROFILING_PRO_STAMP();
sendQueue( sockfd, queue );
clearSendQueue( queue );
PROFILING_POST_STAMP( "Sending dots" );
/* If calibrating, do the calibration */
if( calibrate_exposure ) {
int ret;
ret = calibrateExposureLow( captureControl, detected_dots, ¤tExposure, DD_MAX_EXPOSURE, lastKnownFPS );
switch( ret ) {
case 0: // We are done. Let's leave calibration mode
calibrate_exposure = 0;
printf( "done\n" );
break;
case -1: // We hit the upper limit with no detected dots
fprintf( stderr, "Reached upper limit (%d). Aborting!\n", DD_MAX_EXPOSURE );
calibrate_exposure = 0;
break;
case -2: // We hit lower limit with more then one dot detected
fprintf( stderr, "Too bright. More then one dot found even with minimal exposure. Aborting!\n");
calibrate_exposure = 0;
break;
case -3: //No conclusive results.
fprintf( stderr, "No conclusive results. Giving up\n" );
calibrate_exposure = 0;
break;
}
}
break; //End of GRAB_DOTS
case SELECT_TRANSFORM:
//Falling through here. Poor man's multi-case clause. Not putting this in default as we might
//want to do different things in these two some day.
case SELECT_MASK:
snprintf( strbuf, sizeof( strbuf ), "Select %s point", pointTranslationTable[clickParams.currentPoint]);
cvDisplayOverlay( imagewindowname, strbuf, 5 );
break; //End of SELECT_MASK and SELECT_TRANSFORM
}
// Paint the corners of the detecting area and the calibration area
paintOverlayPoints( grabbedImage, &DD_transform );
//Print some statistics to the image
if( show ) {
snprintf( strbuf, sizeof( strbuf ), "Dots: %i", detected_dots ); //Print number of detected dots to the screen
cvPutText( grabbedImage, strbuf, cvPoint( 10, 20 ), &font, cvScalar( WHITE ));
snprintf( strbuf, sizeof( strbuf ), "FPS: %.1f", lastKnownFPS );
cvPutText( grabbedImage, strbuf, cvPoint( 10, 40 ), &font, cvScalar( WHITE ));
cvCircle( grabbedImage, cvPoint( 15, 55 ), minDotRadius, cvScalar( min.blue, min.green, min.red, min.alpha ), -1, 8, 0 ); // Colors given in order BGR-A, Blue, Green, Red, Alpha
}
//Show images
PROFILING_PRO_STAMP();
if( show ) {
cvShowImage( configwindowname, imgThreshold );
cvShowImage( imagewindowname, grabbedImage );
if( warp ) cvShowImage( warpwindowname, coloredMask );
}
PROFILING_POST_STAMP("Showing images");
//Release the temporary images
cvReleaseImage( &imgThreshold );
cvReleaseImage( &mask );
cvReleaseImage( &coloredMask );
/* Update exposure if needed */
updateAbsoluteExposure( captureControl, currentExposure );
cvSetTrackbarPos( exposure_lable, configwindowname, currentExposure );
//If ESC key pressed, Key=0x10001B under OpenCV 0.9.7( linux version ),
//remove higher bits using AND operator
i = ( cvWaitKey( 10 ) & 0xff );
switch( i ) {
case 'g':
makeCalibrate( &DD_transform, &DD_transform_to, transMat, capture, captureControl, 20 );
updateAbsoluteExposure( captureControl, currentExposure+1 );
break;
case 'e':
toggleCalibrationMode( &calibrate_exposure, ¤tExposure );
break; /* Toggles calibration mode */
case 'c':
openCamera( &capture, &captureControl );
break;
case 's':
show = ~show;
break; //Toggles updating of the image. Can be useful for performance of slower machines... Or as frame freeze
case 'm':
state = SELECT_MASK;
clickParams.currentPoint = TOP_LEFT;
clickParams.DD_box = &DD_mask;
break; //Starts selection of masking area. Will return to dot detection once all four points are set
case 't':
state = SELECT_TRANSFORM;
clickParams.currentPoint = TOP_LEFT;
clickParams.DD_box = &DD_transform;
break; //Starts selection of the transformation area. Returns to dot detection when done.
case 'f':
flip = ~flip;
break; //Toggles horizontal flipping of the image
case 'v':
vflip = ~vflip;
break; //Toggles vertical flipping of the image
case 'w':
warp = ~warp;
toggleWarpOutput( warp );
break; //Toggles showing the warped image
case 'n':
noiceReduction = ( noiceReduction + 1 ) % 3;
break; //Cycles noice reduction algorithm
case 'q': //falling through here to quit
case 27:
done = 1;
break; //ESC. Kills the whole thing( in a nice and controlled manner )
}
fflush( stdout ); //Make sure everything in the buffer is printed before we go on
//PROFILING_POST_STAMP("Main loop");
} //End of main while-loop
// Release the capture device and do some housekeeping
cvReleaseImage( &grabbedImage );
cvReleaseCapture( &capture );
cvReleaseMemStorage( &storage );
cvDestroyWindow( imagewindowname );
cvDestroyWindow( configwindowname );
if( warp ) cvDestroyWindow( warpwindowname ); //If now warp it is already destroyed
destroySendQueue( queue );
close( sockfd );
close( captureControl );
return returnValue;
}