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:

branches/encoders/code/behaviors/encoder_test/main.c
9 9
#include <dragonfly_lib.h>
10 10
//#define usb_puti(x) x
11 11

  
12
#define GO 2048
12
#define TARGET_V 25
13

  
14
#define GO 1024
13 15
#define K 1
14 16

  
15 17
#define ABS(x) ((x>0)?x:-x)
16 18

  
17 19
int main(void)
18 20
{
19
  int l=0,r=0;
21
  int l=0,r=0, motorl=0, motorr=0;
20 22

  
21 23
  short int lmin=0,rmin=0;
24
  
25
  dragonfly_init(ALL_ON);
22 26

  
23
	dragonfly_init(ALL_ON);
27
  encoders_init();
28
  
29
  /*
30
  motor1_set(FORWARD,200);
31
  motor2_set(FORWARD,200);
32
  
33
  while(1){
34
  		l=encoder_get_v(LEFT);
35
		r=encoder_get_v(RIGHT);
36
		
37
		usb_puti(l);usb_puts(" ");
38
		usb_puti(r);usb_puts("\r\n");
39
}*/
40
  
41
  //calibration code:
42
  //TODO: this only find the min for stop->go, which is different than go->stop
24 43

  
25
	encoders_init();
26

  
27
	while(l<10){
44
 	while(l<10){
28 45
	  l=encoder_get_dx(LEFT);
29 46
	  motor1_set(FORWARD,++lmin);
30
	  delay_ms(1);
47
	  encoder_wait(1);
48
	  usb_puts(".");
31 49
	}
32 50
	motor1_set(FORWARD,0);
33 51

  
34 52
	while(r<10){
35 53
	  r=encoder_get_dx(RIGHT);
36 54
	  motor2_set(FORWARD,++rmin);
37
	  delay_ms(1);
55
	  encoder_wait(1);
56
	  usb_puts("|");
38 57
	}
39 58
	motor2_set(FORWARD,0);
40 59

  
41 60
	usb_puts("lmin: ");usb_puti(lmin);
42 61
	usb_puts(" rmin: ");usb_puti(rmin);usb_puts("\r\n");
62
  
63
  
64
  //straight line forever code:
65
  
66
  	motorl=lmin;
67
	motorr=rmin;
68
  
69
  while(1){
70
		l=encoder_get_v(LEFT);
71
		r=encoder_get_v(RIGHT);
72
		
73
		motorl += (TARGET_V - l)*2;
74
		motorr += (TARGET_V - r)*2;
75
		
76
		usb_puti(motorl);usb_puts(":");usb_puti(l);usb_puts("  ");
77
		usb_puti(motorr);usb_puts(":");usb_puti(r);usb_puts("\r\n");
78
		
79
		if(motorl<0)
80
			motorl=0;
81
		if(motorl>255)
82
			motorl=255;
83
			
84
		if(motorr<0)
85
			motorr=0;
86
		if(motorr>255)
87
			motorr=255;
88
		
89
		motor1_set(FORWARD,motorl);
90
		motor2_set(FORWARD,motorr);
91
		
92
		encoder_wait(1);
93
  
94
	}
95
	
96
	
97
  //go exact distance code:
98
  
43 99

  
44 100
	while(1){
45 101
		l=encoder_get_dx(LEFT);
......
59 115
		if(r<0)
60 116
		  r=0;
61 117

  
62
		usb_puti(l);usb_puts(" ");usb_puti(r);usb_puts("\r\n");
118
		usb_puti(l);usb_puts(":");usb_puti(encoder_get_v(LEFT));usb_puts(" ");
119
		usb_puti(r);usb_puts(":");usb_puti(encoder_get_v(RIGHT));usb_puts("\r\n");
63 120

  
64 121
		motor1_set(FORWARD,(l<lmin)?(lmin+5):l);
65 122
		motor2_set(FORWARD,(r<rmin)?(rmin+5):r);
branches/encoders/code/behaviors/encoder_test/Makefile
14 14
# USE_WIRELESS = 1
15 15

  
16 16
# com1 = serial port. Use lpt1 to connect to parallel port.
17
AVRDUDE_PORT = /dev/ttyUSB0
17
AVRDUDE_PORT = com4 # /dev/ttyUSB0
18 18
#
19 19
###################################
20 20

  
branches/encoders/code/lib/src/libdragonfly/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
branches/encoders/code/projects/libdragonfly/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
branches/encoders/code/projects/libdragonfly/encoders.h
12 12
#define INVALID 1024
13 13
#define MAGNET_FAILURE 1025
14 14

  
15

  
16
//delay_ms argument after a full read is complete
17
#define ENCODER_DELAY 20
18

  
19
#define MIN_V (-100)
20
#define MAX_V 100
21

  
15 22
//Data invalid flags (hardware failure):
16 23
#define OCF _BV(4)
17 24
#define COF _BV(3)
......
33 40
int encoder_get_tc(void);
34 41
void encoder_rst_tc(void);
35 42

  
43
int encoder_get_v(char encoder);
44

  
45
//waits for the next encoder reading, then returns
46
void encoder_wait( int nReadings );
47

  
36 48
#endif

Also available in: Unified diff