int blobsSetup()
{
uint8_t c;
// setup camera mode
cam_setMode(CAM_MODE1);
#ifdef DA
// load lut if we've grabbed any frames lately
if (g_rawFrame.m_pixels)
cc_loadLut();
#endif
// if there have been any parameter changes, we should regenerate the LUT (do it regardless)
g_blobs->m_clut.generateLUT();
// setup qqueue and M0
g_qqueue->flush();
exec_runM0(0);
// flush serial receive queue
while(ser_getSerial()->receive(&c, 1));
g_state = 0; // reset recv state machine
return 0;
}
int32_t cc_getRLSFrame(uint32_t *memory, uint8_t *lut, bool sync)
{
int32_t res;
int32_t responseInt = -1;
// check mode, set if necessary
if ((res=cam_setMode(CAM_MODE1))<0)
return res;
// forward call to M0, get frame
if (sync)
{
g_chirpM0->callSync(g_getRLSFrameM0,
UINT32((uint32_t)memory), UINT32((uint32_t)lut), END_OUT_ARGS,
&responseInt, END_IN_ARGS);
return responseInt;
}
else
{
g_chirpM0->callAsync(g_getRLSFrameM0,
UINT32((uint32_t)memory), UINT32((uint32_t)lut), END_OUT_ARGS);
return 0;
}
}
int ptSetup()
{
// setup camera mode
cam_setMode(CAM_MODE1);
// extend range of servos (handled in params)
// rcs_setLimits(0, -200, 200); (handled in rcservo params)
// rcs_setLimits(1, -200, 200); (handled in rcservo params)
// Increasing the PWM frequency makes the servos zippier.
// Pixy updates at 50 Hz, so a default servo update freq of 50 Hz
// adds significant latency to the control loop--- increasing to 100 Hz decreases this.
// Increasing to more than 130 Hz or so creates buzzing, prob not good for the servo.
// rcs_setFreq(100); (handled in rcservo params)
ptLoadParams();
g_panLoop.reset();
g_tiltLoop.reset();
// load lut if we've grabbed any frames lately
if (g_rawFrame.m_pixels)
cc_loadLut();
// setup qqueue and M0
g_qqueue->flush();
exec_runM0(0);
return 0;
}
int blobsSetup()
{
// setup camera mode
cam_setMode(CAM_MODE1);
// setup spi
spi_setCallback(spiCallback);
// setup qqueue and M0
g_qqueue->flush();
exec_runM0(0);
return 0;
}
int blobsSetup()
{
// setup camera mode
cam_setMode(CAM_MODE1);
// load lut if we've grabbed any frames lately
if (g_rawFrame.m_pixels)
cc_loadLut();
// setup qqueue and M0
g_qqueue->flush();
exec_runM0(0);
return 0;
}
int32_t cam_getFrame(uint8_t *memory, uint32_t memSize, uint8_t type, uint16_t xOffset, uint16_t yOffset, uint16_t xWidth, uint16_t yWidth)
{
int32_t res;
int32_t responseInt = -1;
if (xWidth*yWidth>memSize)
return -2;
// check resolutions
res = type >> 4;
if (res==0)
{
if (xOffset+xWidth>CAM_RES0_WIDTH || yOffset+yWidth>CAM_RES0_HEIGHT)
return -1;
}
else if (res==1)
{
if (xOffset+xWidth>CAM_RES1_WIDTH || yOffset+yWidth>CAM_RES1_HEIGHT)
return -1;
}
else if (res==2)
{
if (xOffset+xWidth>CAM_RES2_WIDTH || yOffset+yWidth>CAM_RES2_HEIGHT)
return -1;
}
else
return -3;
// check mode, set if necessary
if ((res=cam_setMode(type&0x0f))<0)
return res;
// forward call to M0, get frame
g_chirpM0->callSync(g_getFrameM0,
UINT8(type), UINT32((uint32_t)memory), UINT16(xOffset), UINT16(yOffset), UINT16(xWidth), UINT16(yWidth), END_OUT_ARGS,
&responseInt, END_IN_ARGS);
if (responseInt==0)
{
g_rawFrame.m_pixels = memory;
g_rawFrame.m_width = xWidth;
g_rawFrame.m_height = yWidth;
}
return responseInt;
}
int chaseSetup()
{
// setup camera mode
cam_setMode(CAM_MODE1);
chaseLoadParams();
rcs_setPos(LEFT_AXIS, RCS_CENTER_POS);
rcs_setPos(RIGHT_AXIS, RCS_CENTER_POS);
// load lut if we've grabbed any frames lately
if (g_rawFrame.m_pixels)
cc_loadLut();
// setup qqueue and M0
g_qqueue->flush();
exec_runM0(0);
return 0;
}
int blobsSetup()
{
uint8_t c;
// setup camera mode
cam_setMode(CAM_MODE1);
// load lut if we've grabbed any frames lately
if (g_rawFrame.m_pixels)
cc_loadLut();
// setup qqueue and M0
g_qqueue->flush();
exec_runM0(0);
// flush serial receive queue
while(ser_getSerial()->receive(&c, 1));
return 0;
}
int cam_init()
{
g_sccb = new CSccb(0x60);
// flush sccb
g_sccb->Read(0xA0);
// start in mode 0
cam_setRegs(g_baseRegs, sizeof(g_baseRegs));
cam_setMode(0);
g_chirpUsb->registerModule(g_module);
g_getFrameM0 = g_chirpM0->getProc("getFrame", NULL);
if (g_getFrameM0>0)
return -1;
return 0;
}
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
}
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
}
