Revision 1845 trunk/code/projects/linefollowing/lineFollow.c

View differences:

lineFollow.c
1 1
#include "lineFollow.h"
2 2

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

  
6 6
int countHi = 0;
7 7
int countLo = 0;
......
41 41
	{
42 42
		position*=30;
43 43
		orb2_set_color(ORB_OFF);
44
		motorLeft(min(SPEED+position, 255));
45
		motorRight(min(SPEED-position, 255));
44
		motorLeft(min(SPEED+position, 150));
45
		motorRight(min(SPEED-position, 150));
46 46
		lost=0;
47 47
	}
48 48
	updateBarCode();
......
95 95

  
96 96
	for(i = 0; i<5; i++)
97 97
	{
98
		count+=(colors[i]+1)/2;
98
		count += colors[i]/2;
99 99
		wsum += (i)*colors[i];
100 100
	}
101 101
	if(count==0)
......
114 114
	current[1] = analog_get10(1);
115 115
//	usb_puti(analog_get10(8));
116 116
//	usb_putc('\t');
117
	usb_puti(analog_get10(1));
118
	usb_putc('\n');
117
//	usb_puti(analog_get10(1));
118
//	usb_putc('\n');
119 119

  
120 120
	if(current[1]>500)
121 121
	{
......
131 131
			maxAvg = max(maxAvg, avg);
132 132
		}
133 133
	}
134
	else if(countHi>10)
134
	else if(countHi>5)
135 135
	{
136 136
		if(countLo++>15)
137 137
		{
138 138
			countHi=countLo=0;
139
		//	if(maxAvg>825)orb1_set_color(RED);
140
		//	else orb1_set_color(BLUE);
139
			if(maxAvg>825)orb1_set_color(RED);
140
			else orb1_set_color(BLUE);
141 141
			barCode[barCodePosition++] = (maxAvg > 825 ? 2:1)-1;
142 142
		}
143 143
	}
......
150 150
int max(int x, int y){return x<y ? y : x;}
151 151

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

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

  

Also available in: Unified diff