Statistics
| Revision:

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

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(IR2);
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

    
24
                counti ++;
25
                averagei += range;
26
                if(counti>= COUNT_MAX){
27
                        if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)
28
                                states = 1;
29
                        averagei = 0;
30
                        counti = 0;
31
                }
32
                break;
33
                case 1:
34
                        merge(ILEFT);
35
                        states = 2;
36
                        canMerge = 0;
37
                case 2:
38
                        if(doDrive(160) == FINISHED) states = 0;
39
                        orb1_set_color(BLUE);
40
                        break;
41
        }
42
}
43

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