left_drive.c

Author: Eric Lundby

Code for the left 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
45
/*
    Robot: Wall Avoiding Robot
    Cubelet: Left Drive Block
*/

//ID of the distance 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)//If we detect an object
    {
        //Go backwards for 1 second
        toggle_directions();
        set_drive(0xdd);
        wait(2000);
        toggle_directions();
        
        //Stop while the other block drives so you can turn
        //set_drive(0x00);
        //toggle_directions();
        //wait(1000);
    }
    else//Drive forward
    {
        set_drive(0xdd);
    }
    
}