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
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);
......
38 34
inline unsigned int right_data_array_prev(void);
39 35
inline unsigned int right_data_array_bottom(void);
40 36

  
41
//RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int);
42 37

  
43
void encoder_recv_complete(void);
44

  
45 38
void encoder_recv_complete(){
46
   encoder_buf_index = 0;
47
   //  usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");
48
   //   usb_puts("\r\n");
49
  data_ready++;
39
  	encoder_buf_index = 0;
40
	data_ready++;
50 41
  
51
  //delay_ms(ENCODER_DELAY);
52

  
53
   spi_transfer(5);
42
   	spi_transfer(5);
54 43
}
55 44

  
56
void put_bin(char data){
57
  int i;
58

  
59
  for(i=7;i>=0;i--)
60
    usb_puti((data>>i)&1);
61
  usb_puts(" ");
62
}
63

  
64
/**
65
 * Initializes the encoders variables
66
 **/
45
/** 
46
* @brief Initializes encoder variables and the hardware interface.
47
*/
67 48
void encoders_init(void){
68 49
	int i;
69 50
	
70 51
	data_ready=0;
71 52

  
72
	spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);
53
	spi_init(encoder_recv, encoder_recv_complete);
73 54
	encoder_buf_index = 0;
74 55
	left_data_buf = 0;
75 56
	right_data_buf= 0;
76 57
	left_data = -1;
77 58
	right_data = -1;
78 59
	
79
	
80
	left_v=0;
81
	right_v=0;
82
	
83 60
	//RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
84 61
	left_data_idx = 0;
85 62
	right_data_idx = 0;
......
90 67
		right_data_array[i] = 0;
91 68
	}
92 69
	spi_transfer(5);
93
	usb_puts("\tencoders.c Debug: ENCODERS INITIALIZED\n\r");
94 70
}
95 71

  
96 72
/**
97
 * Returns the specified encoders value
73
 * @brief Returns the specified encoders value
98 74
 *
99 75
 * @param encoder this is the encoder that you want to read. Valid arguments
100 76
 *          are LEFT and RIGHT
......
102 78
 * @return the value of the specified encoder
103 79
 **/
104 80
int encoder_read(char encoder){
105
	if(encoder==LEFT){
106
		usb_puts("\tencoders.c Debug: PRINTING LEFT ENCODER VALUE\n\r");
107
		return left_data;
108
	}
109
	else if(encoder==RIGHT){
110
		usb_puts("\tencoders.c Debug: PRINTING RIGHT ENCODER VALUE\n\r");
111
		return right_data;
112
	}
113
	else{
114
		usb_puts("\tencoders.c Debug: INVALID ENCODER - USE LEFT/RIGHT\n\r");
115
		 return -1;
116
	}
81
	
82
	if(encoder==LEFT) return left_data;
83
	else if(encoder==RIGHT) return right_data;
84
	else return -1;
117 85
}
118 86

  
119
int encoder_change(char encoder){
120
	return 0;
121
}
122 87

  
88
/** 
89
* @brief Outputs encoder direction as FORWARD OR BACK
90
* A STUB! DO NOT use.
91
* 
92
* @param encoder The encoder you want the direction of.
93
* Valid arguments are right and left.
94
* 
95
* @return FORWARD or BACK (the constants)
96
*/
123 97
char encoder_direction(char encoder){
124 98
	return 0;
125 99
}
100

  
126 101
/**
127
 * Gets the velocity of the specified encoder.
102
 * Gets the total distance covered by the specified encoder (in encoder clicks)
128 103
 *
129 104
 * @param encoder the encoder that you want to read, use LEFT or RIGHT
130 105
 *
131
 * @return The velocity of the specified encoder.
106
 * @return The distance covered by the specified encoder.
132 107
 **/
133 108
int encoder_get_dx(char encoder) {
134
	if(encoder==LEFT)
135
		return left_dx;
136
	else if(encoder==RIGHT)
137
		return right_dx;
109
	
110
	if(encoder==LEFT) return left_dx;
111
	else if(encoder==RIGHT) return right_dx;
138 112
	else return -1;
139 113
}
140 114

  
141 115
/**
142
 * Resets the value of the velocity global variable for the specified
116
 * Resets the distance accumulator for the specified
143 117
 *  encoder.
144 118
 *
145
 * @param encoder the encoder that you want to modify
119
 * @param encoder the encoder that you want to reset distance for
146 120
 **/
147 121
void encoder_rst_dx(char encoder) {
148
	if(encoder==LEFT)
149
		left_dx = 0;
150
	else if(encoder==RIGHT)
151
		right_dx = 0;
122
	
123
	if(encoder==LEFT) left_dx = 0;
124
	else if(encoder==RIGHT) right_dx = 0;
152 125
}
153 126

  
154
/**
155
 * Returns the current time count for the encoders.
156
 **/
127
/** 
128
* @brief Returns the number of encoder reads that have occurred.
129
* 
130
* @return The time count.
131
*/
157 132
int encoder_get_tc(void) {
158 133
	return timecount;
159 134
}
160 135

  
161
/**
162
 * Resets the time count for the encoders.
163
 **/
136
/** 
137
* @brief Resets the encoder read counter.
138
*/
164 139
void encoder_rst_tc(void) {
165 140
	timecount = 0;
166 141
}
167 142

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

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

  
158

  
159
int get_dx(char encoder, int index) {
160
	int dx, ctr;
161
	ctr = 0;
162
	dx = 1024;
163
	do {
164
		if (encoder == LEFT)
165
			dx = left_data_at(index+ctr+38) - left_data_at(index+ctr);
166
		else
167
			dx = right_data_at(index+ctr) - right_data_at(index+ctr+38);
168
		ctr++;
169
	} while ((dx > 30 || dx < -30) && ctr < 3);
170
	if (dx > 30 || dx < -30)
171
		return ERR_VEL;
172
	return dx;
173
}
174
/** 
175
* @brief Returns the approximated instantaneous velocity of the robot
176
* in terms of encoder clicks.
177
* 
178
* @param encoder RIGHT or LEFT - the wheel you want the velocity for.
179
* 
180
* @return The instantaneous velocity for the given wheel.
181
*/
169 182
int encoder_get_v(char encoder){
170
	int last, res=0;
171
	
172
	cli();
173
	
174
	if(encoder==LEFT){
175
		if(left_data_idx==0)
176
			last = BUFFER_SIZE - 1;
177
		else
178
			last = left_data_idx - 1;
179
		res = ((int)left_data_array[last]) - ((int)left_data_array[left_data_idx]);
180
	}
181
	if(encoder==RIGHT){
182
		if(right_data_idx==0)
183
			last = BUFFER_SIZE - 1;
184
		else
185
			last = right_data_idx - 1;
186
		res = ((int)right_data_array[right_data_idx]) - ((int)right_data_array[last]);
187
	}
188
	
189
	sei();
183
	/*
184
	if (encoder == LEFT)
185
		return left_data_array_bottom() - left_data_array_top();
190 186

  
191
	while(res<MIN_V)//underflow
192
		res+=1024;
193
	while(res>MAX_V)//overflow
194
		res-=1024;
187
	if (encoder == RIGHT)
188
		return right_data_array_top() - right_data_array_bottom();
195 189

  
196
	return res;
190
	return -1;
191
	*/
192
	int vel1, vel2, tmp;
193
	vel1 = get_dx(encoder, 0);
194
	vel2 = get_dx(encoder, 1);
195

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

  
207
/** 
208
* @brief Waits until n encoder reads have occurred.
209
* Counter is reset on functions exit.
210
* 
211
* @param n 
212
*/
199 213
void encoder_wait(int n){
200 214
	while(data_ready<n);
201 215
	data_ready=0;
......
228 242
	encoder_buf_index = (encoder_buf_index + 1) % 5;
229 243

  
230 244
	if(encoder_buf_index==0) {
231
		  
232
		if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF))  left_data = INVALID;
233
		else if(((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)) left_data = MAGNET_FAILURE;
245
		
246
		/*Error handling for the left encoder*/ 
247
		if(!(left_data_buf & OCF)) 
248
			left_data = ENCODER_DATA_NOT_READY; 
249
		if(left_data_buf & (COF | LIN))  
250
			left_data = ENCODER_MISALIGNED;
251
		else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 
252
			left_data = ENCODER_MAGNET_FAILURE;
234 253
		else left_data = (left_data_buf>>5) & 1023;
235 254
			
236
		if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)) right_data = INVALID;
237
		else if ( ((right_data_buf & MagINCn) > 0)  && ((right_data_buf & MagDECn) > 0)) right_data = MAGNET_FAILURE;
255
		/*Error handling for the right encoder*/ 
256
	 	if(!(right_data_buf & OCF))
257
			right_data = ENCODER_DATA_NOT_READY;	
258
		if(right_data_buf & (COF | LIN)) 
259
			right_data = ENCODER_MISALIGNED;
260
		else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 
261
			right_data = ENCODER_MAGNET_FAILURE;
238 262
		else right_data = (right_data_buf>>5) & 1023;
239 263
			
240 264
  		left_data_buf = 0;
241 265
		right_data_buf = 0;
242 266

  
243
		
244
		if(left_data != INVALID && left_data != MAGNET_FAILURE) {
267
		/*Above 1023 is invalid data*/	
268
		if(!(left_data > 1023)) {
245 269
			left_data_array_put(left_data);
246 270

  
247 271
			//Adjust left accumulator
248
			dx = left_data_array_prev() - left_data;
272
			dx = - left_data + left_data_array_prev();
249 273
			
250
			//Adjust velocity: save last dx
251
			left_v = left_dx;
252

  
253 274
			if(left_data_array_prev()==0)  dx=0;
254 275

  
255 276
			if(dx > 512) left_dx += dx - 1023; //Underflow
256 277
			else if(dx < -512) left_dx += dx + 1023; //Overflow
257 278
			else left_dx += dx;
258
			
259
			//Adjust velocity: update
260
			left_v = left_dx - left_v;
261 279
		}
262 280

  
263
		if(right_data != INVALID && right_data != MAGNET_FAILURE) {
281
		/*Above 1023 is invalid data*/	
282
		if(!(right_data > 1023)) {
264 283
			right_data_array_put(right_data);
265 284

  
266 285
			//Adjust right accumulator
......
279 298
}
280 299

  
281 300

  
282
void encoders_print_data_array(void){
283
	int i;
284
	usb_puts("Left Data: Pointer is ");
285
	usb_puti(left_data_idx);
286
	usb_puts("\n\r");
287
	for(i=0; i<BUFFER_SIZE; i++){
288
		usb_puti(left_data_array[i]);
289
		usb_putc(' ');
290
	}
291
	usb_puts("\r\n");
292
	usb_puts("Right Data: Pointer is");
293
	usb_puti(right_data_idx);
294
	usb_puts("\n\r");
295
	for(i=0; i<BUFFER_SIZE; i++){
296
		usb_puti(right_data_array[i]);
297
		usb_putc(' ');
298
	}
299
	usb_puts("\r\n\r\n");
300
}
301

  
302

  
303 301
//Helper Functions
304 302
inline void left_data_array_put(unsigned short int value) {
305 303
	if(left_data_idx == BUFFER_SIZE-1)

Also available in: Unified diff