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