// Bbot log sensor data program

#include "pinout.h"
#include "sensors.h"
#include "motors.h"

#define STD_LOOP_TIME 10000 // us loop to loop


// Offsets
#define GYRO_OFFSET	1.2941
#define ACC_OFFSET	-4.0572

uint32_t lastLoopTime = STD_LOOP_TIME;
uint32_t lastLoopUsefulTime = STD_LOOP_TIME;
uint32_t loopStartTime = 0;

double ACC_angle;
double GYRO_rate; // In deg/sec

void setup()
{
	Serial.begin(115200);
	Serial.println("Starting...");

	pinMode(LED_PIN, OUTPUT);
	digitalWrite(LED_PIN,LOW);
	
	InitMotors();
	setupEncoder();
	initSensors();

	Serial.println("Start Logging Data...");
	delay(5000);
}

void loop()
{
	char msg[80];
	uint32_t newStartTime = micros();

	digitalWrite(LED_PIN,HIGH);
	lastLoopTime = newStartTime - loopStartTime;
	loopStartTime = newStartTime;

	updateSensors();  // Get new readings

	ACC_angle = getAccAngle() - ACC_OFFSET;  // In degs
	GYRO_rate = getGyroRate() - GYRO_OFFSET;  // In degs/sec

	// Output the data
	Serial.print(ACC_angle,8);
	Serial.print(",");
	Serial.print(GYRO_rate,8);
	Serial.print(",");
	Serial.println(lastLoopTime);

// *********************** loop timing control **************************
	lastLoopUsefulTime = micros()-loopStartTime;
	digitalWrite(LED_PIN,LOW);
	if(lastLoopUsefulTime<STD_LOOP_TIME)
		delayMicroseconds(STD_LOOP_TIME-lastLoopUsefulTime);
}
