Cubelet.9825.c

Author: Jon Hiller

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
int AmLeft;
int IsForwardState;

void setup()
{
    AmLeft = 0;
    if (MyID0 == 236) AmLeft = 1; //set one block to Left (the other will be right)
    IsForwardState = 1;
}

void loop()
{
    think();
    act();
}

void think()
{
    block_value = get_block_value(8927); //the distance block
    if (IsForwardState==0 && block_value < 10){ //wall is far away, switch back to forward state
        wait (500); //turn a little more
        IsForwardState = 1;
        
    }
    else if (IsForwardState == 1 && block_value > 100){ //coming up to wall. switch to backup state
        IsForwardState = 0;
    }
}

void act()
{
    if (IsForwardState==1){
        set_drive_direction(FORWARD);
        set_drive(255);      
    }
    else {
        if (AmLeft==1){
            set_drive_direction(BACKWARD);
            set_drive(255);
        }
        else {
            set_drive(0);
        }
    }

}