void handleButton() { static uint32_t btPrev = 0; uint32_t bt; static uint8_t r, g, b; bt = button(); if (bt) { cam_getFrame((uint8_t *)SRAM0_LOC, SRAM0_SIZE, 0x21, 0, 0, 320, 200); getColor(&r, &g, &b); saturate(&r, &g, &b); led_setRGB(r, g, b); } else if (btPrev) { led_setRGB(0, 0, 0); delayus(50000); led_setRGB(r, g, b); delayus(50000); led_setRGB(0, 0, 0); delayus(50000); led_setRGB(r, g, b); delayus(50000); led_setRGB(0, 0, 0); } btPrev = bt; }
int32_t cam_getFrameChirpFlags(const uint8_t &type, const uint16_t &xOffset, const uint16_t &yOffset, const uint16_t &xWidth, const uint16_t &yWidth, Chirp *chirp, uint8_t renderFlags) { int32_t result, len; uint8_t *frame = (uint8_t *)SRAM1_LOC; // fill buffer contents manually for return data len = Chirp::serialize(chirp, frame, SRAM1_SIZE, HTYPE(FOURCC('B','A','8','1')), HINT8(renderFlags), UINT16(xWidth), UINT16(yWidth), UINTS8_NO_COPY(xWidth*yWidth), END); // write frame after chirp args result = cam_getFrame(frame+len, SRAM1_SIZE-len, type, xOffset, yOffset, xWidth, yWidth); // tell chirp to use this buffer chirp->useBuffer(frame, len+xWidth*yWidth); return result; }
int32_t cam_getFrameChirp(const uint8_t &type, const uint16_t &xOffset, const uint16_t &yOffset, const uint16_t &xWidth, const uint16_t &yWidth, Chirp *chirp) { int32_t result, prebuf; uint8_t *frame = (uint8_t *)SRAM0_LOC; // force an error to get prebuf length CRP_RETURN(chirp, USE_BUFFER(SRAM0_SIZE, frame), HTYPE(0), UINT16(0), UINT16(0), UINTS8(0, 0), END); prebuf = chirp->getPreBufLen(); if ((result=cam_getFrame(frame+prebuf, SRAM0_SIZE-prebuf, type, xOffset, yOffset, xWidth, yWidth))>=0) // send frame, use in-place buffer CRP_RETURN(chirp, USE_BUFFER(SRAM0_SIZE, frame), HTYPE(FOURCC('B','A','8','1')), UINT16(xWidth), UINT16(yWidth), UINTS8(xWidth*yWidth, frame+prebuf), END); return result; }
void sendCustom(uint8_t renderFlags=RENDER_FLAG_FLUSH) { int32_t len; uint8_t *frame = (uint8_t *)SRAM1_LOC; uint32_t fcc; // fill buffer contents manually for return data len = Chirp::serialize(g_chirpUsb, frame, SRAM1_SIZE, HTYPE(FOURCC('B','A','8','1')), HINT8(renderFlags), UINT16(CAM_RES2_WIDTH), UINT16(CAM_RES2_HEIGHT), UINTS8_NO_COPY(CAM_RES2_WIDTH*CAM_RES2_HEIGHT), END); // write frame after chirp args cam_getFrame(frame+len, SRAM1_SIZE-len, CAM_GRAB_M1R2, 0, 0, CAM_RES2_WIDTH, CAM_RES2_HEIGHT); //convolutionImage(CAM_RES2_WIDTH, CAM_RES2_HEIGHT, len, frame, true); //inverceImage(CAM_RES2_WIDTH, CAM_RES2_HEIGHT, len, frame); // tell chirp to use this buffer g_chirpUsb->useBuffer(frame, len+CAM_RES2_WIDTH*CAM_RES2_HEIGHT); // if (g_execArg==1) // { // cprintf("g_execArg==1"); // // fill buffer contents manually for return data // len = Chirp::serialize(g_chirpUsb, frame, SRAM1_SIZE, HTYPE(FOURCC('C','M','V','2')), HINT8(renderFlags), UINT16(CAM_RES2_WIDTH), UINT16(CAM_RES2_HEIGHT), UINTS8_NO_COPY(CAM_RES2_WIDTH*CAM_RES2_HEIGHT), END); // // write frame after chirp args // cam_getFrame(frame+len, SRAM1_SIZE-len, CAM_GRAB_M1R2, 0, 0, CAM_RES2_WIDTH, CAM_RES2_HEIGHT); // //inverceImage(CAM_RES2_WIDTH, CAM_RES2_HEIGHT, len, frame); // // tell chirp to use this buffer // g_chirpUsb->useBuffer(frame, len+CAM_RES2_WIDTH*CAM_RES2_HEIGHT); // } // else if (100<=g_execArg && g_execArg<200) // { // cprintf("100<=g_execArg && g_execArg<200"); // fcc = FOURCC('E','X',(g_execArg%100)/10 + '0', (g_execArg%10) + '0'); // len = Chirp::serialize(g_chirpUsb, frame, SRAM1_SIZE, HTYPE(fcc), HINT8(renderFlags), UINT16(CAM_RES2_WIDTH), UINT16(CAM_RES2_HEIGHT), UINTS8_NO_COPY(CAM_RES2_WIDTH*CAM_RES2_HEIGHT), END); // // write frame after chirp args // cam_getFrame(frame+len, SRAM1_SIZE-len, CAM_GRAB_M1R2, 0, 0, CAM_RES2_WIDTH, CAM_RES2_HEIGHT); // //inverceImage(CAM_RES2_WIDTH, CAM_RES2_HEIGHT, len, frame); // // tell chirp to use this buffer // g_chirpUsb->useBuffer(frame, len+CAM_RES2_WIDTH*CAM_RES2_HEIGHT); // } // else // { // cprintf("sendCustom else"); // cam_getFrameChirp(CAM_GRAB_M1R2, 0, 0, CAM_RES2_WIDTH, CAM_RES2_HEIGHT, g_chirpUsb); // } }
int main(void) { // pixyInit(SRAM3_LOC, &LR0[0], sizeof(LR0)); #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 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(); } }
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); } } }