Revision 1851
Added turning
| lineFollow.c | ||
|---|---|---|
| 2 | 2 |
|
| 3 | 3 |
#define NOBARCODE -2 |
| 4 | 4 |
#define CODESIZE 5 |
| 5 |
#define INTERSECTION -25 |
|
| 6 |
#define FULL_LINE -26 |
|
| 5 | 7 |
|
| 6 | 8 |
int countHi = 0; |
| 7 | 9 |
int countLo = 0; |
| ... | ... | |
| 9 | 11 |
int barCode[ CODESIZE ]; |
| 10 | 12 |
int barCodePosition=0; |
| 11 | 13 |
|
| 14 |
int turnDistance=0; |
|
| 15 |
int intersection=0; |
|
| 16 |
int disableBarCode=0; |
|
| 17 |
|
|
| 18 |
|
|
| 12 | 19 |
void lineFollow_init() |
| 13 | 20 |
{
|
| 14 | 21 |
analog_init(0); |
| 15 | 22 |
lost = 0; |
| 23 |
intersection=0; |
|
| 24 |
disableBarCode=0; |
|
| 16 | 25 |
} |
| 17 | 26 |
|
| 18 | 27 |
|
| 19 | 28 |
|
| 20 |
int lineFollow() |
|
| 29 |
int lineFollow(int speed)
|
|
| 21 | 30 |
{
|
| 22 |
int colors[6];
|
|
| 31 |
int colors[5];
|
|
| 23 | 32 |
int position; |
| 24 | 33 |
|
| 25 | 34 |
|
| ... | ... | |
| 36 | 45 |
return LINELOST; |
| 37 | 46 |
} |
| 38 | 47 |
} |
| 48 |
else if(position == FULL_LINE) |
|
| 49 |
{
|
|
| 50 |
if(intersection++>4) |
|
| 51 |
{
|
|
| 52 |
orb2_set_color(RED); |
|
| 53 |
barCodePosition=0; |
|
| 54 |
disableBarCode=15; |
|
| 55 |
return INTERSECTION; |
|
| 56 |
} |
|
| 57 |
} |
|
| 39 | 58 |
//on line |
| 40 | 59 |
else |
| 41 | 60 |
{
|
| 42 | 61 |
position*=30; |
| 43 | 62 |
orb2_set_color(ORB_OFF); |
| 44 |
motorLeft(min(SPEED+position, 150));
|
|
| 45 |
motorRight(min(SPEED-position, 150));
|
|
| 63 |
motorLeft(min(speed+position, 255));
|
|
| 64 |
motorRight(min(speed-position, 255));
|
|
| 46 | 65 |
lost=0; |
| 66 |
intersection=0; |
|
| 47 | 67 |
} |
| 68 |
|
|
| 69 |
if(disableBarCode) |
|
| 70 |
{
|
|
| 71 |
disableBarCode--; |
|
| 72 |
return NOBARCODE; |
|
| 73 |
} |
|
| 48 | 74 |
updateBarCode(); |
| 49 | 75 |
return getBarCode(); |
| 50 | 76 |
} |
| 51 | 77 |
|
| 52 |
int getBarCode(){
|
|
| 78 |
|
|
| 79 |
int turnLeft() |
|
| 80 |
{
|
|
| 81 |
motor_l_set(BACKWARD, 200); |
|
| 82 |
motor_r_set(FORWARD, 200); |
|
| 83 |
int colors[5]; |
|
| 84 |
updateLine(colors); |
|
| 85 |
int position = lineLocate(colors); |
|
| 86 |
if(position==NOLINE)turnDistance++; |
|
| 87 |
else if(turnDistance>10) return 0;//turnDistance = 0; |
|
| 88 |
return 1; |
|
| 89 |
} |
|
| 90 |
|
|
| 91 |
int turnRight() |
|
| 92 |
{
|
|
| 93 |
motor_r_set(BACKWARD, 200); |
|
| 94 |
motor_l_set(FORWARD, 200); |
|
| 95 |
int colors[5]; |
|
| 96 |
updateLine(colors); |
|
| 97 |
int position = lineLocate(colors); |
|
| 98 |
if(position==NOLINE)turnDistance++; |
|
| 99 |
else if(turnDistance>10) return 0;//turnDistance = 0; |
|
| 100 |
return 1; |
|
| 101 |
} |
|
| 102 |
|
|
| 103 |
|
|
| 104 |
|
|
| 105 |
|
|
| 106 |
int getBarCode() |
|
| 107 |
{
|
|
| 53 | 108 |
if(barCodePosition!=CODESIZE) return NOBARCODE ; |
| 54 | 109 |
int temp = 0; |
| 55 | 110 |
int i; |
| ... | ... | |
| 63 | 118 |
|
| 64 | 119 |
void updateLine(int* values) |
| 65 | 120 |
{
|
| 121 |
int ports[5] = {13, 12, 3, 2, 9};
|
|
| 66 | 122 |
for(int i = 0; i<5; i++) |
| 67 |
values[i] = assignColor(i, updateIR(i));
|
|
| 123 |
values[i] = analog_get10(ports[i])<150 ? WHITE : BLACK;
|
|
| 68 | 124 |
} |
| 69 | 125 |
|
| 70 | 126 |
|
| 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 | 127 |
|
| 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 | 128 |
int lineLocate(int* colors) |
| 91 | 129 |
{
|
| 92 | 130 |
int i; |
| ... | ... | |
| 99 | 137 |
wsum += (i)*colors[i]; |
| 100 | 138 |
} |
| 101 | 139 |
if(count==0) |
| 102 |
return NOLINE; |
|
| 103 |
|
|
| 140 |
return NOLINE; |
|
| 141 |
if(count==5) |
|
| 142 |
return FULL_LINE; |
|
| 104 | 143 |
return (wsum/count)-4; |
| 105 | 144 |
} |
| 106 | 145 |
|
| ... | ... | |
| 138 | 177 |
countHi=countLo=0; |
| 139 | 178 |
if(maxAvg>825)orb1_set_color(RED); |
| 140 | 179 |
else orb1_set_color(BLUE); |
| 141 |
barCode[barCodePosition++] = (maxAvg > 825 ? 2:1)-1;
|
|
| 180 |
barCode[barCodePosition++] = maxAvg > 825 ? 1:0;
|
|
| 142 | 181 |
} |
| 143 | 182 |
} |
| 144 | 183 |
else countHi/=2; |
Also available in: Unified diff