int blobsLoop()
{
BlobA *blobs;
BlobB *ccBlobs;
uint32_t numBlobs, numCCBlobs;
// create blobs
g_blobs->blobify();
// handle received data immediately
handleRecv();
// send blobs
g_blobs->getBlobs(&blobs, &numBlobs, &ccBlobs, &numCCBlobs);
cc_sendBlobs(g_chirpUsb, blobs, numBlobs, ccBlobs, numCCBlobs);
ser_getSerial()->update();
cc_setLED();
// deal with any latent received data until the next frame comes in
while(!g_qqueue->queued())
handleRecv();
return 0;
}
int blobsSetup()
{
uint8_t c;
// setup camera mode
cam_setMode(CAM_MODE1);
#ifdef DA
// load lut if we've grabbed any frames lately
if (g_rawFrame.m_pixels)
cc_loadLut();
#endif
// if there have been any parameter changes, we should regenerate the LUT (do it regardless)
g_blobs->m_clut.generateLUT();
// setup qqueue and M0
g_qqueue->flush();
exec_runM0(0);
// flush serial receive queue
while(ser_getSerial()->receive(&c, 1));
g_state = 0; // reset recv state machine
return 0;
}
int blobsLoop()
{
BlobA *blobs;
uint32_t numBlobs;
// create blobs
g_blobs->blobify();
// send blobs
g_blobs->getBlobs(&blobs, &numBlobs);
cc_sendBlobs(g_chirpUsb, blobs, numBlobs);
ser_getSerial()->update();
cc_setLED();
return 0;
}
int blobsSetup()
{
uint8_t c;
// setup camera mode
cam_setMode(CAM_MODE1);
// load lut if we've grabbed any frames lately
if (g_rawFrame.m_pixels)
cc_loadLut();
// setup qqueue and M0
g_qqueue->flush();
exec_runM0(0);
// flush serial receive queue
while(ser_getSerial()->receive(&c, 1));
return 0;
}
void handleRecv()
{
uint8_t i, a;
static uint8_t state=0, b=1;
uint16_t s0, s1;
Iserial *serial = ser_getSerial();
for (i=0; i<10; i++)
{
switch(state)
{
case 0: // look for sync
if(serial->receive(&a, 1)<1)
return;
if (a==0xff && b==0x00)
state = 1;
b = a;
break;
case 1: // read rest of data
if (serial->receiveLen()>=4)
{
serial->receive((uint8_t *)&s0, 2);
serial->receive((uint8_t *)&s1, 2);
//cprintf("servo %d %d\n", s0, s1);
rcs_setPos(0, s0);
rcs_setPos(1, s1);
state = 0;
}
break;
default:
state = 0;
break;
}
}
}
void handleRecv()
{
uint8_t i, a;
static uint16_t w=0xffff;
static uint8_t lastByte;
uint16_t s0, s1;
Iserial *serial = ser_getSerial();
for (i=0; i<10; i++)
{
switch(g_state)
{
case 0: // reset
lastByte = 0xff; // This is not part of any of the sync word most significant bytes
g_state = 1;
break;
case 1: // sync word
if(serial->receive(&a, 1))
{
w = lastByte << 8;
w |= a;
lastByte = a;
g_state = 2; // compare
}
break;
case 2: // receive data byte(s)
if (w==SYNC_SERVO)
{ // read rest of data
if (serial->receiveLen()>=4)
{
serial->receive((uint8_t *)&s0, 2);
serial->receive((uint8_t *)&s1, 2);
//cprintf("servo %d %d\n", s0, s1);
rcs_setPos(0, s0);
rcs_setPos(1, s1);
g_state = 0;
}
}
else if (w==SYNC_CAM_BRIGHTNESS)
{
if(serial->receive(&a, 1))
{
cam_setBrightness(a);
g_state = 0;
}
}
else if (w==SYNC_SET_LED)
{
if (serial->receiveLen()>=3)
{
uint8_t r, g, b;
serial->receive(&r, 1);
serial->receive(&g, 1);
serial->receive(&b, 1);
led_setRGB(r, g, b);
//cprintf("%x %x %x\n", r, g ,b);
g_ledSet = true; // it will stay true until the next power cycle
g_state = 0;
}
}
else
g_state = 1; // try another word, but read only a byte
break;
default:
g_state = 0; // try another whole word
break;
}
}
}
int blobsLoop()
{
#if 1
BlobA *blobs;
BlobB *ccBlobs;
uint32_t numBlobs, numCCBlobs;
static uint32_t drop = 0;
// create blobs
if (g_blobs->blobify()<0)
{
DBG("drop %d\n", drop++);
return 0;
}
// handle received data immediately
handleRecv();
// send blobs
g_blobs->getBlobs(&blobs, &numBlobs, &ccBlobs, &numCCBlobs);
cc_sendBlobs(g_chirpUsb, blobs, numBlobs, ccBlobs, numCCBlobs);
ser_getSerial()->update();
// if user isn't controlling LED, set it here, according to biggest detected object
if (!g_ledSet)
cc_setLED();
// deal with any latent received data until the next frame comes in
while(!g_qqueue->queued())
handleRecv();
#endif
#if 0
Qval qval;
static int i = 0;
while(1)
{
if (g_qqueue->dequeue(&qval) && qval.m_col==0xffff)
{
cprintf("%d\n", i++);
break;
}
}
#endif
#if 0
BlobA *blobs;
BlobB *ccBlobs;
uint32_t numBlobs, numCCBlobs;
static uint32_t drop = 0;
// create blobs
if (g_blobs->blobify()<0)
{
DBG("drop %d\n", drop++);
return 0;
}
g_blobs->getBlobs(&blobs, &numBlobs, &ccBlobs, &numCCBlobs);
cc_sendBlobs(g_chirpUsb, blobs, numBlobs, ccBlobs, numCCBlobs);
#endif
return 0;
}
uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
{
uint8_t c;
uint16_t d;
uint16_t numBlobs;
uint32_t temp, width, height;
Iserial *serial = ser_getSerial();
if (serial->receive(&c, 1)==0)
return 0;
#if 1
if (c==0x00)
{
//printf("0\n");
char *str = "V0.1";
strcpy((char *)buf, str);
return 5;
//return strlen((char *)str);
}
if (c==0x08)
{
//printf("8\n");
char *str = "Pixy";
strcpy((char *)buf, str);
return 5;
//return strlen((char *)str);
}
else if (c==0x10)
{
//printf("10\n");
char *str = "Pixy";
strcpy((char *)buf, str);
return 5;
//return strlen((char *)str);
}
else
#endif
if (c>=0x51 && c<=0x57)
{
#if 0
buf[0] = 1;
buf[1] = 2;
buf[2] = 3;
buf[3] = 4;
buf[4] = 5;
#else
BlobA *max;
max = g_blobs->getMaxBlob(c-0x50, &numBlobs);
if (max==0)
memset(buf, 0, 5);
else if (max==(BlobA *)-1)
memset(buf, -1, 5);
else
{
width = max->m_right - max->m_left;
height = max->m_bottom - max->m_top;
buf[0] = numBlobs;
temp = ((max->m_left + width/2)*819)>>10;
buf[1] = temp;
buf[2] = max->m_top + height/2;
temp = (width*819)>>10;
buf[3] = temp;
buf[4] = height;
}
#endif
return 5;
}