Project

General

Profile

Revision 771

Added by Brad Neuman over 13 years ago

control law for speed starting to work.
WARNING: can't use delay_ms with encoders_init

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
volatile int recv_count;
26

  
27
volatile short int data_ready;
28

  
22 29
void encoder_recv(char data);
23 30

  
24 31
//Helper Function Prototypes
......
37 44
void encoder_recv_complete(void);
38 45

  
39 46
void encoder_recv_complete(){
40
  //    usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");
41
  //    usb_puts("\r\n");
42
	spi_transfer(5);
47

  
48
int dx;
49

  
50
   //  usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");
51
   //   usb_puts("\r\n");
52
  data_ready++;
53
  
54
  delay_ms(ENCODER_DELAY);
55

  
56
   spi_transfer(5);
43 57
}
44 58

  
45 59
void put_bin(char data){
......
52 66

  
53 67
void encoders_init(void){
54 68
	int i;
69
	
70
	data_ready=0;
55 71

  
56 72
	spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);
57 73
	buf_index = 0;
......
59 75
	right_data_buf= 0;
60 76
	left_data = -1;
61 77
	right_data = -1;
78
	
79
	recv_count=0;
80
	
81
	left_v=0;
82
	right_v=0;
83
	
62 84
	//RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
63 85
	left_data_idx = 0;
64 86
	right_data_idx = 0;
......
111 133
}
112 134

  
113 135

  
136
int encoder_get_v(char encoder){
137
	int last, res=0;
138
	
139
	cli();
140
	
141
	if(encoder==LEFT){
142
		if(left_data_idx==0)
143
			last = BUFFER_SIZE - 1;
144
		else
145
			last = left_data_idx - 1;
146
		res = ((int)left_data_array[last]) - ((int)left_data_array[left_data_idx]);
147
	}
148
	if(encoder==RIGHT){
149
		if(right_data_idx==0)
150
			last = BUFFER_SIZE - 1;
151
		else
152
			last = right_data_idx - 1;
153
		res = ((int)right_data_array[right_data_idx]) - ((int)right_data_array[last]);
154
	}
155
	
156
	sei();
157

  
158
	while(res<MIN_V)//underflow
159
		res+=1024;
160
	while(res>MAX_V)//overflow
161
		res-=1024;
162

  
163
	return res;
164
}
165

  
166
void encoder_wait(int n){
167
	while(data_ready<n);
168
	data_ready=0;
169
}
170

  
171

  
114 172
//Full reads occur every 40 microseconds. This function should be called
115 173
//every 8 microseconds.
116 174
void encoder_recv(char data){
117 175
	//usb_puti(buf_index);
118 176
	short int dx;
119 177
	
120
	if(buf_index == 0)
121
	  right_data_buf |= ((short)data)<<8 & 0xff00;
178
	recv_count++;
179
	//if(++recv_count==ENCODER_DELAY){
180
			
181
		recv_count=0;
182
		
183
		if(buf_index == 0)
184
		  right_data_buf |= ((short)data)<<8 & 0xff00;
122 185

  
123
	else if (buf_index == 1)
124
	  right_data_buf |= ((short)data) & 0xff;
125
	
126
	else if (buf_index == 2)
127
		left_data_buf |= (((short)data) << 9) & (0x7F << 9);
128
	
129
	else if (buf_index == 3)
130
		left_data_buf |= (((short)data) << 1) & (0xFF<<1);
131
	
132
	else if (buf_index == 4)
133
		left_data_buf |= (((short)data)>>7) & 0x1;
134

  
135
	
136
	buf_index = (buf_index + 1) % 5;
137

  
138
	if(buf_index==0) {
139
	  if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF)){
140
			left_data = INVALID;
141
	  }
186
		else if (buf_index == 1)
187
		  right_data_buf |= ((short)data) & 0xff;
142 188
		
143
	  else if(((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)){
144
	    left_data = MAGNET_FAILURE;
145
	  }
146
	  else{
147
	    left_data = (left_data_buf>>5) & 1023;
148
	  }
189
		else if (buf_index == 2)
190
			left_data_buf |= (((short)data) << 9) & (0x7F << 9);
149 191
		
150
	  if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)){
151
	    right_data = INVALID;
152
	  }
153
	  else if ( ((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)){
154
	    left_data = MAGNET_FAILURE;
155
	  }
156
	  else{
157
	    right_data = (right_data_buf>>5) & 1023;
158
	  }
159
	
192
		else if (buf_index == 3)
193
			left_data_buf |= (((short)data) << 1) & (0xFF<<1);
194
		
195
		else if (buf_index == 4)
196
			left_data_buf |= (((short)data)>>7) & 0x1;
160 197

  
161
		//if(left_data!=INVALID || right_data!=INVALID){
162
		//  usb_puts("{");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("}\r\n");
163
		//}
164 198
		
165
		left_data_buf = 0;
166
		right_data_buf = 0;
199
		buf_index = (buf_index + 1) % 5;
167 200

  
168
	
169
	if(left_data < INVALID) {
170
		//Put new data onto data array
171
		left_data_array_put(left_data);
201
		if(buf_index==0) {
202
		  if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF)){
203
				left_data = INVALID;
204
		  }
205
			
206
		  else if(((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)){
207
			left_data = MAGNET_FAILURE;
208
		  }
209
		  else{
210
			left_data = (left_data_buf>>5) & 1023;
211
		  }
212
			
213
		  if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)){
214
			right_data = INVALID;
215
		  }
216
		  else if ( ((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)){
217
			left_data = MAGNET_FAILURE;
218
		  }
219
		  else{
220
			right_data = (right_data_buf>>5) & 1023;
221
		  }
222
		
172 223

  
173
		//Adjust left accumulator
174
		//dx = left_data - left_data_array_prev();
175
		dx = left_data_array_prev() - left_data;
224
			//if(left_data!=INVALID || right_data!=INVALID){
225
			//  usb_puts("{");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("}\r\n");
226
			//}
227
			
228
			  
229
  
230
  			left_data_buf = 0;
231
			right_data_buf = 0;
176 232

  
177
		/*left dx was negative on robot 7, could just be that
178
		  the encoder is backwards, so this may need to change
179
		  back
180
		*/
233
		
234
		if(left_data < INVALID) {
235
			//Put new data onto data array
236
			left_data_array_put(left_data);
181 237

  
238
			//Adjust left accumulator
239
			//dx = left_data - left_data_array_prev();
240
			dx = left_data_array_prev() - left_data;
241
			
242
			//Adjust velocity: save last dx
243
			left_v = left_dx;
182 244

  
245
			/*left dx was negative on robot 7, could just be that
246
			  the encoder is backwards, so this may need to change
247
			  back
248
			*/
183 249

  
184
		if(left_data_array_prev()==0)
185
		  dx=0;
186 250

  
187
		if(dx > 5) { //underflow
188
			left_dx += dx - 1023;
251
			if(left_data_array_prev()==0)
252
			  dx=0;
253

  
254
			if(dx > 5) { //underflow
255
				left_dx += dx - 1023;
256
			}
257
			else if(dx < -5) { //overflow
258
				left_dx += dx + 1023;
259
			}
260
			else {
261
				left_dx += dx;
262
			}
263
			
264
			//Adjust velocity: update
265
			left_v = left_dx - left_v;
189 266
		}
190
		else if(dx < -5) { //overflow
191
			left_dx += dx + 1023;
192
		}
193
		else {
194
			left_dx += dx;
195
		}
196
	}
197 267

  
198
	if(right_data < INVALID) {
199
		//Put new data onto data array
200
		right_data_array_put(right_data);
268
		if(right_data < INVALID) {
269
			//Put new data onto data array
270
			right_data_array_put(right_data);
201 271

  
202
		//Adjust right accumulator
203
		dx = right_data - right_data_array_prev();
272
			//Adjust right accumulator
273
			dx = right_data - right_data_array_prev();
204 274

  
205
		if(right_data_array_prev()==0)
206
		  dx=0;
275
			if(right_data_array_prev()==0)
276
			  dx=0;
207 277

  
208
		if(dx > 5) { //underflow
209
			right_dx += dx - 1023;
278
			if(dx > 5) { //underflow
279
				right_dx += dx - 1023;
280
			}
281
			else if(dx < -5) { //overflow
282
				right_dx += dx + 1023;
283
			}
284
			else {
285
				right_dx += dx;
286
			}
210 287
		}
211
		else if(dx < -5) { //overflow
212
			right_dx += dx + 1023;
288
  
289
		
213 290
		}
214
		else {
215
			right_dx += dx;
216
		}
217
	}
218
	}
219 291

  
220
	//Increment timecount accumulator
221
	timecount++;
292
		//Increment timecount accumulator
293
		timecount++;
294
		
295
	//}
222 296
}
223 297

  
224 298
//Helper Functions

Also available in: Unified diff