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 main(void) { uint16_t major, minor, build; int i, res, count; // main init of hardware plus a version-dependent number for the parameters that will // force a format of parameter between version numbers. pixyInit(SRAM3_LOC, &LR0[0], sizeof(LR0)); cc_init(g_chirpUsb); ser_init(); exec_init(g_chirpUsb); // load programs exec_addProg(&g_progBlobs); ptLoadParams(); exec_addProg(&g_progPt); #if 0 chaseLoadParams(); exec_addProg(&g_progChase); #endif exec_addProg(&g_progVideo, true); // this code formats if the version has changed for (i=0, count=0; i<25; i++) { res = prm_get("fwver", &major, &minor, &build, END); if (res>=0 && major==FW_MAJOR_VER && minor==FW_MINOR_VER && build==FW_BUILD_VER) count++; } if (count==0) prm_format(); // check version prm_add("fwver", PRM_FLAG_INTERNAL, "", UINT16(FW_MAJOR_VER), UINT16(FW_MINOR_VER), UINT16(FW_BUILD_VER), END); ser_setInterface(SER_INTERFACE_UART); rcs_setLimits(1, -250, 250); // extends servo range to maximum//0x__BBRRGG //28 == full back, 12 == full angle down, 22 == parallel uint8_t WBV_sub = 0x16; uint32_t WBV = WBV_sub | WBV_sub << 8 | WBV_sub << 16; cam_setWBV(WBV); // sets the white balance manually cam_setAEC(0); // turns off auto exposure correction cam_setBrightness(35); // sets the brightness while(1) { //edgeDetect_highres_run(); edgeDetect_run(); // run the main edgeDetect function //exec_loop(); // Debug through pixymon } #if 0 #define DELAY 1000000 rcs_setFreq(100); rcs_setLimits(0, -200, 200); rcs_setLimits(1, -200, 200); while(1) { rcs_setPos(0, 0); delayus(DELAY); rcs_setPos(0, 500); delayus(DELAY); rcs_setPos(0, 1000); delayus(DELAY); rcs_setPos(1, 0); delayus(DELAY); rcs_setPos(1, 500); delayus(DELAY); rcs_setPos(1, 1000); delayus(DELAY); } #endif #if 0 while(1) { g_chirpUsb->service(); handleButton(); } #endif }
void ServoLoop::reset() { m_pos = SERVO_CENTER; rcs_setPos(m_axis, m_pos); }
int main(void) { #if 0 pixyInit(); cc_init(g_chirpUsb); ser_init(); exec_init(g_chirpUsb); #endif #if 1 /* test loop */ pixyInit(); exec_init(g_chirpUsb); #if 0 int i = 0; cam_setMode(1); while(1) { //uint8_t reg = cam_getRegister(0x0a); g_chirpUsb->service(); cprintf("hello world %d\n", i++); } #endif #if 0 while(1) { uint8_t *frame = (uint8_t *)SRAM1_LOC; int res; res = cam_getFrame(frame, SRAM1_SIZE, CAM_GRAB_M1R2, 0, 0, CAM_RES2_WIDTH, CAM_RES2_HEIGHT); i++; if (i%50==0) { lpc_printf("%d\n", i); } } #endif #endif #if 1 exec_addProg(&g_progBlobs); ptLoadParams(); exec_addProg(&g_progPt); exec_addProg(&g_progVideo, true); exec_loop(); #endif #if 0 //prm_format(); ColorModel model, *model2; uint32_t len; model.m_hue[0].m_slope = 1.0; model.m_hue[0].m_yi = 2.0; model.m_hue[1].m_slope = 3.0; model.m_hue[1].m_yi = 4.0; model.m_sat[0].m_slope = 5.0; model.m_sat[0].m_yi = 6.0; model.m_sat[1].m_slope = 7.0; model.m_sat[1].m_yi = 8.0; prm_add("signature1", "Color signature 1", INTS8(sizeof(ColorModel), &model), END); prm_set("signature1", INTS8(sizeof(ColorModel), &model), END); model.m_hue[0].m_slope = 9.0; model.m_hue[0].m_yi = 10.0; model.m_hue[1].m_slope = 11.0; model.m_hue[1].m_yi = 12.0; model.m_sat[0].m_slope = 13.0; model.m_sat[0].m_yi = 14.0; model.m_sat[1].m_slope = 15.0; model.m_sat[1].m_yi = 16.0; prm_add("signature2", "Color signature 2", INTS8(sizeof(ColorModel), &model), END); prm_set("signature2", INTS8(sizeof(ColorModel), &model), END); prm_get("signature1", &len, &model2, END); model.m_hue[0].m_slope = 17.0; model.m_hue[0].m_yi = 18.0; model.m_hue[1].m_slope = 19.0; model.m_hue[1].m_yi = 20.0; model.m_sat[0].m_slope = 21.0; model.m_sat[0].m_yi = 22.0; model.m_sat[1].m_slope = 23.0; model.m_sat[1].m_yi = 24.0; prm_get("signature1", &len, &model2, END); prm_set("signature1", INTS8(sizeof(ColorModel), &model), END); prm_get("signature1", &len, &model2, END); prm_get("signature2", &len, &model2, END); #endif #if 0 #define DELAY 1000000 rcs_setFreq(100); rcs_setLimits(0, -200, 200); rcs_setLimits(1, -200, 200); while(1) { rcs_setPos(0, 0); delayus(DELAY); rcs_setPos(0, 500); delayus(DELAY); rcs_setPos(0, 1000); delayus(DELAY); rcs_setPos(1, 0); delayus(DELAY); rcs_setPos(1, 500); delayus(DELAY); rcs_setPos(1, 1000); delayus(DELAY); } #endif #if 0 while(1) { g_chirpUsb->service(); handleButton(); } #endif }
void ServoLoop::reset() { m_pos = RCS_CENTER_POS; rcs_setPos(m_axis, m_pos); }
int main(void) { uint16_t major, minor, build; char *type; int i, res, count, count2; volatile uint32_t d; // insert a small delay so power supply can stabilize for (d=0; d<2500000; d++); #ifdef KEIL pixyInit(SRAM3_LOC, &LR0[0], sizeof(LR0)); #else pixyInit(); #endif #if 0 i = 0; char *foo; while(1) { foo = new (std::nothrow) char[128]; if (foo==NULL) { _DBG("full\n"); break; } else { _DBH32((int)foo); _DBG(" "); _DBH32(i); _DBG("\n"); } i++; } while(1); #endif // main init of hardware plus a version-dependent number for the parameters that will // force a format of parameter between version numbers. #ifndef LEGO rcs_init(); #endif cc_init(g_chirpUsb); ser_init(); exec_init(g_chirpUsb); #if 0 exec_addProg(&g_progBlobs); ptLoadParams(); exec_addProg(&g_progPt); exec_addProg(&g_progVideo, true); #if 0 cam_setMode(CAM_MODE1); while(1) periodic(); #endif #endif #if 1 // load programs exec_addProg(&g_progBlobs); #ifndef LEGO // need to call this to get the pan/tilt parameters to display. // We can make some properties modal, meaning they are only diaplayed when the program is running. // We might want to do this here, but this is good for now. ptLoadParams(); exec_addProg(&g_progPt); #endif #if 0 chaseLoadParams(); exec_addProg(&g_progChase); #endif exec_addProg(&g_progVideo, true); #if 1 // this code formats if the version has changed for (i=0, count=0, count2=0; i<25; i++) { res = prm_get("fwver", &major, &minor, &build, END); if (res>=0 && major==FW_MAJOR_VER && minor==FW_MINOR_VER && build==FW_BUILD_VER) count++; res = prm_get("fwtype", &type, END); if (res>=0 && strcmp(type, FW_TYPE)==0) count2++; } if (count==0 || count2==0) prm_format(); #endif // check version prm_add("fwver", PRM_FLAG_INTERNAL, "", UINT16(FW_MAJOR_VER), UINT16(FW_MINOR_VER), UINT16(FW_BUILD_VER), END); prm_add("fwtype", PRM_FLAG_INTERNAL, "", STRING(FW_TYPE), END); exec_loop(); #endif #if 0 #define DELAY 1000000 rcs_setFreq(100); rcs_setLimits(0, -200, 200); rcs_setLimits(1, -200, 200); while(1) { rcs_setPos(0, 0); delayus(DELAY); rcs_setPos(0, 500); delayus(DELAY); rcs_setPos(0, 1000); delayus(DELAY); rcs_setPos(1, 0); delayus(DELAY); rcs_setPos(1, 500); delayus(DELAY); rcs_setPos(1, 1000); delayus(DELAY); } #endif #if 0 while(1) { g_chirpUsb->service(); handleButton(); } #endif }
void edgeDetect_highres_run() { uint8_t *frame = (uint8_t *)SRAM1_LOC; uint8_t *frameloc = (uint8_t *)(SRAM1_LOC + 0); uint8_t *sendPositions = (uint8_t*)(SRAM1_LOC); float theta; // recieve the command to get a frame while(1) { // red LED: Stopped waiting for data led_setRGB(255, 0, 0); while(1) { if(UART_DATA_AVAILABLE) { // Data has come! theta = (float)UART_DATA_AVAILABLE; UART_DATA_AVAILABLE = 0; break; } } if(theta > 45) { // make sure that theta is casted as a float theta = (float)(theta*(3.14159/180.0)); // green LED, lets go! led_setRGB(0, 255, 0); // grab frame cam_getFrame(frameloc, SRAM1_SIZE, CAM_GRAB_M1R2, 0, 0, RES_WIDTH, RES_HEIGHT); frameloc = frame; // // double for loop for calculating edges for(uint16_t y = 1; y < (RES_HEIGHT); y += 1) { uint16_t ypo = y + 1; uint16_t ymo = y - 1; for(uint16_t x = 1; x < (RES_WIDTH); x += 1) { uint16_t xpo = x + 1; uint16_t xmo = x - 1; // Gradient calculation // intensity calculation for the pixel groups around each pixel uint16_t intense_XPO_Y; uint16_t intense_XMO_Y; uint16_t intense_X_YPO; uint16_t intense_XPO_YPO; uint16_t intense_XMO_YPO; uint16_t intense_X_YMO; uint16_t intense_XPO_YMO; uint16_t intense_XMO_YMO; if( ( (x % 2 == 0) && (y % 2 == 0) ) || ( (x % 2 == 1) && (y % 2 == 1) ) ) { // We are on a blue or red pixel intense_XPO_Y = intensityCalc_GreenPixel(frameloc, xpo, y); intense_XMO_Y = intensityCalc_GreenPixel(frameloc, xmo, y); intense_X_YPO = intensityCalc_GreenPixel(frameloc, x, ypo); intense_XPO_YPO = intensityCalc_BlueRedPixel(frameloc, xpo, ypo); intense_XMO_YPO = intensityCalc_BlueRedPixel(frameloc, xmo, ypo); intense_X_YMO = intensityCalc_GreenPixel(frameloc, x, ymo); intense_XPO_YMO = intensityCalc_BlueRedPixel(frameloc, xpo, ymo); intense_XMO_YMO = intensityCalc_BlueRedPixel(frameloc, xmo, ymo); } else { // We are on a green pixel intense_XPO_Y = intensityCalc_BlueRedPixel(frameloc, xpo, y); intense_XMO_Y = intensityCalc_BlueRedPixel(frameloc, xmo, y); intense_X_YPO = intensityCalc_BlueRedPixel(frameloc, x, ypo); intense_XPO_YPO = intensityCalc_GreenPixel(frameloc, xpo, ypo); intense_XMO_YPO = intensityCalc_GreenPixel(frameloc, xmo, ypo); intense_X_YMO = intensityCalc_BlueRedPixel(frameloc, x, ymo); intense_XPO_YMO = intensityCalc_GreenPixel(frameloc, xpo, ymo); intense_XMO_YMO = intensityCalc_GreenPixel(frameloc, xmo, ymo); } float grad1 = abs(intense_XPO_Y - intense_XMO_Y + intense_XPO_YPO - intense_XMO_YPO + intense_XPO_YMO - intense_XMO_YMO); float grad2 = abs(intense_X_YPO - intense_X_YMO + intense_XPO_YPO - intense_XPO_YMO + intense_XMO_YPO - intense_XMO_YMO); // Threashold detection if( (grad1 + grad2) > THREASHOLD_LOW ) { // EDGE frameloc[y*RES_WIDTH + x] = 255; } else { // NO EDGE frameloc[y*RES_WIDTH + x] = 0; } } } // end nested for loop led_setRGB(255, 0, 255); // Purple LED // floor detection & distance extrapolation uint16_t count = 0; for(float x = (POS_OFFSET); x < (RES_WIDTH - POS_OFFSET); x += 1.0) { // start on the left float xPos; for(float y = (RES_HEIGHT - POS_OFFSET); y > POS_OFFSET; y -= 1.0) { // start from the bottom if(frameloc[((uint16_t)y)*RES_WIDTH + (uint16_t)x] != 0) { float yPos; double theta_ph = atan(((2.0*y-200.0)/200.0)*TAN_FOVH_DIV_2); // angle of the pixel double cos_theta_ph = cos(theta_ph); // used in the computations double cos_theta_minus_ph = cos(theta - theta_ph); // used in the computations yPos = ((double)((3.9)*((cos_theta_ph))))/(cos_theta_minus_ph) + (2.1)*tan(theta - theta_ph); // y distance from the bot xPos = (yPos*(2.0*x - 320.0))/417.0; // x distance from the bot sendPositions[2*count] = xPos; // these two lines send x,y pairs sendPositions[2*count + 1] = yPos; count++; // count of the number of edges of obsticles detected break; // stop looking for the edge, break to the next x co-ordinate } else { // color the floor a different color. Not used in this scenario } } } UART_Send(LPC_USART0, sendPositions, 2*count, BLOCKING); // sends x,y pairs count = 0; // clear array for(uint16_t x = 0; x < 640; x++) { sendPositions[x] = 255; } } // end edge detecting else if(theta > 1 ) { // Servo move routine uint16_t position; int8_t retVal; // Move the servo based on the input from the PIC // theta == 2 corrisponds to a 45 degree angle, // theta == 42 corrisponds to a 135 degree angle. position = (position - 2)*(25); retVal = rcs_setPos(1, position); if(retVal == 0) { uint8_t retStr[] = "successful Servo Move\n\r"; UART_Send(LPC_USART0, retStr, 24, BLOCKING); } else { uint8_t retStr[] = "Error: Servo Move\n\r"; UART_Send(LPC_USART0, retStr, 20, BLOCKING); } } else { // theta == 1, they are asking for my ID // tell the processing script/pic/whatever that I am the pixy uint8_t ID[] = "I am the Pixy!\n\r"; UART_Send(LPC_USART0, ID, 17, BLOCKING); } } }