root / branches / encoders / code / behaviors / encoder_test / main.c @ 772
History | View | Annotate | Download (3.03 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, Il=0, Ir=0;
|
22 |
|
23 |
unsigned int l=0, r=0; |
24 |
|
25 |
int motorl=0, motorr=0; |
26 |
|
27 |
short int lmin=0,rmin=0; |
28 |
|
29 |
dragonfly_init(ALL_ON); |
30 |
|
31 |
encoders_init(); |
32 |
|
33 |
/*
|
34 |
motor1_set(FORWARD,200);
|
35 |
motor2_set(FORWARD,200);
|
36 |
|
37 |
while(1){
|
38 |
l=encoder_get_v(LEFT);
|
39 |
r=encoder_get_v(RIGHT);
|
40 |
|
41 |
usb_puti(l);usb_puts(" ");
|
42 |
usb_puti(r);usb_puts("\r\n");
|
43 |
}*/
|
44 |
|
45 |
//calibration code:
|
46 |
//TODO: this only find the min for stop->go, which is different than go->stop
|
47 |
/*
|
48 |
while(l<10){
|
49 |
l=encoder_get_dx(LEFT);
|
50 |
motor1_set(FORWARD,++lmin);
|
51 |
encoder_wait(1);
|
52 |
usb_puts(".");
|
53 |
}
|
54 |
motor1_set(FORWARD,0);
|
55 |
|
56 |
while(r<10){
|
57 |
r=encoder_get_dx(RIGHT);
|
58 |
motor2_set(FORWARD,++rmin);
|
59 |
encoder_wait(1);
|
60 |
usb_puts("|");
|
61 |
}
|
62 |
motor2_set(FORWARD,0);
|
63 |
|
64 |
usb_puts("lmin: ");usb_puti(lmin);
|
65 |
usb_puts(" rmin: ");usb_puti(rmin);usb_puts("\r\n");
|
66 |
|
67 |
button1_wait();
|
68 |
*/
|
69 |
lmin=161;
|
70 |
rmin=161;
|
71 |
|
72 |
//straight line forever code:
|
73 |
/*
|
74 |
motorl=lmin;
|
75 |
motorr=rmin;
|
76 |
|
77 |
while(1){
|
78 |
l=encoder_get_v(LEFT);
|
79 |
r=encoder_get_v(RIGHT);
|
80 |
|
81 |
motorl += (TARGET_V - l)*1;
|
82 |
motorr += (TARGET_V - r)*1;
|
83 |
|
84 |
Il += (TARGET_V - l);
|
85 |
Ir += (TARGET_V - r); //worked with +???
|
86 |
|
87 |
motorl += Il / 2;
|
88 |
motorr += Ir / 2;
|
89 |
|
90 |
usb_puti(l);usb_puts(":");usb_puti(motorl);usb_puts(" i: ");usb_puti(Il);usb_puts(" || ");
|
91 |
usb_puti(r);usb_puts(":");usb_puti(motorr);usb_puts(" i: ");usb_puti(Ir);usb_puts("\r\n");
|
92 |
|
93 |
if(motorl<0)
|
94 |
motorl=0;
|
95 |
if(motorl>255)
|
96 |
motorl=255;
|
97 |
|
98 |
if(motorr<0)
|
99 |
motorr=0;
|
100 |
if(motorr>255)
|
101 |
motorr=255;
|
102 |
|
103 |
motor1_set(FORWARD,motorl);
|
104 |
motor2_set(FORWARD,motorr);
|
105 |
|
106 |
encoder_wait(5);
|
107 |
|
108 |
}*/
|
109 |
|
110 |
//straight line v2
|
111 |
|
112 |
l=0;
|
113 |
r=0;
|
114 |
|
115 |
lmin+=lmin/16;
|
116 |
rmin+=rmin/16;
|
117 |
|
118 |
motorl=0;
|
119 |
motorr=0;
|
120 |
|
121 |
while(1){ |
122 |
l = encoder_get_dx(LEFT); |
123 |
r = encoder_get_dx(RIGHT); |
124 |
|
125 |
if(l < r)
|
126 |
motorl++; |
127 |
else
|
128 |
motorr++; |
129 |
|
130 |
if(motorl>motorr){
|
131 |
motorl-=motorr; |
132 |
motorr=0;
|
133 |
} |
134 |
else if(r<l){ |
135 |
r-=l; |
136 |
l=0;
|
137 |
} |
138 |
|
139 |
usb_puti(l);usb_puts(":");usb_puti(motorl);usb_puts(" "); |
140 |
usb_puti(r);usb_puts(":");usb_puti(motorr);usb_puts("\r\n"); |
141 |
|
142 |
motor1_set(FORWARD,lmin+motorl); |
143 |
motor2_set(FORWARD,rmin+motorr); |
144 |
|
145 |
encoder_wait(10);
|
146 |
} |
147 |
|
148 |
|
149 |
//go exact distance code:
|
150 |
|
151 |
|
152 |
while(1){ |
153 |
l=encoder_get_dx(LEFT); |
154 |
r=encoder_get_dx(RIGHT); |
155 |
|
156 |
l = (GO-l)/K; |
157 |
|
158 |
r = (GO-r)/K; |
159 |
|
160 |
if(l>255) |
161 |
l=255;
|
162 |
if(l<0) |
163 |
l=0;
|
164 |
|
165 |
if(r>255) |
166 |
r=255;
|
167 |
if(r<0) |
168 |
r=0;
|
169 |
|
170 |
usb_puti(l);usb_puts(":");usb_puti(encoder_get_v(LEFT));usb_puts(" "); |
171 |
usb_puti(r);usb_puts(":");usb_puti(encoder_get_v(RIGHT));usb_puts("\r\n"); |
172 |
|
173 |
motor1_set(FORWARD,(l<lmin)?(lmin+5):l);
|
174 |
motor2_set(FORWARD,(r<rmin)?(rmin+5):r);
|
175 |
|
176 |
if(ABS(r)<2 && ABS(l)<2){ |
177 |
move(0,0); |
178 |
usb_puts("done!\r\n");
|
179 |
while(1); |
180 |
} |
181 |
|
182 |
} |
183 |
|
184 |
} |
185 |
|