root / branches / encoders / code / behaviors / encoder_test / main.c @ 771
History | View | Annotate | Download (2.22 KB)
1 |
|
---|---|
2 |
/* encoders_test: this behavior uses the encoders to move a given
|
3 |
* distance, specified by GO where GO/1024 is the number of wheel
|
4 |
* rotations.
|
5 |
* It first calibrates the motors by finding the minimum reading
|
6 |
* it can use to move them.
|
7 |
*/
|
8 |
|
9 |
#include <dragonfly_lib.h> |
10 |
//#define usb_puti(x) x
|
11 |
|
12 |
#define TARGET_V 25 |
13 |
|
14 |
#define GO 1024 |
15 |
#define K 1 |
16 |
|
17 |
#define ABS(x) ((x>0)?x:-x) |
18 |
|
19 |
int main(void) |
20 |
{ |
21 |
int l=0,r=0, motorl=0, motorr=0; |
22 |
|
23 |
short int lmin=0,rmin=0; |
24 |
|
25 |
dragonfly_init(ALL_ON); |
26 |
|
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
|
43 |
|
44 |
while(l<10){ |
45 |
l=encoder_get_dx(LEFT); |
46 |
motor1_set(FORWARD,++lmin); |
47 |
encoder_wait(1);
|
48 |
usb_puts(".");
|
49 |
} |
50 |
motor1_set(FORWARD,0);
|
51 |
|
52 |
while(r<10){ |
53 |
r=encoder_get_dx(RIGHT); |
54 |
motor2_set(FORWARD,++rmin); |
55 |
encoder_wait(1);
|
56 |
usb_puts("|");
|
57 |
} |
58 |
motor2_set(FORWARD,0);
|
59 |
|
60 |
usb_puts("lmin: ");usb_puti(lmin);
|
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 |
|
99 |
|
100 |
while(1){ |
101 |
l=encoder_get_dx(LEFT); |
102 |
r=encoder_get_dx(RIGHT); |
103 |
|
104 |
l = (GO-l)/K; |
105 |
|
106 |
r = (GO-r)/K; |
107 |
|
108 |
if(l>255) |
109 |
l=255;
|
110 |
if(l<0) |
111 |
l=0;
|
112 |
|
113 |
if(r>255) |
114 |
r=255;
|
115 |
if(r<0) |
116 |
r=0;
|
117 |
|
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"); |
120 |
|
121 |
motor1_set(FORWARD,(l<lmin)?(lmin+5):l);
|
122 |
motor2_set(FORWARD,(r<rmin)?(rmin+5):r);
|
123 |
|
124 |
if(ABS(r)<2 && ABS(l)<2){ |
125 |
move(0,0); |
126 |
usb_puts("done!\r\n");
|
127 |
while(1); |
128 |
} |
129 |
|
130 |
} |
131 |
|
132 |
} |
133 |
|