right_drive.c

Author: Eric Lundby

Code for the right Drive Block of a Wall Avoiding Robot. 

 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
/*
    Robot: Wall Avoiding Robot
    Cubelet: Right Drive Block
*/

uint32_t DISTANCE_ID = 19599;

void setup()
{

}

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

void think()
{
    block_value = weighted_average(neighbor_data);
}

void act()
{
    if(get_block_value(DISTANCE_ID) > 200)
    {
        //Go Backwards
        toggle_directions();
        set_drive(0xdd);
        wait(1000);
        
        //Go forward with one drive block(this one) to turn
        set_drive(0x00);
        toggle_directions();
        set_drive(127);
        wait(1500);
    }
    else
    {
        set_drive(0xdd);
    }
    
}