static void task_hard_routine(void * cookie) {
unsigned long n = 0; /* how many IRQ since init? */
RTIME time_prev = 0; /* when was the previous IRQ? */
RTIME time_curr = 0; /* when was the current IRQ? */
/* last IRQ is now! (init value) */
time_prev = rt_timer_read();
while (1) {
/* wait for an IRQ */
rt_intr_wait(&intr, TM_INFINITE);
/* fetch current timestamp */
time_curr = rt_timer_read();
/*
Ok, now we've got to check two things:
- is it a "good transition" (edge triggering, two IRQ = 1 hit)?
- is it a "real wheel rotation" (our crappy sensor wire sometimes
decides to become an antenna, so here's a nasty software filter...)
*/
#define EPSILON 100 * 1000 * 1000
if (++n % 2 && time_curr > time_prev + EPSILON) {
#undef EPSILON
/* post the current timestamp to the message queue */
rt_queue_write(&queue, &time_curr, sizeof time_curr, Q_NORMAL);
/* our job is done, we are now the previous IRQ */
time_prev = time_curr;
}
}
(void) cookie;
}
void taskOne(void *arg)
{
int retval;
char message[] = "Message from taskOne";
/* send message */
retval = rt_queue_write(&myqueue,message,sizeof(message),Q_NORMAL);
if (retval < 0 ) {
rt_printf("Sending error\n");
} else {
rt_printf("taskOne sent message to mailbox\n");
}
}
void lEnc(void *arg)
{
int l_last = LOW;
int inrl = 0;
int inrBl = 0;
int ltick = 0;
char lticks[40];
int fd = open("/dev/mem",O_RDWR | O_SYNC);
ulong* pinconf1 = (ulong*) mmap(NULL, 0x1000, PROT_READ | PROT_WRITE, MAP_SHARED, fd, GPIO1_ADDR);
//configure encoder pins as input
pinconf1[OE_ADDR/4] |= ((1<<15)|(1<<14)); //P8_11, P8_12, P8_15, P8_16
rt_task_set_periodic(NULL, TM_NOW, period);
rt_printf("Reading Left Encoder!\n");
while (1){
rt_task_wait_period(NULL);
if(pinconf1[GPIO_DATAIN/4] & (1 << 15)){
inrl = HIGH;
}else{
inrl = LOW;
}
if((l_last == LOW)&&(inrl == HIGH)){
if(pinconf1[GPIO_DATAIN/4] & (1 << 14)) {
inrBl = HIGH;
}else{
inrBl = LOW;
}
if(inrBl == LOW){
ltick--;
}else{
ltick++;
}
}
l_last = inrl;
//rt_printf("Left ticks: %d \n", ltick);
//send to odometry task
sprintf(lticks, "%d", ltick);
rt_queue_write(&lqueue, lticks, sizeof(lticks), Q_NORMAL);
}
return;
}
