Project

General

Profile

Revision 1345

Added by Rich Hong over 11 years ago

Final spline code for master/slave

updated outdated libdragonfly and libwireless

View differences:

encoders.c
54 54
	encoder_buf_index = 0;
55 55
	left_data_buf = 0;
56 56
	right_data_buf= 0;
57

  
57 58
	left_data = -1;
58 59
	right_data = -1;
59 60
	
......
140 141
	timecount = 0;
141 142
}
142 143

  
144
///////////////////////////////////////////////////////////////////////
145
int left_data_at(int index) {
146
	int tmp_idx = left_data_idx - index;
147
	if (tmp_idx < 0)
148
		tmp_idx += BUFFER_SIZE;
149
	return left_data_array[tmp_idx];
150
}
143 151

  
152
int right_data_at(int index) {
153
	int tmp_idx = right_data_idx - index;
154
	if (tmp_idx < 0)
155
		tmp_idx += BUFFER_SIZE;
156
	return right_data_array[tmp_idx];	
157
}
158

  
159

  
160
int get_dx(char encoder, int index) {
161
	int dx, ctr;
162
	ctr = 0;
163
	dx = 1024;
164
	do {
165
		if (encoder == LEFT)
166
			dx = left_data_at(index+ctr+38) - left_data_at(index+ctr);
167
		else
168
			dx = right_data_at(index+ctr) - right_data_at(index+ctr+38);
169
		ctr++;
170
	} while ((dx > 30 || dx < -30) && ctr < 3);
171
	if (dx > 30 || dx < -30)
172
		return ERR_VEL;
173
	return dx;
174
}
144 175
/** 
145 176
* @brief Returns the approximated instantaneous velocity of the robot
146 177
* in terms of encoder clicks.
......
150 181
* @return The instantaneous velocity for the given wheel.
151 182
*/
152 183
int encoder_get_v(char encoder){
184
	/*
153 185
	if (encoder == LEFT)
154 186
		return left_data_array_bottom() - left_data_array_top();
155 187

  
156 188
	if (encoder == RIGHT)
157 189
		return right_data_array_top() - right_data_array_bottom();
158 190

  
159
	return -1; /* TODO: velocity could be -1, use another value for error */
191
	return -1;
192
	*/
193
	int vel1, vel2, tmp;
194
	vel1 = get_dx(encoder, 0);
195
	vel2 = get_dx(encoder, 1);
196

  
197
	if (vel1 == ERR_VEL && vel2 == ERR_VEL)
198
		return ERR_VEL << 1;
199
	else if (vel2 == ERR_VEL)
200
		return vel1 << 1;
201
	else if (vel1 == ERR_VEL) 
202
		return vel2 << 1;
203
	else
204
		return vel1 + vel2;
160 205
}
206
/////////////////////////////////////////////////////////////////////////
161 207

  
162

  
163 208
/** 
164 209
* @brief Waits until n encoder reads have occurred.
165 210
* Counter is reset on functions exit.

Also available in: Unified diff