Pixy.cpp

00001 
00002 #include "Pixy.h"
00003 
00004 Pixy::Pixy(Serial& _cam) : cam(_cam)
00005 {
00006     //cam.baud( 230400 );
00007     cam.baud( 460800 );
00008     cam.attach(this, &Pixy::rxCallback, Serial::RxIrq);
00009 }
00010 
00011 
00012 // This function is called when a character goes into the RX buffer.
00013 void Pixy::rxCallback()
00014 {
00015     static const int buffersize = 256;
00016     static int detects = 0;
00017     static int startPoint = 0;
00018     static uint8_t buffer[buffersize] = {};
00019     static bool startFound = false;
00020     static int ii = 1;
00021 
00022     while( cam.readable() ) {
00023         buffer[ii] = cam.getc();
00024         if( buffer[ii-1] == 85  && (buffer[ii] == 170 )  ) {
00025             startPoint = ii-1;
00026 
00027             //check if detection was on the edge of buffer. Skip package if so.
00028             if( ii<(buffersize-14))
00029                 startFound = true;
00030             else
00031                 ii = 1;
00032                 
00033             detects++;
00034         }
00035         ++ii;
00036 
00037         //reset ii
00038         if( ii>=(buffersize-1))
00039             ii = 1;
00040     }
00041 
00042     //start not found, reset ii to 1
00043     if( !startFound && ii >= 3 || ii >= (buffersize-1)) {
00044         ii = 1;
00045         return;
00046     }
00047 
00048     //start is found but not enough bytes received - return
00049     if( (ii-startPoint) <= 13 )
00050         return;
00051 
00052     //copy memory to pixy struct
00053     memcpy( &pixy, buffer + startPoint+2, 12);
00054 
00055     //reset variables
00056     startFound = false;
00057     ii = 1;
00058 }
00059 
00060 int Pixy::getX()
00061 {
00062     return pixy.x;
00063 }
00064 
00065 int Pixy::getY()
00066 {
00067     return pixy.y;
00068 }
00069 
00070 int Pixy::getSignature()
00071 {
00072     return pixy.signature;
00073 }
00074