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; } } }
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; }