root / trunk / code / projects / linefollowing / main.c @ 1838
History | View | Annotate | Download (2.43 KB)
1 | 16 | bcoltin | #include <dragonfly_lib.h> |
---|---|---|---|
2 | |||
3 | 1836 | dgurjar | #define WHITE 0 |
4 | #define GREY 1 |
||
5 | #define BLACK 2 |
||
6 | 1838 | dgurjar | #define COLOR_BUFFER 10 |
7 | 1836 | dgurjar | #define CENTER 3 |
8 | 1838 | dgurjar | #define NOLINE -42 |
9 | 1836 | dgurjar | |
10 | 1838 | dgurjar | 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 | 857 | bcoltin | int main(void) |
19 | { |
||
20 | 1419 | jsexton | |
21 | 967 | alevkoy | /* initialize components, set wireless channel */
|
22 | 857 | bcoltin | dragonfly_init(ALL_ON); |
23 | 1419 | jsexton | |
24 | 1836 | dgurjar | straightLineFollow(); |
25 | 967 | alevkoy | |
26 | 1836 | dgurjar | return 0; |
27 | } |
||
28 | 967 | alevkoy | |
29 | 1838 | dgurjar | |
30 | |||
31 | 1836 | dgurjar | //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 | 1838 | dgurjar | int assignColor(int port, int analog) |
34 | 1836 | dgurjar | { |
35 | int color;
|
||
36 | |||
37 | if(analog < 341-COLOR_BUFFER) |
||
38 | color = WHITE; |
||
39 | else if (analog < 341+COLOR_BUFFER) |
||
40 | 1838 | dgurjar | color = assignColor(port, updateIR(port)); |
41 | 1836 | dgurjar | else if (analog < 682-COLOR_BUFFER) |
42 | color = GREY; |
||
43 | else if (analog < 682+COLOR_BUFFER) |
||
44 | 1838 | dgurjar | color = assignColor(port, updateIR(port)); |
45 | 1836 | dgurjar | else
|
46 | color = BLACK; |
||
47 | |||
48 | return color;
|
||
49 | 1419 | jsexton | |
50 | 1836 | dgurjar | } |
51 | 1419 | jsexton | |
52 | 1836 | dgurjar | //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 | 967 | alevkoy | |
58 | 1836 | dgurjar | return analog10(ports[n]);
|
59 | 16 | bcoltin | } |
60 | 773 | kwoo | |
61 | 1836 | dgurjar | //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 | 1838 | dgurjar | return analog10(ports[1]) | analog10(ports[2]); |
68 | 1836 | dgurjar | } |
69 | |||
70 | // updates all of the values, and assigns them new colors
|
||
71 | 1838 | dgurjar | void updateLine(int* values) |
72 | 1836 | dgurjar | { |
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 | 1838 | dgurjar | int lineLocate(int* colors) |
81 | 1836 | dgurjar | { |
82 | int i;
|
||
83 | int wsum = 0; |
||
84 | 1838 | dgurjar | int count=0; |
85 | 1836 | dgurjar | |
86 | for(i = 0; i<5; i++) |
||
87 | 1838 | dgurjar | { |
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 | 1836 | dgurjar | } |
96 | |||
97 | //The robot will follow a line until it is interrupted by a barcode.
|
||
98 | int straightLineFollow()
|
||
99 | { |
||
100 | 1838 | dgurjar | int colors[6]; |
101 | 1836 | dgurjar | int location = 0; |
102 | |||
103 | |||
104 | while(1) |
||
105 | { |
||
106 | 1838 | dgurjar | updateLine(colors); |
107 | 1836 | dgurjar | location = lineLocate(colors); |
108 | |||
109 | 1838 | dgurjar | //not on line
|
110 | if(location == NOLINE)
|
||
111 | { |
||
112 | orb1_set_color(GREEN); |
||
113 | orb2_set_color(GREEN); |
||
114 | } |
||
115 | 1836 | dgurjar | //turn left
|
116 | 1838 | dgurjar | else if(location > 0) |
117 | 1836 | dgurjar | { |
118 | orb1_set_color(GREEN); |
||
119 | orb2_set_color(ORB_OFF); |
||
120 | } |
||
121 | //turn right
|
||
122 | 1838 | dgurjar | else if(location < 0) |
123 | 1836 | dgurjar | { |
124 | orb1_set_color(ORB_OFF); |
||
125 | orb2_set_color(GREEN); |
||
126 | } |
||
127 | 1838 | dgurjar | else
|
128 | { |
||
129 | orb1_set_color(ORB_OFF); |
||
130 | orb2_set_color(ORB_OFF); |
||
131 | } |
||
132 | 1836 | dgurjar | } |
133 | 1838 | dgurjar | |
134 | return 0; |
||
135 | 1836 | dgurjar | } |