__intrinsic void
reg_set(void *d, unsigned int s_val, size_t n)
{
/* Make sure the parameters are as we expect */
ctassert(__is_in_reg_or_lmem(d));
ctassert(__is_ct_const(n));
ctassert(n <= 64);
ctassert((n % 4) == 0);
#ifdef __REG_SET
#error "Attempting to redefine __REG_SET"
#endif
if (__is_in_lmem(d)) {
/* if type lmem must be defined as such */
#define __REG_SET(_x) ((__lmem unsigned int *)d)[_x] = s_val
_REG_UTILS_SWITCH_CASE_IMPLEMENT(n, __REG_SET);
#undef __REG_SET
} else {
/* else is a register type */
#define __REG_SET(_x) ((__gpr unsigned int *)d)[_x] = s_val
_REG_UTILS_SWITCH_CASE_IMPLEMENT(n, __REG_SET);
#undef __REG_SET
}
}
__intrinsic int
reg_eq(void *s1, void *s2, size_t n)
{
int ret = 0;
/* Make sure the parameters are as we expect */
ctassert(__is_in_reg_or_lmem(s1));
ctassert(__is_in_reg_or_lmem(s2));
ctassert(__is_ct_const(n));
ctassert(n <= 64);
ctassert((n % 4) == 0);
#ifdef __REG_EQ
#error "Attempting to redefine __REG_EQ"
#endif
if (__is_in_lmem(s1) && __is_in_lmem(s2)) {
/* both s1 and s2 are lmem */
#define __REG_EQ(_x) (((__lmem unsigned int *)s1)[_x] == \
((__lmem unsigned int *)s2)[_x])
_REG_UTILS_EQ_SWITCH_CASE_IMPLEMENT(n);
#undef __REG_EQ
} else if (__is_in_lmem(s1) && __is_in_reg(s2)) {
/* s1 is lmem and s2 is reg */
#define __REG_EQ(_x) (((__lmem unsigned int *)s1)[_x] == \
((__gpr unsigned int *)s2)[_x])
_REG_UTILS_EQ_SWITCH_CASE_IMPLEMENT(n);
#undef __REG_EQ
} else if (__is_in_reg(s1) && __is_in_lmem(s2)) {
/* s1 is reg and s2 is lmem */
#define __REG_EQ(_x) (((__gpr unsigned int *)s1)[_x] == \
((__lmem unsigned int *)s2)[_x])
_REG_UTILS_EQ_SWITCH_CASE_IMPLEMENT(n);
#undef __REG_EQ
} else {
/* s1 and s2 are reg */
#define __REG_EQ(_x) (((__gpr unsigned int *)s1)[_x] == \
((__gpr unsigned int *)s2)[_x])
_REG_UTILS_EQ_SWITCH_CASE_IMPLEMENT(n);
#undef __REG_EQ
}
return ret;
}
__intrinsic void
reg_cp(void *d, void *s, size_t n)
{
/* Make sure the parameters are as we expect */
ctassert(__is_in_reg_or_lmem(d));
ctassert(__is_in_reg_or_lmem(s));
ctassert(__is_ct_const(n));
ctassert(n <= 64);
ctassert((n % 4) == 0);
#ifdef __REG_CP
#error "Attempting to redefine __REG_CP"
#endif
if (__is_in_lmem(d) && __is_in_lmem(s)) {
/* both dst and src are lmem */
#define __REG_CP(_x) (((__lmem unsigned int *)d)[_x] = \
((__lmem unsigned int *)s)[_x])
_REG_UTILS_SWITCH_CASE_IMPLEMENT(n, __REG_CP);
#undef __REG_CP
} else if (__is_in_lmem(d) && __is_in_reg(s)){
/* dst is lmem and src is reg */
#define __REG_CP(_x) (((__lmem unsigned int *)d)[_x] = \
((__gpr unsigned int *)s)[_x])
_REG_UTILS_SWITCH_CASE_IMPLEMENT(n, __REG_CP);
#undef __REG_CP
} else if (__is_in_reg(d) && __is_in_lmem(s)){
/* else if dst is reg and src is lmem */
#define __REG_CP(_x) (((__gpr unsigned int *)d)[_x] = \
((__lmem unsigned int *)s)[_x])
_REG_UTILS_SWITCH_CASE_IMPLEMENT(n, __REG_CP);
#undef __REG_CP
} else {
/* both dst and src are reg */
#define __REG_CP(_x) (((__gpr unsigned int *)d)[_x] = \
((__gpr unsigned int *)s)[_x])
_REG_UTILS_SWITCH_CASE_IMPLEMENT(n, __REG_CP);
#undef __REG_CP
}
}
void Pixel565toHSV::test()
{
ctassert(convert(Pixel565::black()) == HSV( 0, 0, 0));
ctassert(convert(Pixel565::white()) == HSV( 0, 0,255));
ctassert(convert(Pixel565::red()) == HSV( 0,255,255));
ctassert(convert(Pixel565::yellow()) == HSV( 60,255,255));
ctassert(convert(Pixel565::green()) == HSV(120,255,255));
ctassert(convert(Pixel565::cyan()) == HSV(180,255,255));
ctassert(convert(Pixel565::blue()) == HSV(240,255,255));
ctassert(convert(Pixel565::magenta()) == HSV(300,255,255));
}
Vision::Vision() :
m_colorTracker(Vision::NUM_CHANNELS)
{
ctassert(sizeof(Pixel565) == 2);
ctassert(sizeof(ushort) == 2);
ctassert(sizeof(short) == 2);
Pixel565toHSV::init();
m_camera=NULL;
#ifdef HAS_MICRODIA_CAMERA
bool use_simulated_camera = false; // change to true to use simulated cam on chumby
#else
bool use_simulated_camera = true;
#endif
if (use_simulated_camera) {
printf("Using simulated camera\n");
SimulatedCamera *sc = new SimulatedCamera(160, 120);
//Image simulatedImage(120, 160);
//simulatedImage.loadRaw("/mnt/usb/crashframe.raw");
QImage simulatedImage("simulated.png");
if (!simulatedImage.width()) {
fprintf(stderr, "Can't load simulated.png");
exit(1);
}
sc->loadSimulatedImage(simulatedImage);
m_camera = sc;
} else {
#ifdef HAS_MICRODIA_CAMERA
printf("Using Microdia camera\n");
m_camera = new MicrodiaCamera();
#else
ctassert(0);
#endif
}
m_colorTracker.shareResults("/tmp/color_tracking_results");
ctassert(m_camera);
m_camera->addFrameHandler(&m_colorTracker);
m_camera->requestContinuousFrames();
}
void ROTARY_Init()
{
// This assumes all rotary pins are handled by the same interrupt
ctassert(ROTARY_PIN0.port == ROTARY_PIN1.port, All_rotary_pins_0_1_must_be_on_same_port);
ctassert(ROTARY_PIN0.port == ROTARY_PRESS_PIN.port, rotary_pins_0_ent_must_be_on_same_port);
ctassert(NVIC_EXTIx_IRQ(ROTARY_PIN0) == NVIC_EXTIx_IRQ(ROTARY_PIN1), rotary_pins_0_1_must_be_on_same_irq);
ctassert(NVIC_EXTIx_IRQ(ROTARY_PIN0) == NVIC_EXTIx_IRQ(ROTARY_PRESS_PIN), rotary_pins_0_ent_must_be_on_same_irq);
// All pins guaranteed to be on the same port
rcc_periph_clock_enable(get_rcc_from_pin(ROTARY_PIN0));
/*Rotary */
GPIO_setup_input(ROTARY_PIN0, ITYPE_PULLUP);
GPIO_setup_input(ROTARY_PIN1, ITYPE_PULLUP);
GPIO_setup_input(ROTARY_PRESS_PIN, ITYPE_PULLUP);
nvic_enable_irq(NVIC_EXTIx_IRQ(ROTARY_PIN0));
nvic_set_priority(NVIC_EXTIx_IRQ(ROTARY_PIN0), 66); // Medium priority
exti_select_source(EXTIx(ROTARY_PIN0) | EXTIx(ROTARY_PIN1), ROTARY_PIN0.port);
exti_set_trigger(EXTIx(ROTARY_PIN0) | EXTIx(ROTARY_PIN1), EXTI_TRIGGER_BOTH);
exti_enable_request(EXTIx(ROTARY_PIN0) | EXTIx(ROTARY_PIN1));
}
