Statistics
| Revision:

root / trunk / code / projects / linefollowing / main.c @ 1839

History | View | Annotate | Download (2.62 KB)

1
#include <dragonfly_lib.h>
2

    
3
#define WHITE                        0
4
#define GREY                        1
5
#define BLACK                         2
6
#define COLOR_BUFFER        10
7
#define CENTER                        3
8
#define NOLINE                        -42
9

    
10
int assignColor(int port, int analog);
11
int updateIR(int n);
12
void updateLine(int* values); 
13
int lineLocate(int* colors);
14
int straightLineFollow();
15

    
16

    
17

    
18
int main(void)
19
{
20

    
21
        /* initialize components, set wireless channel */
22
        dragonfly_init(ALL_ON);
23
        
24
        straightLineFollow();
25

    
26
        return 0;
27
}
28

    
29

    
30

    
31
//Converts an analog color to the ternary black, grey, white system.
32
//If the value is between two codes, the port is read again and the function is called again
33
int assignColor(int port, int analog)
34
{
35
        int color;
36
        
37
                if(analog < 341-COLOR_BUFFER)
38
                        color = WHITE;
39
                else if (analog < 341+COLOR_BUFFER)
40
                        color = assignColor(port, updateIR(port));
41
                else if (analog < 682-COLOR_BUFFER)
42
                        color = GREY; 
43
                else if (analog < 682+COLOR_BUFFER)
44
                        color = assignColor(port, updateIR(port));
45
                else
46
                        color = BLACK;
47
                        
48
                return color;
49

    
50
}
51

    
52
//Gets the value of the nth IR sensor
53
int updateIR(int n)
54
{
55
        //maps the sensors to the analog input ports
56
        int ports[5] = {13, 12, 3, 2, 9};
57

    
58
        return analog10(ports[n]);
59
}
60

    
61
//Gets the value of the nth IR sensor
62
int updateBarCode()
63
{
64
        //maps the sensors to the analog input ports
65
        int ports[2] = {8,1};
66
        
67
        return analog10(ports[1]) | analog10(ports[2]);
68
}
69

    
70
// updates all of the values, and assigns them new colors
71
void updateLine(int* values)
72
{        
73
        int i;
74

    
75
        for(i = 0; i<5; i++)
76
                values[i] = assignColor(i, updateIR(i)); 
77
}
78

    
79
// given an array of ints this function will take the arrays weighted average. High numbers indicate that a left turn is needed. Low numberes indicate that a right turn is needed.
80
int lineLocate(int* colors)
81
{
82
        int i;
83
        int wsum = 0;
84
        int count=0;
85

    
86
        for(i = 0; i<5; i++)
87
        {
88
                count+=(colors[i]+1)/2;
89
                wsum += (i)*colors[i];
90
        }
91
        if(count==0)
92
                        return NOLINE;        
93
                
94
        return (wsum/count)-4;
95
}
96

    
97
//The robot will follow a line until it is interrupted by a barcode.
98
int straightLineFollow()
99
{
100
        int colors[6];
101
        int location = 0;
102
        
103

    
104
        while(1)
105
        {        
106
                updateLine(colors);
107
                location = lineLocate(colors);         
108
                
109
                //not on line
110
                if(location == NOLINE)
111
                {
112
                        orb1_set_color(GREEN);
113
                        orb2_set_color(GREEN);
114
                        motors_off();
115
                }
116
                //turn left
117
                else if(location > 0)
118
                {
119
                        orb1_set_color(GREEN);
120
                        orb2_set_color(ORB_OFF);
121
                        motor_l_set(FORWARD, 150);
122
                        motor_r_set(FORWARD, 200);
123
                }
124
                //turn right
125
                else if(location < 0)
126
                {
127
                        orb1_set_color(ORB_OFF);
128
                        orb2_set_color(GREEN);
129
                        motor_l_set(FORWARD, 200);
130
                        motor_r_set(FORWARD, 150);
131
                }
132
                else
133
                {
134
                        orb1_set_color(ORB_OFF);
135
                        orb2_set_color(ORB_OFF);
136
                        motor_l_set(FORWARD, 200);
137
                        motor_r_set(FORWARD, 200);
138
                }
139
        }
140
        
141
        return 0;
142
}