Project

General

Profile

Revision 1191

Added by Rich Hong about 15 years ago

move spline slave to sub-directory and update encoder_get_v function

View differences:

encoders.c
19 19
int right_dx;
20 20
long int timecount;
21 21

  
22
int left_v;
23
int right_v;
24

  
25

  
26 22
volatile short int data_ready;
27 23

  
28 24
void encoder_recv(char data);
......
61 57
	left_data = -1;
62 58
	right_data = -1;
63 59
	
64
	
65
	left_v=0;
66
	right_v=0;
67
	
68 60
	//RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
69 61
	left_data_idx = 0;
70 62
	right_data_idx = 0;
......
158 150
* @return The instantaneous velocity for the given wheel.
159 151
*/
160 152
int encoder_get_v(char encoder){
161
	int last, res=0;
162
	
163
	cli();
164
	
165
	if(encoder==LEFT){
166
		if(left_data_idx==0)
167
			last = BUFFER_SIZE - 1;
168
		else
169
			last = left_data_idx - 1;
170
		res = ((int)left_data_array[last]) - ((int)left_data_array[left_data_idx]);
171
	}
172
	if(encoder==RIGHT){
173
		if(right_data_idx==0)
174
			last = BUFFER_SIZE - 1;
175
		else
176
			last = right_data_idx - 1;
177
		res = ((int)right_data_array[right_data_idx]) - ((int)right_data_array[last]);
178
	}
179
	
180
	sei();
153
	if (encoder == LEFT)
154
		return left_data_array_bottom() - left_data_array_top();
181 155

  
182
	while(res<MIN_V)//underflow
183
		res+=1024;
184
	while(res>MAX_V)//overflow
185
		res-=1024;
156
	if (encoder == RIGHT)
157
		return right_data_array_top() - right_data_array_bottom();
186 158

  
187
	return res;
159
	return -1; /* TODO: velocity could be -1, use another value for error */
188 160
}
189 161

  
190 162

  
......
253 225
			left_data_array_put(left_data);
254 226

  
255 227
			//Adjust left accumulator
256
			dx = left_data - left_data_array_prev();
228
			dx = - left_data + left_data_array_prev();
257 229
			
258
			//Adjust velocity: save last dx
259
			left_v = left_dx;
260
			
261 230
			if(left_data_array_prev()==0)  dx=0;
262 231

  
263 232
			if(dx > 512) left_dx += dx - 1023; //Underflow
264 233
			else if(dx < -512) left_dx += dx + 1023; //Overflow
265 234
			else left_dx += dx;
266
			
267
			//Adjust velocity: update
268
			left_v = left_dx - left_v;
269 235
		}
270 236

  
271 237
		/*Above 1023 is invalid data*/	

Also available in: Unified diff