root / branches / encoders / code / behaviors / encoder_test / main.c @ 753
History | View | Annotate | Download (965 Bytes)
1 | 749 | bneuman | #include <dragonfly_lib.h> |
---|---|---|---|
2 | 752 | bneuman | //#define usb_puti(x) x
|
3 | 749 | bneuman | |
4 | 753 | bneuman | #define GO 1024 |
5 | #define K 1 |
||
6 | |||
7 | #define ABS(x) ((x>0)?x:-x) |
||
8 | |||
9 | 749 | bneuman | int main(void) |
10 | { |
||
11 | 753 | bneuman | int l=0,r=0; |
12 | |||
13 | short int lmin=0,rmin=0; |
||
14 | |||
15 | 749 | bneuman | dragonfly_init(ALL_ON); |
16 | |||
17 | encoders_init(); |
||
18 | 752 | bneuman | |
19 | 753 | bneuman | while(l<4){ |
20 | l=encoder_get_dx(LEFT); |
||
21 | motor1_set(FORWARD,++lmin); |
||
22 | delay_ms(1);
|
||
23 | } |
||
24 | motor1_set(FORWARD,0);
|
||
25 | |||
26 | while(r<4){ |
||
27 | r=encoder_get_dx(RIGHT); |
||
28 | motor2_set(FORWARD,++rmin); |
||
29 | delay_ms(1);
|
||
30 | } |
||
31 | motor2_set(FORWARD,0);
|
||
32 | |||
33 | usb_puts("lmin: ");usb_puti(lmin);
|
||
34 | usb_puts(" rmin: ");usb_puti(rmin);usb_puts("\r\n"); |
||
35 | |||
36 | 749 | bneuman | while(1){ |
37 | 753 | bneuman | l=encoder_get_dx(LEFT); |
38 | r=encoder_get_dx(RIGHT); |
||
39 | |||
40 | l = (GO-l)/K; |
||
41 | |||
42 | r = (GO-r)/K; |
||
43 | |||
44 | if(l>255) |
||
45 | l=255;
|
||
46 | if(l<0) |
||
47 | l=0;
|
||
48 | |||
49 | if(r>255) |
||
50 | r=255;
|
||
51 | if(r<0) |
||
52 | r=0;
|
||
53 | |||
54 | usb_puti(l);usb_puts(" ");usb_puti(r);usb_puts("\r\n"); |
||
55 | |||
56 | motor1_set(FORWARD,(l<lmin)?(lmin+5):l);
|
||
57 | motor2_set(FORWARD,(r<rmin)?(rmin+5):r);
|
||
58 | |||
59 | if(ABS(r)<2 && ABS(l)<2){ |
||
60 | move(0,0); |
||
61 | usb_puts("done!\r\n");
|
||
62 | while(1); |
||
63 | } |
||
64 | |||
65 | 749 | bneuman | } |
66 | |||
67 | } |