void
command_start_group(uint32_t *args)
{
sched_del_timer(&group_timer);
group_timer.func = group_end_event;
group_timer.waketime = args[0];
sched_add_timer(&group_timer);
}
static void object_avoider_next(object_avoider_state_t* oas)
{
if (oas->timer == NULL)
{
/* rotate one quadran (freq ~= 2) */
is_rotate_done = 0;
move_rotate_left();
oas->timer = sched_add_timer(2, on_rotate_timer, 1);
}
else if (is_rotate_done)
{
object_avoider_stop(oas);
}
}
// Schedule a set of steps with a given timing
void
command_queue_step(uint32_t *args)
{
struct stepper *s = stepper_oid_lookup(args[0]);
struct stepper_move *m = move_alloc();
m->interval = args[1];
m->count = args[2];
if (!m->count)
shutdown("Invalid count parameter");
m->add = args[3];
m->next = NULL;
m->flags = 0;
irq_disable();
uint8_t flags = s->flags;
if (!!(flags & SF_LAST_DIR) != !!(flags & SF_NEXT_DIR)) {
flags ^= SF_LAST_DIR;
m->flags |= MF_DIR;
}
flags &= ~SF_NO_NEXT_CHECK;
if (m->count == 1 && (m->flags || flags & SF_LAST_RESET))
// count=1 moves after a reset or dir change can have small intervals
flags |= SF_NO_NEXT_CHECK;
s->flags = flags & ~SF_LAST_RESET;
if (s->count) {
if (s->first)
*s->plast = m;
else
s->first = m;
s->plast = &m->next;
} else if (flags & SF_NEED_RESET) {
move_free(m);
} else {
s->first = m;
stepper_load_next(s, s->next_step_time + m->interval);
sched_add_timer(&s->time);
}
irq_enable();
}
void main(void)
{
sched_timer_t* light_timer;
sched_timer_t* dist_timer;
unsigned char is_light_disabled;
unsigned char is_dist_disabled;
osc_setup();
int_setup();
srf04_setup();
sched_setup();
behavior_setup();
/* sensor timers */
light_timer = sched_add_timer(2, on_light_timer, 1);
is_light_disabled = 0;
dist_timer = sched_add_timer(2, on_distance_timer, 1);
is_dist_disabled = 0;
/* default behaviour */
behavior_start(BEHAVIOR_ID_LAND_EXPLORER);
/* main loop */
sched_enable();
while (1)
{
/* sensors. order matters so that object
avoider takes priority over light tracker.
this can be removed as soon as behavior
priority is implemented.
*/
if (TIMER_MAP_ISSET(DISTANCE))
{
{
#define MIN_DISTANCE_VALUE 0x0a00 /* 20 cms */
unsigned int dist = srf04_get_distance(MIN_DISTANCE_VALUE);
if (dist <= MIN_DISTANCE_VALUE)
{
if (behavior_switch(BEHAVIOR_ID_OBJECT_AVOIDER) != -1)
{
sched_disable_timer(dist_timer);
is_dist_disabled = 1;
}
}
}
/* clear after the timer may have been disabled */
TIMER_MAP_CLEAR(DISTANCE);
}
if (TIMER_MAP_ISSET(LIGHT))
{
{
unsigned short light = adc_read(LIGHT_ADC_CHANNEL);
if ((light <= ADC_QUANTIZE_5_10(2.35)) || (light >= ADC_QUANTIZE_5_10(2.65)))
{
if (behavior_switch(BEHAVIOR_ID_LIGHT_TRACKER) != -1)
{
sched_disable_timer(light_timer);
is_light_disabled = 1;
}
}
}
TIMER_MAP_CLEAR(LIGHT);
}
/* schedule behavior */
behavior_next();
if (behavior_is_done())
{
/* reenable timer */
if (is_light_disabled)
{
sched_enable_timer(light_timer);
is_light_disabled = 0;
}
if (is_dist_disabled)
{
sched_enable_timer(dist_timer);
is_dist_disabled = 0;
}
/* switch to default behavior */
behavior_switch(BEHAVIOR_ID_DEFAULT);
}
}
}
static void light_tracker_next(light_tracker_state_t* lts)
{
switch (lts->state)
{
case LIGHT_TRACKER_STATE_INIT:
{
lts->state = LIGHT_TRACKER_STATE_SENSE;
/* fallthru */
}
case LIGHT_TRACKER_STATE_SENSE:
{
void (*rotate)(void);
unsigned short cur_level;
unsigned short cur_delta;
cur_level = adc_read(LIGHT_ADC_CHANNEL);
if (cur_level >= ADC_QUANTIZE_5_10(2.35))
if (cur_level <= ADC_QUANTIZE_5_10(2.65))
{
/* found, we are done */
lts->state = LIGHT_TRACKER_STATE_DONE;
break;
}
if (cur_level >= ADC_QUANTIZE_5_10(2.5))
{
cur_delta = cur_level - ADC_QUANTIZE_5_10(2.5);
rotate = move_rotate_left;
}
else
{
cur_delta = ADC_QUANTIZE_5_10(2.5) - cur_level;
rotate = move_rotate_right;
}
if (lts->prev_delta < cur_delta)
{
/* diverging, we are done */
lts->state = LIGHT_TRACKER_STATE_DONE;
break;
}
lts->prev_delta = cur_delta;
lts->rem_delta = cur_delta;
rotate();
lts->timer = sched_add_timer(0, on_light_tracker_timer, 0);
lts->state = LIGHT_TRACKER_STATE_ROTATE;
break;
}
case LIGHT_TRACKER_STATE_ROTATE:
{
unsigned int tmp;
if (!lts->rem_delta)
{
/* rotation done */
move_stop();
sched_del_timer(lts->timer);
lts->timer = NULL;
lts->state = LIGHT_TRACKER_STATE_SENSE;
break;
}
tmp = delta_to_freq(lts->rem_delta);
if (tmp > SCHED_MAX_FREQ)
tmp = SCHED_MAX_FREQ;
sched_set_timer_freq(lts->timer, tmp);
tmp = freq_to_delta(tmp);
if (tmp > lts->rem_delta)
tmp = lts->rem_delta;
lts->rem_delta -= tmp;
lts->state = LIGHT_TRACKER_STATE_WAIT;
is_rotation_done = 0;
sched_enable_timer(lts->timer);
break;
}
case LIGHT_TRACKER_STATE_WAIT:
{
/* wait for the rotation to finish */
if (!is_rotation_done)
break;
sched_disable_timer(lts->timer);
lts->state = LIGHT_TRACKER_STATE_ROTATE;
break;
}
case LIGHT_TRACKER_STATE_DONE:
default:
{
light_tracker_stop(lts);
break;
}
}
}
