| |
I've managed to get a proximity detection working in legOS, using egcs 1.1.1 as
the cross compiler. It has a very short range, and I haven't yet tested if the
NQC variants have better range. If that is the case, I believe somebody will
have to fix lnp_logical_range() somehow. Anyway, here goes:
#include <sys/lnp-logical.h>
#include <dsensor.h>
#include <dmotor.h>
#include <conio.h>
#include <unistd.h>
#define BEHAVIOR_BACKUP 1 /* alternative is spin */
int blinker(int, char**) __attribute__ ((__noreturn__));
int ticker(int, char**) __attribute__ ((__noreturn__));
int main(int, char**) __attribute__ ((__noreturn__));
volatile int proximity, tick, amb;
wakeup_t tooclose(wakeup_t t)
{
if(t)
return proximity>8; /* if we got more than 8 ticks last time */
else
return !proximity;
}
int main(int argc, char **argv)
{
pid_t b;
proximity=0; /* initialize to no obstacles */
b=execi(blinker, 0, NULL, PRIO_NORMAL, DEFAULT_STACK_SIZE); /* start the
radar */
motor_c_dir(fwd); /* start driving */
motor_a_dir(fwd);
while(1)
{
wait_event(tooclose, 1); /* wait until we detect something in front of us
*/
motor_a_dir(brake); /* stop */
motor_c_dir(brake);
msleep(200);
#if BEHAVIOR_BACKUP
motor_a_dir(rev); /* back up */
motor_c_dir(rev);
msleep(400);
#else /* spin in place until we don't see any obstacles */
motor_a_dir(rev);
motor_c_dir(fwd);
wait_event(tooclose, 0);
#endif
motor_a_dir(brake); /* stop again */
motor_c_dir(brake);
msleep(200);
motor_a_dir(fwd);
#if BEHAVIOR_BACKUP /* this is where the backup version turns */
msleep(300);
#endif
motor_c_dir(fwd); /* back to full forward */
}
}
int blinker(int argc, char **argv)
{
char buf[]={0xff, 0xff, 0xff, 0xff}; /* less than three reduces range,
more than four don't seem to improve things */
lnp_logical_range(1); /* high-power IR */
execi(ticker, 0, NULL, PRIO_NORMAL, DEFAULT_STACK_SIZE); /* start ticker
thread */
while(1)
{
amb=SENSOR_2-0x80; /* read ambience level */
tick=0; /* reset tick counter (ticker starts its measuring now */
lnp_logical_write(buf, sizeof(buf)); /* flash the IR */
proximity=tick; /* after flash, we can retrieve the tick count */
}
}
int ticker(int argc, char **argv)
{
while(1)
if(SENSOR_2<amb) /* just increase tick if we see more light than amb */
tick++;
}
|
|
1 Message in This Thread:
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
