Statistics
| Revision:

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

History | View | Annotate | Download (1.77 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
*/
40
                        int range = range_read_distance(IR2);
41
                usb_puti(range);
42
                       usb_puts(" : ");
43
                usb_puti(averagei);
44
                usb_puts("\r\n");
45
                orb1_set_color(CYAN);
46
                       if(range == -1){
47
                        range = 200;
48
                }
49
                counti ++;
50
                averagei += range;
51
                if(counti>= COUNT_MAX){
52
                        if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)
53
                                states = 1;
54
                        averagei = 0;
55
                        counti = 0;
56
                }
57
                break;
58
                }
59
        case 1:
60
                        //merge(ILEFT);
61
                        states = 2;
62
                        canMerge = 0;
63
        case 2:
64
                        if(changeLanes()){
65
                           states = 0;
66
                           orb1_set_color(GREEN);
67
                        }
68
                        else orb1_set_color(YELLOW);
69
                        break;
70
        }
71
        return 0;
72
}
73

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