void cam_loadParams() { prm_add("Brightness", 0, "Sets the average brightness of the camera, can be between 0 and 255 (default 90)", UINT8(CAM_BRIGHTNESS_DEFAULT), END); uint8_t brightness; prm_get("Brightness", &brightness, END); cam_setBrightness(brightness); }
void cam_setRegs(const uint8_t *rPairs, int len) { int i; // put imager in sleep mode g_sccb->Write(0x09, 0x10); for (i=0; i<len; i+=2) { //printf("0x%x = 0x%x\n", rPairs[i], g_sccb->Read(rPairs[i])); g_sccb->Write(rPairs[i], rPairs[i+1]); if (g_sccb->Read(rPairs[i])!=rPairs[i+1]) volatile int q = 0; } // take imager out of sleep mode g_sccb->Write(0x09, 0x00); cam_setBrightness(g_brightness); }
void edgeDetect_run() { cam_setBrightness(BRIGHTNESS); // 0 to 255 uint8_t *frame = (uint8_t *)SRAM1_LOC; uint8_t *frameloc = (uint8_t *)(SRAM1_LOC + 2); uint8_t *sendPositions = (uint8_t*)(SRAM1_LOC); float theta; // recieve the command to get a frame while(1) { led_setRGB(255, 0, 0); while(1) { if(UART_DATA_AVAILABLE) { //rxbuf = UART_ReceiveByte(LPC_USART0); theta = (float)UART_DATA_AVAILABLE; UART_DATA_AVAILABLE = 0; break; } } theta = (float)(theta*(3.14159/180.0)); led_setRGB(0, 255, 0); cam_getFrame(frameloc, SRAM1_SIZE, CAM_GRAB_M1R2, 0, 0, RES_WIDTH, RES_HEIGHT); // second time through gets a frame fine frameloc = frame; for(uint16_t y = 1 + OFFSET; y < (RES_HEIGHT - OFFSET); y += 2) { uint16_t ypo = y + 1; uint16_t ymo = y - 1; for(uint16_t x = 1 + OFFSET; x < (RES_WIDTH - OFFSET); x += 2) { uint16_t xpo = x + 1; uint16_t xmo = x - 1; uint16_t grad; uint16_t intense_XPO_Y = frameloc[y*RES_WIDTH + xpo] + frameloc[ypo*RES_WIDTH + xpo+1] + (frameloc[ypo*RES_WIDTH + xpo] + frameloc[y*RES_WIDTH + xpo+1])/2; uint16_t intense_XMO_Y = frameloc[y*RES_WIDTH + xmo] + frameloc[ypo*RES_WIDTH + x] + (frameloc[ypo*RES_WIDTH + xmo] + frameloc[y*RES_WIDTH + x])/2; uint16_t intense_X_YPO = frameloc[ypo*RES_WIDTH + x] + frameloc[(ypo+1)*RES_WIDTH + xpo] + (frameloc[(ypo+1)*RES_WIDTH + x] + frameloc[ypo*RES_WIDTH + xpo])/2; uint16_t intense_XPO_YPO = frameloc[ypo*RES_WIDTH + xpo] + frameloc[(ypo+1)*RES_WIDTH + xpo+1] + (frameloc[(ypo+1)*RES_WIDTH + xpo] + frameloc[ypo*RES_WIDTH + xpo+1])/2; uint16_t intense_XMO_YPO = frameloc[(ypo)*RES_WIDTH + xmo] + frameloc[(ypo+1)*RES_WIDTH + x] + (frameloc[(ypo+1)*RES_WIDTH + xmo] + frameloc[ypo*RES_WIDTH + x])/2; uint16_t intense_X_YMO = frameloc[ymo*RES_WIDTH + x] + frameloc[y*RES_WIDTH + xpo] + (frameloc[y*RES_WIDTH + x] + frameloc[ymo*RES_WIDTH + xpo])/2; uint16_t intense_XPO_YMO = frameloc[ymo*RES_WIDTH + xpo] + frameloc[y*RES_WIDTH + xpo+1] + (frameloc[y*RES_WIDTH + xpo] + frameloc[ymo*RES_WIDTH + xpo+1])/2; uint16_t intense_XMO_YMO = frameloc[ymo*RES_WIDTH + xmo] + frameloc[y*RES_WIDTH + x] + (frameloc[y*RES_WIDTH + xmo] + frameloc[ymo*RES_WIDTH + x])/2; uint16_t grad1 = abs(intense_XPO_Y - intense_XMO_Y + intense_XPO_YPO - intense_XMO_YPO + intense_XPO_YMO - intense_XMO_YMO); uint16_t grad2 = abs(intense_X_YPO - intense_X_YMO + intense_XPO_YPO - intense_XPO_YMO + intense_XMO_YPO - intense_XMO_YMO); grad = grad1 + grad2; // when it loops a second time through, it cannot make it passed the next line // Deleted if statement, hopefully that'll do 'er. Really inefficient though if(grad > THREASHOLD) { frameloc[ymo*RES_WIDTH + xmo] = 255; frameloc[y*RES_WIDTH + xmo] = 255; frameloc[ymo*RES_WIDTH + x] = 255; frameloc[y*RES_WIDTH + x] = 255; } else { frameloc[ymo*RES_WIDTH + xmo] = 0; frameloc[y*RES_WIDTH + xmo] = 0; frameloc[ymo*RES_WIDTH + x] = 0; frameloc[y*RES_WIDTH + x] = 0; } /* TEST DIAGONAL LINE if( y == x) { frameloc[ymo*RES_WIDTH + xmo] = 255; frameloc[y*RES_WIDTH + xmo] = 255; frameloc[ymo*RES_WIDTH + x] = 255; frameloc[y*RES_WIDTH + x] = 255; } else { frameloc[ymo*RES_WIDTH + xmo] = 0; frameloc[y*RES_WIDTH + xmo] = 0; frameloc[ymo*RES_WIDTH + x] = 0; frameloc[y*RES_WIDTH + x] = 0; } */ } } led_setRGB(255, 0, 255); // floor detection & uint16_t count = 0; for(float x = (POS_OFFSET); x < (RES_WIDTH - POS_OFFSET); x += 2.0) { float xPos; for(float y = (RES_HEIGHT - POS_OFFSET); y > POS_OFFSET; y -= 2.0) { if(frameloc[((uint8_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); // This works double cos_theta_ph = cos(theta_ph); //sendPositions[count] = cos_theta_ph*128; double cos_theta_minus_ph = cos(theta - theta_ph); //sendPositions[count] = cos_theta_minus_ph*128; //sendPositions[count] = (int8_t)(theta_ph * (180.0/3.142)); // these two lines send x,y pairs //sendPositions[count] = (cos_theta_ph)/(cos_theta_minus_ph); yPos = ((double)((3.8)*((cos_theta_ph))))/(cos_theta_minus_ph) + (0.0)*tan(theta - theta_ph); xPos = (yPos*(2.0*x - 320.0))/417.0; // hopefully the x cord. won't be super inaccurate. Not crucial though. //sendPositions[2*count + 1] = yPos; //(int8_t)yPos; //sendPositions[2*count] = x; // these two lines send x,y pairs sendPositions[count] = yPos; //(int8_t)yPos; count++; break; } else { // color the floor a different color. Not used in this scenario } } } UART_Send(LPC_USART0, sendPositions, count, BLOCKING); // sends x,y pairs //UART_Send(LPC_USART0, sendPositions, count, BLOCKING); // sends only the y distance count = 0; // Byte packing for processing script /* for(uint16_t y = 0; y < RES_HEIGHT/2; y += 1) { for (uint16_t x = 0; x < RES_WIDTH/2; x += 8) { */ /* // Checkerboard for configuring things if(y&1) frameloc[y*20 + x/8] = 0x55; else frameloc[y*20 + x/8] = 0xAA; */ /* frameloc[y*(RES_WIDTH/16) + x/8] = (frameloc[(y*2+1)*RES_WIDTH + (2*(x+0)+1)] & 0x80) | (frameloc[(y*2+1)*RES_WIDTH + (2*(x+1)+1)] & 0x40) | (frameloc[(y*2+1)*RES_WIDTH + (2*(x+2)+1)] & 0x20) | (frameloc[(y*2+1)*RES_WIDTH + (2*(x+3)+1)] & 0x10) | (frameloc[(y*2+1)*RES_WIDTH + (2*(x+4)+1)] & 0x08) | (frameloc[(y*2+1)*RES_WIDTH + (2*(x+5)+1)] & 0x04) | (frameloc[(y*2+1)*RES_WIDTH + (2*(x+6)+1)] & 0x02) | (frameloc[(y*2+1)*RES_WIDTH + (2*(x+7)+1)] & 0x01); } } */ // UART_Send(LPC_USART0, frameloc, 16000, BLOCKING); // Send the frame to see it in tera term. NO BYTE PACK // frame[0] = 'A'; // key byte // UART_Send(LPC_USART0, frameloc, 2001, BLOCKING); // Send the frame byte packed to see it in processing //UART_Send(LPC_USART0, sendPositions, (RES_WIDTH - 2*POS_OFFSET)>>1, BLOCKING); // clear array for(uint8_t x = 0; x < 240; x++) { sendPositions[x] = 255; } toggleLED(); } }
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 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; } } }