root / trunk / code / behaviors / formation_control / encoder_censored_forward / encoder_censored_forward.c @ 1750
History | View | Annotate | Download (2.78 KB)
1 |
|
---|---|
2 |
|
3 |
|
4 |
|
5 |
#include <dragonfly_lib.h> |
6 |
#include <wl_basic.h> |
7 |
#include <encoders.h> |
8 |
|
9 |
|
10 |
int delta = 16; |
11 |
int divisor = 7; |
12 |
int epsilon = 8; |
13 |
/*
|
14 |
speed initial motor speed.
|
15 |
distance in encoder_get_x unit.
|
16 |
|
17 |
Has trouble when the distance difference is greater than epsilon * divisor
|
18 |
|
19 |
better re initiate with encoders_init() since it might overflow. or with initial difference.
|
20 |
|
21 |
@param speed
|
22 |
@param distance 30000 is roughly <10 meter
|
23 |
@param time
|
24 |
not yet implemented.
|
25 |
*/
|
26 |
void encoder_censored_forward(int speed, int distance, int time){ |
27 |
encoders_init(); |
28 |
|
29 |
int dist_l = encoder_get_x(LEFT);
|
30 |
usb_puts("dist_l: ");
|
31 |
usb_puti(dist_l); |
32 |
usb_puts("\r");
|
33 |
|
34 |
int dist_r = encoder_get_x(RIGHT);
|
35 |
usb_puts("dist_l: ");
|
36 |
usb_puti(dist_l); |
37 |
usb_puts("\r");
|
38 |
|
39 |
motor_l_set(FORWARD, speed); |
40 |
motor_r_set(FORWARD, speed); |
41 |
|
42 |
int spd_l = speed;
|
43 |
int spd_r = speed;
|
44 |
int dist_diff = 0; |
45 |
// int dist_diff_new = 0; // was used for ensure the new speed is helping
|
46 |
int count = 0; // count for time |
47 |
while (dist_l<distance && dist_r<distance){
|
48 |
delay_ms(40);
|
49 |
count++; |
50 |
dist_l = encoder_get_x(LEFT); |
51 |
dist_r = encoder_get_x(RIGHT); |
52 |
dist_diff = dist_l-dist_r; |
53 |
if (dist_diff< -delta) {
|
54 |
spd_l += -dist_diff/divisor; |
55 |
spd_l = (spd_l >speed+epsilon)? speed+epsilon : spd_l; |
56 |
|
57 |
spd_r -= -dist_diff/divisor; |
58 |
spd_r = (spd_r <speed-epsilon)? speed-epsilon : spd_r; |
59 |
|
60 |
}else if (dist_diff > delta){ |
61 |
spd_l -= dist_diff/divisor; |
62 |
spd_l = (spd_l <speed-epsilon)? speed-epsilon : spd_l; |
63 |
|
64 |
spd_r += dist_diff/divisor; |
65 |
spd_r = (spd_r >speed+epsilon)? speed+epsilon : spd_r; |
66 |
|
67 |
} |
68 |
|
69 |
motor_l_set(FORWARD, spd_l); |
70 |
motor_r_set(FORWARD, spd_r); |
71 |
|
72 |
usb_puts( "dist_l and dist_r: ");
|
73 |
usb_puti(dist_l); |
74 |
usb_puts(" ");
|
75 |
usb_puti(dist_r); |
76 |
usb_puts("\r");
|
77 |
} |
78 |
|
79 |
motor_l_set(FORWARD, 0);
|
80 |
motor_r_set(FORWARD, 0);
|
81 |
usb_puts("time :");
|
82 |
usb_puti(count *40);
|
83 |
} |
84 |
|
85 |
void test_encoder(void){ |
86 |
encoders_init(); |
87 |
|
88 |
int dist_l = encoder_get_x(LEFT);
|
89 |
usb_puts("dist_l: ");
|
90 |
usb_puti(dist_l); |
91 |
usb_puts("\r");
|
92 |
|
93 |
int dist_r = encoder_get_x(RIGHT);
|
94 |
usb_puts("dist_r: ");
|
95 |
usb_puti(dist_r); |
96 |
usb_puts("\r\r\r");
|
97 |
|
98 |
for (int i=180;i<256; i++){ |
99 |
motor_l_set(FORWARD, i); |
100 |
motor_r_set(FORWARD, i); |
101 |
delay_ms(200);
|
102 |
usb_puts("\rspeed is: ");
|
103 |
usb_puti(i); |
104 |
usb_puts(" speed_l ");
|
105 |
usb_puti(encoder_get_dx(LEFT)); |
106 |
usb_puts(" speed_r ");
|
107 |
usb_puti(encoder_get_dx(RIGHT)); |
108 |
} |
109 |
|
110 |
|
111 |
|
112 |
} |
113 |
|
114 |
int main(void) |
115 |
{ |
116 |
/* Initialize dragonfly board */
|
117 |
dragonfly_init(ALL_ON); |
118 |
/* Initialize the basic wireless library */
|
119 |
wl_basic_init_default(); |
120 |
/* Set the XBee channel to 24 - must be standard among robots */
|
121 |
wl_set_channel(24);
|
122 |
|
123 |
encoder_censored_forward(225, 30000, 3000); |
124 |
// test_encoder();
|
125 |
|
126 |
while(1); /* END HERE, just in case something happened. This way we can see the red orb. */ |
127 |
} |
128 |
|
129 |
|