Statistics
| Revision:

root / trunk / code / projects / traffic_navigation / highways.c @ 1889

History | View | Annotate | Download (1.28 KB)

1
#include "highways.h"
2
#include "lineDrive.h"
3
#include <dragonfly_lib.h>
4
static int counti =0;
5
static long averagei = 0;
6
static int states = 0;
7
int canMerge = 1;
8

    
9
void highwayFSM(){
10
        switch(states){
11
        
12
        case 0: // Normal Drive
13
                doDrive(225);
14
                int range = range_read_distance(IR3);
15
                usb_puti(range);
16
                       usb_puts(" : ");
17
                usb_puti(averagei);
18
                usb_puts("\r\n");
19
                orb1_set_color(PURPLE);
20
                       if(range == -1){
21
                        return;
22
                }
23
                counti ++;
24
                averagei += range;
25
                if(counti>= COUNT_MAX){
26
                        if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)
27
                                states = 1;
28
                        averagei = 0;
29
                        counti = 0;
30
                }
31
                break;
32
                case 1:
33
                        merge(ILEFT);
34
                        states = 2;
35
                        canMerge = 0;
36
                case 2:
37
                        if(doDrive(160) == FINISHED) states = 0;
38
                        orb1_set_color(BLUE);
39
                        break;
40
        }
41
}
42

    
43
/*
44
void driveHighway( void ){
45
        doDrive(200);
46
        int range = range_read_distance(IR2);
47
        usb_puti(range);
48
        usb_puts(" : ");
49
        usb_puti(average);
50
        usb_puts("\r\n");
51
        if(range == -1){
52
                return;
53
        }
54
        
55
        count ++;
56
        average += range;
57
        if(count >= COUNT_MAX){
58
                if(state[0] != MERGELEFT){
59
                        orb1_set_color(ORANGE);
60
                        if(average / COUNT_MAX < PASS_DISTANCE){
61
                                merge(ILEFT);
62
                                usb_puts("MERGE \r\n");        
63
                        }
64
                }
65
                
66
                count =0;
67
                average = 0;
68
        }
69
        //else orb1_set_color(GREEN);
70
}
71
*/