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