Revision 1838 trunk/code/projects/linefollowing/main.c

View differences:

main.c
3 3
#define WHITE			0
4 4
#define GREY			1
5 5
#define BLACK	 		2
6
#define COLOR_BUFFER		10
6
#define COLOR_BUFFER	10
7 7
#define CENTER			3
8
#define NOLINE			-42
8 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

  
9 18
int main(void)
10 19
{
11 20

  
......
17 26
	return 0;
18 27
}
19 28

  
29

  
30

  
20 31
//Converts an analog color to the ternary black, grey, white system.
21 32
//If the value is between two codes, the port is read again and the function is called again
22
int asssignColor(int analog)
33
int assignColor(int port, int analog)
23 34
{
24 35
	int color;
25 36
	
26 37
		if(analog < 341-COLOR_BUFFER)
27 38
			color = WHITE;
28 39
		else if (analog < 341+COLOR_BUFFER)
29
			color = assignColor(updateIR(port));
40
			color = assignColor(port, updateIR(port));
30 41
		else if (analog < 682-COLOR_BUFFER)
31 42
			color = GREY; 
32 43
		else if (analog < 682+COLOR_BUFFER)
33
			color = assignColor(updateIR(port));
44
			color = assignColor(port, updateIR(port));
34 45
		else
35 46
			color = BLACK;
36 47
			
......
53 64
	//maps the sensors to the analog input ports
54 65
	int ports[2] = {8,1};
55 66
	
56
	return analog10(ports[1]) | analog10(ports[2];
67
	return analog10(ports[1]) | analog10(ports[2]);
57 68
}
58 69

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

  
64 75
	for(i = 0; i<5; i++)
65 76
		values[i] = assignColor(i, updateIR(i)); 
66
		
67
	return values;
68 77
}
69 78

  
70
// updates all of the values, and assigns them new colors
71
int* updateLine(int* values)
72
{	
73
	int i;
74
	
75
	values[i] = assignColor(i, updateIR(i)); 
76
	
77
	return values;
78
}
79

  
80

  
81 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.
82
int lineLocate(int[] colors)
80
int lineLocate(int* colors)
83 81
{
84 82
	int i;
85 83
	int wsum = 0;
84
	int count=0;
86 85

  
87 86
	for(i = 0; i<5; i++)
88
		wsum += (i+1)*colors[i];
89

  
90
	return (wsum/15) -1;
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;
91 95
}
92 96

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

  
100 104
	while(1)
101 105
	{	
102
		colors  = updateLine(colors);
106
		updateLine(colors);
103 107
		location = lineLocate(colors); 	
104 108
		
109
		//not on line
110
		if(location == NOLINE)
111
		{
112
			orb1_set_color(GREEN);
113
			orb2_set_color(GREEN);
114
		}
105 115
		//turn left
106
		if(location > 3)
116
		else if(location > 0)
107 117
		{
108 118
			orb1_set_color(GREEN);
109 119
			orb2_set_color(ORB_OFF);
110 120
		}
111 121
		//turn right
112
		else if(location < 3)
122
		else if(location < 0)
113 123
		{
114 124
			orb1_set_color(ORB_OFF);
115 125
			orb2_set_color(GREEN);
116 126
		}
127
		else
128
		{
129
			orb1_set_color(ORB_OFF);
130
			orb2_set_color(ORB_OFF);
131
		}
117 132
	}
133
	
134
	return 0;
118 135
}

Also available in: Unified diff