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_ldist_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 <speedepsilon)? speedepsilon : spd_r; 
59  
60 
}else if (dist_diff > delta){ 
61 
spd_l = dist_diff/divisor; 
62 
spd_l = (spd_l <speedepsilon)? speedepsilon : 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 