Project

General

Profile

Statistics
| Revision:

root / trunk / code / behaviors / formation_control / push_pull / push_pull.c @ 1511

History | View | Annotate | Download (3.27 KB)

1
#include <stdint.h>
2
#include <dragonfly_lib.h>
3
#include <wl_basic.h>
4
#include <stdlib.h>
5

    
6
/* Struct for storing vector components */
7
typedef struct {
8
        int x;
9
        int y;
10
} Vector;
11

    
12

    
13
/* Function Prototypes */
14
static int get_bom_vector(Vector*);
15

    
16

    
17
/*******************************
18
 * BOM Vector Component Tables *
19
 *******************************/
20

    
21
/*
22
 * The x component of each BOM detector (indexed from 0 to 15)
23
 * was calculated using the following formula:
24
 *
25
 *                x_comp[i] = round(100 * cos ( 2 * pi / 16 * i) )
26
 *
27
 * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,
28
 * this effectively calculates the x component of the emitter vector where
29
 * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
30
 */
31
static const signed int x_comp[16] = {
32
        100,
33
        92,
34
        71,
35
        38,
36
        0,
37
        -38,
38
        -71,
39
        -92,
40
        -100,
41
        -92,
42
        -71,
43
        -38,
44
        0,
45
        38,
46
        71,
47
        92
48
};
49

    
50

    
51
/*
52
 * The y component of each BOM detector (indexed from 0 to 15)
53
 * was calculated using the following formula:
54
 *
55
 *                y_comp[i] = round(100 * sin ( 2 * pi / 16 * i) )
56
 *
57
 * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,
58
 * this effectively calculates the y component of the emitter vector where
59
 * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
60
 */
61
static signed int y_comp[16] = {
62
        0,
63
        38,
64
        71,
65
        92,
66
        100,
67
        92,
68
        71,
69
        38,
70
        0,
71
        -38,
72
        -71,
73
        -92,
74
        -100,
75
        -92,
76
        -71,
77
        -38
78
};
79

    
80

    
81

    
82
int main (void) {
83

    
84
        /* Store current BOM readings and use them as a weighting factor */
85
        uint8_t intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
86

    
87
        /* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
88
         * components. Calculated by multiplying the intensity by the x and y
89
         * component respectively (x and y components are stored in the tables
90
         * above). */
91
        int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
92
        int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
93
        
94
        /* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
95
         * components for the entire robot. */
96
        long net_x_comp = 0;
97
        long net_y_comp = 0;
98

    
99
        /* Variables used to normalize the net component values */
100
        int total_intensity = 0;
101
        int normalized_net_x_comp = 0;
102
        int normalized_net_y_comp = 0;
103

    
104
        int i = 0;
105

    
106
        dragonfly_init(ALL_ON);
107
        xbee_init();
108
        encoders_init();
109

    
110
        orbs_set_color(BLUE, GREEN);
111
        delay_ms(1000);
112
        orbs_set_color(GREEN, BLUE);
113
        delay_ms(1000);
114
        orbs_set_color(RED, RED);
115

    
116
        while (1) {
117

    
118
                /* Make sure to clear our accumulators */
119
                net_x_comp = 0;
120
                net_y_comp = 0;
121
                total_intensity = 0;
122

    
123
                bom_refresh(BOM_ALL);
124
                for (i = 0; i < 16; i++) {
125
                        /* BOM intensity is actually measured as more intense = closer to 0 */
126
                        intensity[i] = 255 - bom_get(i);
127
                        weighted_x_comp[i] = intensity[i] * x_comp[i];
128
                        weighted_y_comp[i] = intensity[i] * y_comp[i];
129
                        net_x_comp += weighted_x_comp[i];
130
                        net_y_comp += weighted_y_comp[i];
131
                        total_intensity += intensity[i];
132
                }
133

    
134
                if (total_intensity > 0) {
135
                        normalized_net_x_comp = net_x_comp / total_intensity;
136
                        normalized_net_y_comp = net_y_comp / total_intensity;
137
                }
138

    
139
                usb_puts("x: ");        
140
                usb_puti(normalized_net_x_comp);
141
                usb_puts("\ty: ");
142
                usb_puti(normalized_net_y_comp);
143
                usb_puts("\n");
144

    
145
                delay_ms(50);
146

    
147
        }
148

    
149
        while(1);
150

    
151
}
152

    
153

    
154
static int get_bom_vector(Vector* bom_vector) {
155

    
156
        return EXIT_SUCCESS;
157

    
158
}