Statistics
| Revision:

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