Building Robots With Lego Mindstorms

Simulating Flight • Chapter 24 487

At this point, there’s one last thing you have to code: the motion of the plat-

form.We used a simple system to control speed: In a given period, the motors stay

on for a time proportional to speed.These instructions are in a separate task that

loops with a tighter interval than the main one so motion results more smoothly

than it would in a loop of 1 second. Our program cycles every 20 hundredths.

#define PERIOD 20

while (true)

{

if (speed==0)

{

Off(OUT_A+OUT_B);

}

else

{

on_left=(speed*PERIOD)/100;

on_right=on_left-turn*3;

on_left+=turn*3;

OnFwd(OUT_A+OUT_B);

if (on_left==on_right)

{

Wait(on_left);

Float(OUT_A+OUT_B);

Wait(PERIOD-on_left); ...