Statistics
| Revision:

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

History | View | Annotate | Download (1.41 KB)

1
#include "highways.h"
2
#include "lineDrive.h"
3
#include <dragonfly_lib.h>
4

    
5
static int counti =0;
6
static long averagei = 0;
7
static int states = 0;
8
int canMerge = 1;
9
void highwayStart(){
10
        canMerge = 1;
11
}
12
int highwayFSM(){
13
        switch(states){
14
        
15
        case 0: // Normal Drive
16
                {
17
                int theCode = doDrive(225);
18
                if(theCode != NORMAL && theCode >= 0)
19
                        return theCode;
20
                int range = range_read_distance(IR2);
21
                usb_puti(range);
22
                       usb_puts(" : ");
23
                usb_puti(averagei);
24
                usb_puts("\r\n");
25
                orb1_set_color(PURPLE);
26
                       if(range == -1){
27
                        range = 200;
28
                }
29
                counti ++;
30
                averagei += range;
31
                if(counti>= COUNT_MAX){
32
                        if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)
33
                                states = 1;
34
                        averagei = 0;
35
                        counti = 0;
36
                }
37
                break;
38
                }
39
        case 1:
40
                        merge(ILEFT);
41
                        states = 2;
42
                        canMerge = 0;
43
        case 2:
44
                        if(doDrive(160) == FINISHED) states = 0;
45
                        orb1_set_color(BLUE);
46
                        break;
47
        }
48
        return 0;
49
}
50

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