Revision 1540
Edited BOM Tracker project Makefile to make correctly. Removed old test file for net IR BOM vector from behaviors folder. Cleaning up Push-Pull
behavior to remove code which was moved into BOM library.
| trunk/code/behaviors/formation_control/push_pull/vectorTest.c | ||
|---|---|---|
| 1 |
#include <stdint.h> |
|
| 2 |
#include <dragonfly_lib.h> |
|
| 3 |
#include <wl_basic.h> |
|
| 4 |
#include <stdlib.h> |
|
| 5 |
|
|
| 6 |
int main (void) {
|
|
| 7 |
|
|
| 8 |
Vector v; |
|
| 9 |
int i; |
|
| 10 |
int bomVals[16]; |
|
| 11 |
|
|
| 12 |
dragonfly_init(ALL_ON); |
|
| 13 |
xbee_init(); |
|
| 14 |
encoders_init(); |
|
| 15 |
|
|
| 16 |
orbs_set_color(BLUE, GREEN); |
|
| 17 |
delay_ms(1000); |
|
| 18 |
orbs_set_color(GREEN, BLUE); |
|
| 19 |
delay_ms(1000); |
|
| 20 |
orbs_set_color(RED, RED); |
|
| 21 |
|
|
| 22 |
while (1) {
|
|
| 23 |
|
|
| 24 |
bom_refresh(BOM_ALL); |
|
| 25 |
for (i = 0; i < 16; i++) {
|
|
| 26 |
bomVals[i] = bom_get(i); |
|
| 27 |
} |
|
| 28 |
|
|
| 29 |
bom_print_usb(bomVals); |
|
| 30 |
|
|
| 31 |
bom_get_norm_vector(&v, bomVals); |
|
| 32 |
|
|
| 33 |
usb_puts("x: ");
|
|
| 34 |
usb_puti(v.x); |
|
| 35 |
usb_puts("\ty: ");
|
|
| 36 |
usb_puti(v.y); |
|
| 37 |
usb_puts("\n");
|
|
| 38 |
|
|
| 39 |
delay_ms(50); |
|
| 40 |
|
|
| 41 |
} |
|
| 42 |
|
|
| 43 |
while(1); |
|
| 44 |
|
|
| 45 |
} |
|
| trunk/code/behaviors/formation_control/push_pull/push_pull.c | ||
|---|---|---|
| 3 | 3 |
#include <wl_basic.h> |
| 4 | 4 |
#include <stdlib.h> |
| 5 | 5 |
|
| 6 |
/* Struct for storing vector components */ |
|
| 7 |
typedef struct {
|
|
| 8 |
int x; |
|
| 9 |
int y; |
|
| 10 |
} Vector; |
|
| 11 | 6 |
|
| 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(25 * 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 |
25, |
|
| 33 |
23, |
|
| 34 |
17, |
|
| 35 |
9, |
|
| 36 |
0, |
|
| 37 |
-9, |
|
| 38 |
-17, |
|
| 39 |
-23, |
|
| 40 |
-25, |
|
| 41 |
-23, |
|
| 42 |
-17, |
|
| 43 |
-9, |
|
| 44 |
0, |
|
| 45 |
9, |
|
| 46 |
17, |
|
| 47 |
23 |
|
| 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(25 * 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 |
9, |
|
| 64 |
17, |
|
| 65 |
23, |
|
| 66 |
25, |
|
| 67 |
23, |
|
| 68 |
17, |
|
| 69 |
9, |
|
| 70 |
0, |
|
| 71 |
-9, |
|
| 72 |
-17, |
|
| 73 |
-23, |
|
| 74 |
-25, |
|
| 75 |
-23, |
|
| 76 |
-17, |
|
| 77 |
-9 |
|
| 78 |
}; |
|
| 79 |
|
|
| 80 |
|
|
| 81 |
|
|
| 82 | 7 |
int main (void) {
|
| 83 | 8 |
|
| 84 | 9 |
/* Store current BOM readings and use them as a weighting factor */ |
| ... | ... | |
| 151 | 76 |
while(1); |
| 152 | 77 |
|
| 153 | 78 |
} |
| 154 |
|
|
| 155 |
|
|
| 156 |
static int get_bom_vector(Vector* bom_vector) {
|
|
| 157 |
|
|
| 158 |
return 0; |
|
| 159 |
|
|
| 160 |
} |
|
| trunk/code/projects/bom_tracker/Makefile | ||
|---|---|---|
| 4 | 4 |
|
| 5 | 5 |
# Relative path to the root directory (containing lib directory) |
| 6 | 6 |
ifndef COLONYROOT |
| 7 |
COLONYROOT = ../../..
|
|
| 7 |
COLONYROOT = ../.. |
|
| 8 | 8 |
endif |
| 9 | 9 |
|
| 10 | 10 |
# Target file name (without extension). |
Also available in: Unified diff