Revision 1854
lineFollow now supports merge. line drive is UNTESTED and has NO COMMENTS, but will provide higher level driving control
| lineFollow.c | ||
|---|---|---|
| 1 | 1 |
#include "lineFollow.h" |
| 2 | 2 |
|
| 3 |
#define NOBARCODE -2 |
|
| 4 | 3 |
#define CODESIZE 5 |
| 5 |
#define INTERSECTION -25 |
|
| 6 |
#define FULL_LINE -26 |
|
| 7 | 4 |
|
| 8 | 5 |
int countHi = 0; |
| 9 | 6 |
int countLo = 0; |
| ... | ... | |
| 75 | 72 |
} |
| 76 | 73 |
|
| 77 | 74 |
|
| 78 |
int turnLeft()
|
|
| 75 |
int mergeLeft()
|
|
| 79 | 76 |
{
|
| 80 |
motor_l_set(BACKWARD, 200); |
|
| 81 |
motor_r_set(FORWARD, 200); |
|
| 77 |
motor_l_set(FORWARD, 200); |
|
| 78 |
if(turnDistance!=21)motor_r_set(FORWARD, 230); |
|
| 79 |
else motor_r_set(FORWARD, 210); |
|
| 82 | 80 |
int colors[5]; |
| 83 | 81 |
updateLine(colors); |
| 84 | 82 |
int position = lineLocate(colors); |
| 85 |
if(position>2 || position<-2)turnDistance++;
|
|
| 83 |
if(position>3 || position<-3)turnDistance++;
|
|
| 86 | 84 |
|
| 87 |
if(turnDistance>1)
|
|
| 85 |
if(turnDistance>20)
|
|
| 88 | 86 |
{
|
| 87 |
turnDistance=21; |
|
| 88 |
|
|
| 89 | 89 |
if(position<3 && position>-3) |
| 90 | 90 |
{
|
| 91 | 91 |
turnDistance = 0; |
| ... | ... | |
| 95 | 95 |
return 1; |
| 96 | 96 |
} |
| 97 | 97 |
|
| 98 |
|
|
| 99 |
int mergeRight() |
|
| 100 |
{
|
|
| 101 |
motor_r_set(FORWARD, 200); |
|
| 102 |
if(turnDistance!=21)motor_l_set(FORWARD, 230); |
|
| 103 |
else motor_l_set(FORWARD, 210); |
|
| 104 |
int colors[5]; |
|
| 105 |
updateLine(colors); |
|
| 106 |
int position = lineLocate(colors); |
|
| 107 |
if(position>3 || position<-3)turnDistance++; |
|
| 108 |
|
|
| 109 |
if(turnDistance>20) |
|
| 110 |
{
|
|
| 111 |
turnDistance=21; |
|
| 112 |
|
|
| 113 |
if(position<3 && position>-3) |
|
| 114 |
{
|
|
| 115 |
turnDistance = 0; |
|
| 116 |
return 0; |
|
| 117 |
} |
|
| 118 |
} |
|
| 119 |
return 1; |
|
| 120 |
} |
|
| 121 |
|
|
| 122 |
|
|
| 123 |
|
|
| 124 |
int turnLeft() |
|
| 125 |
{
|
|
| 126 |
motor_l_set(BACKWARD, 200); |
|
| 127 |
motor_r_set(FORWARD, 200); |
|
| 128 |
int colors[5]; |
|
| 129 |
updateLine(colors); |
|
| 130 |
int position = lineLocate(colors); |
|
| 131 |
if(position>2 || position<-2)turnDistance++; |
|
| 132 |
if(turnDistance>1) |
|
| 133 |
{
|
|
| 134 |
if(position<3 && position>-3) |
|
| 135 |
{
|
|
| 136 |
turnDistance = 0; |
|
| 137 |
return 0; |
|
| 138 |
} |
|
| 139 |
} |
|
| 140 |
return 1; |
|
| 141 |
} |
|
| 142 |
|
|
| 143 |
|
|
| 144 |
|
|
| 98 | 145 |
int turnRight() |
| 99 | 146 |
{
|
| 100 | 147 |
motor_r_set(BACKWARD, 200); |
| ... | ... | |
| 134 | 181 |
{
|
| 135 | 182 |
int ports[5] = {13, 12, 3, 2, 9};
|
| 136 | 183 |
for(int i = 0; i<5; i++) |
| 137 |
values[i] = analog_get10(ports[i])<150 ? WHITE : BLACK;
|
|
| 184 |
values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
|
|
| 138 | 185 |
} |
| 139 | 186 |
|
| 140 | 187 |
|
| ... | ... | |
| 160 | 207 |
|
| 161 | 208 |
void updateBarCode() |
| 162 | 209 |
{
|
| 210 |
|
|
| 211 |
//NOTE: currently only uses one of the barcode sensors. |
|
| 212 |
|
|
| 163 | 213 |
//maps the sensors to the analog input ports |
| 164 | 214 |
int ports[2] = {8,1};
|
| 165 | 215 |
int current[2]; |
| 166 |
// current[0] = analog_get10(8); |
|
| 167 |
current[1] = analog_get10(1); |
|
| 168 |
// usb_puti(analog_get10(8)); |
|
| 169 |
// usb_putc('\t');
|
|
| 170 |
// usb_puti(analog_get10(1)); |
|
| 171 |
// usb_putc('\n');
|
|
| 216 |
// current[0] = analog_get10(ports[0]); |
|
| 217 |
current[1] = analog_get10(ports[1]); |
|
| 172 | 218 |
|
| 173 | 219 |
if(current[1]>500) |
| 174 | 220 |
{
|
Also available in: Unified diff