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:

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);

Also available in: Unified diff