Project

General

Profile

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.

View differences:

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