Statistics
| Revision:

root / trunk / code / projects / linefollowing / lineFollow.c @ 1845

History | View | Annotate | Download (2.4 KB)

1
#include "lineFollow.h"
2

    
3
#define NOBARCODE -2
4
#define CODESIZE 5 
5

    
6
int countHi = 0;
7
int countLo = 0;
8
int maxAvg, avg;
9
int barCode[ CODESIZE ];
10
int barCodePosition=0;
11

    
12
void lineFollow_init()
13
{
14
        analog_init(0);
15
        lost = 0;
16
}
17

    
18

    
19

    
20
int lineFollow()
21
{
22
        int colors[6];
23
        int position;
24
        
25

    
26
        updateLine(colors);
27
        position = lineLocate(colors);         
28
        
29
        //not on line
30
        if(position == NOLINE)
31
        {
32
                if(lost++>20)
33
                {
34
                        orb2_set_color(GREEN);
35
                        motors_off();
36
                        return LINELOST;
37
                }
38
        }
39
        //on line
40
        else
41
        {
42
                position*=30;
43
                orb2_set_color(ORB_OFF);
44
                motorLeft(min(SPEED+position, 150));
45
                motorRight(min(SPEED-position, 150));
46
                lost=0;
47
        }
48
        updateBarCode();
49
        return getBarCode();
50
}
51

    
52
int getBarCode(){
53
        if(barCodePosition!=CODESIZE) return NOBARCODE ;
54
        int temp = 0;
55
        int i;
56
        for(i=0; i<CODESIZE; i++)
57
                temp += (barCode[i] << i);
58
        barCodePosition = 0;
59
        return temp;
60
}
61

    
62

    
63

    
64
void updateLine(int* values)
65
{        
66
        for(int i = 0; i<5; i++)
67
                values[i] = assignColor(i, updateIR(i));
68
}
69

    
70

    
71
int updateIR(int n)
72
{
73
        //maps the sensors to the analog input ports
74
        int ports[5] = {13, 12, 3, 2, 9};
75

    
76
        return analog_get10(ports[n]);
77
}
78

    
79

    
80

    
81
int assignColor(int port, int analog)
82
{
83
        if (analog < 150)
84
                return  WHITE;
85
        else
86
                return  BLACK;
87
}
88

    
89

    
90
int lineLocate(int* colors)
91
{
92
        int i;
93
        int wsum = 0;
94
        int count=0;
95

    
96
        for(i = 0; i<5; i++)
97
        {
98
                count += colors[i]/2;
99
                wsum += (i)*colors[i];
100
        }
101
        if(count==0)
102
                        return NOLINE;        
103
                
104
        return (wsum/count)-4;
105
}
106

    
107

    
108
void updateBarCode()
109
{
110
        //maps the sensors to the analog input ports
111
        int ports[2] = {8,1};
112
        int current[2];
113
//        current[0] = analog_get10(8);
114
        current[1] = analog_get10(1);
115
//        usb_puti(analog_get10(8));
116
//        usb_putc('\t');
117
//        usb_puti(analog_get10(1));
118
//        usb_putc('\n');
119

    
120
        if(current[1]>500)
121
        {
122
                if(countHi++==0) 
123
                {
124
                        avg = 500;
125
                        maxAvg = 500;
126
                }
127
                else
128
                {
129
                        avg = 3*avg + current[1];
130
                        avg/=4;
131
                        maxAvg = max(maxAvg, avg);
132
                }
133
        }
134
        else if(countHi>5)
135
        {
136
                if(countLo++>15)
137
                {
138
                        countHi=countLo=0;
139
                        if(maxAvg>825)orb1_set_color(RED);
140
                        else orb1_set_color(BLUE);
141
                        barCode[barCodePosition++] = (maxAvg > 825 ? 2:1)-1;
142
                }
143
        }
144
        else countHi/=2;
145
        if(countHi==0)countLo=0; 
146
}
147

    
148

    
149
int min(int x, int y){return x>y ? y : x;}
150
int max(int x, int y){return x<y ? y : x;}
151

    
152
void motorLeft(int speed){
153
        ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127);
154
}
155

    
156
void motorRight(int speed){
157
        ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127);
158
}
159