void Filter::input(double v)
{
double data[FILTER_BUF];
double tmp;
int i, j;
m_trajectory[m_iTraj] = v;
if(++m_iTraj >= m_windowLength)
{
m_iTraj = 0;
}
for (i = 0; i<m_windowLength; i++)
{
data[i] = m_trajectory[i];
for (j = i; j>0; j--)
{
if (data[j] < data[j - 1])
{
SWITCH(data[j], data[j - 1], tmp);
}
else
{
break;
}
}
}
m_vMid = data[m_iMedian];
}
static void
vips_math2_buffer( VipsArithmetic *arithmetic,
PEL *out, PEL **in, int width )
{
VipsMath2 *math2 = (VipsMath2 *) arithmetic;
VipsImage *im = arithmetic->ready[0];
const int sz = width * vips_image_get_bands( im );
int x;
switch( math2->math2 ) {
case VIPS_OPERATION_MATH2_POW: SWITCH( LOOP, POW ); break;
case VIPS_OPERATION_MATH2_WOP: SWITCH( LOOP, WOP ); break;
default:
g_assert( 0 );
}
}
static void set_led(bool on)
{
#define ON(color) ((current_color & color) ? LOW : HIGH)
#define OFF(color) HIGH
#define SWITCH(on, color) (on ? ON(color) : OFF(color))
DEBUG_PRINTLN2("set_led ", on);
DEBUG_PRINTLN2("color ", current_color);
current_enabled = on;
digitalWrite(RED_LED, SWITCH(on, 4));
digitalWrite(GREEN_LED, SWITCH(on, 2));
digitalWrite(BLUE_LED, SWITCH(on, 1));
#undef SWITCH
#undef OFF
#undef ON
}
static void
vips_complex_buffer( VipsArithmetic *arithmetic,
VipsPel *out, VipsPel **in, int width )
{
VipsComplex *cmplx = (VipsComplex *) arithmetic;
VipsImage *im = arithmetic->ready[0];
const int sz = width * vips_image_get_bands( im );
int x;
switch( cmplx->cmplx ) {
case VIPS_OPERATION_COMPLEX_POLAR: SWITCH( POLAR ); break;
case VIPS_OPERATION_COMPLEX_RECT: SWITCH( RECT ); break;
case VIPS_OPERATION_COMPLEX_CONJ: SWITCH( CONJ ); break;
default:
g_assert( 0 );
}
}
static void
vips_round_buffer( VipsArithmetic *arithmetic,
VipsPel *out, VipsPel **in, int width )
{
VipsRound *round = (VipsRound *) arithmetic;
VipsImage *im = arithmetic->ready[0];
/* Complex just doubles the size.
*/
const int sz = width * im->Bands *
(vips_bandfmt_iscomplex( im->BandFmt ) ? 2 : 1);
int x;
switch( round->round ) {
case VIPS_OPERATION_ROUND_RINT: SWITCH( VIPS_RINT ); break;
case VIPS_OPERATION_ROUND_CEIL: SWITCH( ceil ); break;
case VIPS_OPERATION_ROUND_FLOOR: SWITCH( floor ); break;
default:
g_assert( 0 );
}
}
// Here go just wrapper functions for export into Python
boost::python::list SWITCH(VECTOR r1,VECTOR r2, double R_on,double R_off){
boost::python::list res;
double SW = 0.0;
VECTOR dSW;
SWITCH(r1,r2,R_on,R_off,SW,dSW);
res.append(SW);
res.append(dSW);
return res;
}
void scheduler(void)
{
uint32_t *stack;
uint32_t esp0, eflags;
uint16_t ss, cs;
if (nb_proc == 0){
return;
}
else if (nb_proc >= 2){
stack = GET_STACK();
ktss.esp0 = (uint32_t)(stack+19);
/* save context execution */
current_proc->regs.gs = stack[2];
current_proc->regs.fs = stack[3];
current_proc->regs.es = stack[4];
current_proc->regs.ds = stack[5];
current_proc->regs.edi = stack[6];
current_proc->regs.esi = stack[7];
current_proc->regs.ebp = stack[8];
current_proc->regs.ebx = stack[10];
current_proc->regs.edx = stack[11];
current_proc->regs.ecx = stack[12];
current_proc->regs.eax = stack[13];
current_proc->regs.eip = stack[14];
current_proc->regs.cs = stack[15];
current_proc->regs.eflags = stack[16];
current_proc->regs.esp = stack[17];
current_proc->regs.ss = stack[18];
current_proc = current_proc->next;
}
/* if !current_proc, init current_proc, because is a first proc */
if (!current_proc)
current_proc = last_proc;
ss = current_proc->regs.ss;
cs = current_proc->regs.cs;
eflags = (current_proc->regs.eflags | 0x200) & 0xFFFFBFFF;
esp0 = ktss.esp0;
SWITCH();
}
void Kollision(void){
char switches = '0';
char zero = '0';
char one = '0';
char two = '0';
char three = '0';
char four = '0';
char five = '0';
switches = PollSwitch() & PollSwitch() & 0b00111111;
zero = switches & SWITCH(1);
one = switches & SWITCH(2);
two = switches & SWITCH(1);
three = switches & SWITCH(2);
four = switches & SWITCH(1);
five = switches & SWITCH(2);
if(zero || one || two || three || four || five){
MotorSpeed(0,0);
StereoSound(duck_melody,duck_melody);
Lichtorgel();
}
}
void main (void) {
volatile uint32_t i = 0; /* Dummy idle counter */
uint8_t option;
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
/* Loop forever */
for (;;)
{
TransmitData("\n\r**The Freescale Cup**");
TransmitData("\n\r*********************");
TransmitData("\n\r1.Led\n\r");
TransmitData("2.Switch\n\r");
TransmitData("3.Servo\n\r");
TransmitData("4.Motor Left\n\r");
TransmitData("5.Motor Right\n\r");
TransmitData("6.Camera\n\r");
TransmitData("9.Camera 2");
TransmitData("7.Left Motor Current\n\r");
TransmitData("8.Right Motor Current");
TransmitData("\n\r**********************");
option = ReadData();
switch(option)
{
case '1':
LED();
break;
case '2':
SWITCH();
break;
case '3':
SERVO();
break;
case '4':
MOTOR_LEFT();
break;
case '5':
MOTOR_RIGHT();
break;
case '6':
CAMERA();
break;
case '7':
LEFT_MOTOR_CURRENT();
break;
case '8':
RIGHT_MOTOR_CURRENT();
break;
case '9':
CAMERA2();
break;
default:
break;
}
}
}
