Revision 1510
Implementing function to calculate get_bom_vector() function. Added
MATLAB script to assist in calculating the scalar value used to scale up
the BOM unit vectors such that the worst case sum will not overflow an
int data type.
| trunk/code/behaviors/formation_control/push_pull/main.c | ||
|---|---|---|
| 1 |
#include <stdint.h> |
|
| 1 | 2 |
#include <dragonfly_lib.h> |
| 2 | 3 |
#include <wl_basic.h> |
| 3 |
#include <stdint.h>
|
|
| 4 |
#include <stdlib.h>
|
|
| 4 | 5 |
|
| 5 | 6 |
/* Struct for storing vector components */ |
| 6 | 7 |
typedef struct {
|
| 7 |
uint8_t x;
|
|
| 8 |
uint8_t y;
|
|
| 8 |
int x;
|
|
| 9 |
int y;
|
|
| 9 | 10 |
} Vector; |
| 10 | 11 |
|
| 11 | 12 |
|
| 12 | 13 |
/* Function Prototypes */ |
| 13 |
static const Vector get_bom_vector();
|
|
| 14 |
static int get_bom_vector(Vector*);
|
|
| 14 | 15 |
|
| 15 | 16 |
|
| 16 | 17 |
/******************************* |
| ... | ... | |
| 81 | 82 |
int main (void) {
|
| 82 | 83 |
|
| 83 | 84 |
/* Store current BOM readings and use them as a weighting factor */ |
| 84 |
int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
|
| 85 |
uint8_t intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
|
| 85 | 86 |
|
| 86 | 87 |
/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
|
| 87 | 88 |
* components. Calculated by multiplying the intensity by the x and y |
| ... | ... | |
| 150 | 151 |
} |
| 151 | 152 |
|
| 152 | 153 |
|
| 153 |
static Vector get_bom_vector() {
|
|
| 154 |
static int get_bom_vector(Vector* bom_vector) {
|
|
| 154 | 155 |
|
| 156 |
return EXIT_SUCCESS; |
|
| 157 |
|
|
| 155 | 158 |
} |
| trunk/code/behaviors/formation_control/push_pull/vectorSizeSpecs.m | ||
|---|---|---|
| 1 |
% vectorSizeSpecs - Script used to help calculate the max scalar value by |
|
| 2 |
% which to multiply the BOM unit vectors (thus increasing the |
|
| 3 |
% precision of the scaled vectors) such that in the worst case, the |
|
| 4 |
% total net sum will not overflow the data type used to represent |
|
| 5 |
% the net vector components. |
|
| 6 |
% |
|
| 7 |
% Author: John Sexton, Colony Project, CMU Robotics Club |
|
| 8 |
|
|
| 9 |
|
|
| 10 |
% Parameters |
|
| 11 |
maxIntensity = 255; |
|
| 12 |
dataBits = 16; |
|
| 13 |
scalar = 25; |
|
| 14 |
|
|
| 15 |
n = 0:8; |
|
| 16 |
angle = n * 2 * pi / 16; |
|
| 17 |
|
|
| 18 |
vector_components = sin(angle); |
|
| 19 |
max_net_component = sum(vector_components); |
|
| 20 |
|
|
| 21 |
% Formula for max_net_component (mnc), obtained by summing |
|
| 22 |
% values from the unit circle at angles of 0 : pi/8 : pi |
|
| 23 |
% in quadrants I and II |
|
| 24 |
mnc = 1; %sin(pi/2) |
|
| 25 |
mnc = mnc + (2* (1/2)*sqrt(2+sqrt(2))); %sin(3pi/8) and sin(5pi/8) |
|
| 26 |
mnc = mnc + (2* sqrt(2)/2); %sin(pi/4) and sin(3pi/4) |
|
| 27 |
mnc = mnc + (2* (1/2)*sqrt(2-sqrt(2))); %sin(pi/8) and sin(7pi/8) |
|
| 28 |
|
|
| 29 |
|
|
| 30 |
calc_scalar = (2^(dataBits-1)-1) / (max_net_component * maxIntensity); |
|
| 31 |
% calc_scalar = 25.560 => use 25 as scalor |
|
| 32 |
|
|
| 33 |
fprintf('With %d data bits, calculated scalar value: %.3f\n\n', dataBits, calc_scalar);
|
|
| 34 |
|
|
| 35 |
|
|
| 36 |
% Check worst case |
|
| 37 |
scaled_vector_components = scalar * vector_components; |
|
| 38 |
|
|
| 39 |
worst_case = scaled_vector_components * maxIntensity; |
|
| 40 |
worst_sum = sum(worst_case); |
|
| 41 |
fprintf('With scalar %d, max worst case sum: %d\n', scalar, worst_sum);
|
|
| 42 |
fprintf('Max number: 2^(%d-1) - 1 = %d\n', dataBits, (2^(dataBits-1)-1));
|
|
Also available in: Unified diff