Statistics
| Revision:

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

History | View | Annotate | Download (1.76 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

    
10
void highwayStart(){
11
        canMerge = 1;
12
}
13
int changeLanes(){
14
        if(counti<10000){
15
                counti++;
16
                motor_r_set(FORWARD,232);
17
                motor_l_set(FORWARD,200);
18
                return 0;
19
        }
20
        int colours[5];
21
        updateLine(colours);
22
        int pos = lineLocate(colours);
23
        if(pos >= 3 || pos <= -3){
24
                motor_r_set(FORWARD,187);
25
                motor_l_set(FORWARD,190);
26
                return 0;
27
        }
28
        counti = 0;
29
        return 1;
30
}
31
int highwayFSM(){
32
        switch(states){
33
        
34
        case 0: // Normal Drive
35
                {
36
                int theCode = doDrive(205);
37
                if(theCode != NORMAL && theCode >= 0)
38
                        return theCode;
39
                int range = range_read_distance(IR3);
40
                usb_puti(range);
41
                       usb_puts(" : ");
42
                usb_puti(averagei);
43
                usb_puts("\r\n");
44
                orb1_set_color(CYAN);
45
                       if(range == -1){
46
                        range = PASS_DISTANCE+50;
47
                }
48
                counti ++;
49
                averagei += range;
50
                if(counti>= COUNT_MAX){
51
                        if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)
52
                                states = 1;
53
                        averagei = 0;
54
                        counti = 0;
55
                }
56
                break;
57
                }
58
        case 1:
59
                        states = 2;
60
                        canMerge = 0;
61
        case 2:
62
                        if(changeLanes()){
63
                           states = 0;
64
                           orb1_set_color(GREEN);
65
                        }
66
                        else orb1_set_color(YELLOW);
67
                        break;
68
        }
69
        return 0;
70
}
71

    
72
/*
73
void driveHighway( void ){
74
        doDrive(200);
75
        int range = range_read_distance(IR2);
76
        usb_puti(range);
77
        usb_puts(" : ");
78
        usb_puti(average);
79
        usb_puts("\r\n");
80
        if(range == -1){
81
                return;
82
        }
83
        
84
        count ++;
85
        average += range;
86
        if(count >= COUNT_MAX){
87
                if(state[0] != MERGELEFT){
88
                        orb1_set_color(ORANGE);
89
                        if(average / COUNT_MAX < PASS_DISTANCE){
90
                                merge(ILEFT);
91
                                usb_puts("MERGE \r\n");        
92
                        }
93
                }
94
                
95
                count =0;
96
                average = 0;
97
        }
98
        //else orb1_set_color(GREEN);
99
}
100
*/