Cam.cpp

#include <cmath>
#include <cstring>
#include <cstdio>

float sensors[9];

struct float2 {
	float x, y;
};

const float diag = 1.f / sqrt(2.f);
const float2 directions[9] = { { -diag, -diag }, { 0.f, -1.f }, { diag, -diag },
							   { -1.f, 0.f }, { 0.f, 0.f }, { 1.f, 0.f }, 
							   { -diag, diag }, { 0.f, 1.f }, { diag, diag } };

void readSensors(float s[9]) {
	// Reads sensors into s
	

	// Normalize sensors
	float sum = 0.;
	for (int i = 0; i < 9; ++i) {
		sum += s[i];
	}
	for (int i = 0; i < 9; ++i) {
		s[i] /= sum;
	}
};

int main() {
	while (true) {
		readSensors(sensors);

		// Process dat data
		float x = 0.f, y = 0.f;
		for (int i = 0; i < 9; ++i) {
			x += directions[i].x * sensors[i];
			y += directions[i].y * sensors[i];
		}

		float angle = atan2f(y, x) * (180.f / 3.14f), magnitude = sqrt(x*x + y*y);

		printf("x: %f y: %f theta: %f mag: %f\n", x, y, angle, magnitude); 
	}
	
	return 0;
};