I think I can see that program hanging.
Back to my last idea.
Lets say you have a timer interrupt triggering every 2msec and you have a global variable
int COUNTER;
Then you main loop could be something like
main()
{
while(1)
}
and you have a timer interrupt like
timer_interrupt()
{
COUNTER++;
COUNTER = COUNTER & 0x000F; // counts 0 .. 15
switch(COUNTER)
{
case 0:
run_high_priority_stuff();
break;
case 1:
run_high_priority_stuff();
do_some_other_things();
break;
case 2:
run_high_priority_stuff();
check_the_pedal();
break;
.
.
.
.
case 15:
run_high_priority_stuff();
}
}
From there everything is written as a function and simply called somewhere in the switch statement.
How is that for a challenge?