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