File content as of revision 0:8f5db5085df7:

#include "FXAS21002.h"
#include "FXOS8700Q.h"
#include "mbed.h"
#include "CarPWM.h"
#include "receiver.h"
#include "Motor.hpp"
#include "rtos.h"

#include "LED.hpp"
#include "PIDController.hpp"

#define PI 3.141593
#define Ts 0.02			// Controller period(Seconds)
#define END_THRESH 4	//For magnetometer calibration
#define START_THRESH 10 //For magnetometer calibration
#define MINIMUM_VELOCITY 15
#define GYRO_PERIOD 15000 //us

#define MIN -1.5
#define MAX 1.5

//Control Objetcs
PwmOut servo(PTD1); // Servo connected to pin PTD3
Motor *motor = new Motor(PTC3);
FXOS8700Q_mag mag(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
FXOS8700Q_acc acc(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
FXAS21002 *gyro = new FXAS21002(PTE25, PTE24);
//Leds Objects

LED *led = new LED(PTD0, PTC4, PTC12, LED_RED, LED_GREEN, LED_BLUE);
DigitalOut main_led(PTB2);
//Protocol Objects
Receiver rcv;
EthernetInterface eth;

// PID controller parameters and functions
PIDController *pidController = new PIDController(motor, &servo, gyro);

// Motor and servo variables
float saved_velocity = 0;
bool brake = false;

// Magnetometer/Gyro variables and functions
float max_x = 0, max_y = 0, min_x = 0, min_y = 0, x, y;
MotionSensorDataUnits mag_data;
MotionSensorDataCounts mag_raw;
float processMagAngle();
void magCal();

// Protocol
void readProtocol();

int main()
{
	// Initializing sensors
	main_led = 1;
	acc.enable();
	gyro->gyro_config(MODE_1);

	// Set initial control configurations
	motor->setVelocity(0);

	// Protocol initialization

	printf("Initializing Ethernet!\r\n");
	led->set_leds_color_nxp(LED::RED);
	led->set_leds_color_main(LED::RED);

	eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
	eth.connect();
	printf("Protocol initialized! \r\n");
	gyro->start_measure(GYRO_PERIOD);
	led->set_leds_color_nxp(LED::BLUE);
	led->set_leds_color_main(LED::BLUE);
	

	rcv.set_socket();

	main_led = 0;
	
	pidController->PIDAttach();

	//main loop
	while (1)
	{
		readProtocol();
	}
}

void readProtocol()
{
	if (!rcv.receive())
		return;
	char msg = rcv.get_msg();

	switch (msg)
	{
	case NONE:
		break;
	case BRAKE:
		float intensity, b_wait;
		rcv.get_brake(&intensity, &b_wait);
		if (!brake)
		{
			led->set_leds_color_nxp(LED::YELLOW);
			motor->stopJogging();
			setServoPWM(0, servo);
			pidController->PIDDetach();
			motor->brakeMotor(intensity, b_wait);
			pidController->PIDAttach();

			saved_velocity = 0;
			brake = true;
		}
		break;
	case GYRO_ZERO:
		gyro->stop_measure();
		wait(0.05);
		gyro->start_measure(GYRO_PERIOD);
		break;
	case ANG_SET:
		led->set_leds_color_nxp(LED::PURPLE);
		//printf("ANG_SET\r\n");
		pidController->setGyroReference(gyro->get_angle());
		pidController->initializeController();
		break;
	case ANG_RST:
		//printf("ANG_RST\r\n");
		pidController->setGyroReference(0);
		led->set_leds_color_nxp(LED::PURPLE);
		pidController->initializeController();
		break;
	case MAG_ANG_REF:
		led->set_leds_color_nxp(LED::BLUE);
		pidController->setTargetAngle(rcv.get_mag_ang_ref() - processMagAngle());
		break;
	case ABS_ANG_REF:
		led->set_leds_color_nxp(LED::GREEN);
		pidController->setTargetAngle(rcv.get_abs_ang_ref());
		break;
	case REL_ANG_REF:
		led->set_leds_color_nxp(LED::RED);
		pidController->setTargetAngle(rcv.get_rel_ang_ref() + gyro->get_angle() * PI / 180);
		break;
	case GND_VEL:
		led->set_leds_color_nxp(LED::AQUA);
		saved_velocity = rcv.get_gnd_vel();
		if (saved_velocity > 0)
		{
			motor->setVelocity(saved_velocity);
			if (abs(saved_velocity) > MINIMUM_VELOCITY)
				brake = false;
		}
		break;
	case JOG_VEL:
		led->set_leds_color_nxp(LED::WHITE);
		float p, r;
		rcv.get_jog_vel(&p, &r);
		if (p == 0 || r == 0)
			motor->stopJogging();
		else
			motor->startJogging(r, p);
		break;
	case PID_PARAMS:
		led->set_leds_color_nxp(LED::WHITE);
		float arr[4];
		rcv.get_pid_params(arr);
		pidController->setPIDConstants(arr[0], arr[1], arr[2], arr[3]);

		break;
	case MAG_CALIB:
		led->set_leds_color_nxp(LED::BLUE);
		float mag[4];
		rcv.get_mag_calib(mag);
		max_x = mag[1];
		max_y = mag[3];
		min_x = mag[0];
		min_y = mag[2];
		//printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]);
		break;
	case LED_ON:
		led->set_leds_color_nxp(LED::WHITE);
		led->set_leds_color_main(LED::WHITE);
		main_led = 1;
		break;
	case LED_OFF:
		led->turn_leds_off_nxp();
		led->turn_leds_off_main();
		main_led = 0;
		break;
	default:
		//ser.printf("unknown command!\r\n");
	}
}

/* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
/* Function to normalize the magnetometer reading */
void magCal()
{
	//red_led = 0;
	printf("Starting Calibration");
	mag.enable();
	wait(0.01);
	mag.getAxis(mag_data);
	float x0 = max_x = min_y = mag_data.x;
	float y0 = max_y = min_y = mag_data.y;
	bool began = false;
	while (!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH))
	{
		mag.getAxis(mag_data);
		if (mag_data.x > max_x)
			max_x = mag_data.x;
		if (mag_data.y > max_y)
			max_y = mag_data.y;
		if (mag_data.y < min_y)
			min_y = mag_data.y;
		if (mag_data.x < min_x)
			min_x = mag_data.x;
		if (abs(mag_data.x - x0) > START_THRESH && abs(mag_data.y - y0) > START_THRESH)
			began = true;
		printf("began: %d  X-X0: %f , Y-Y0: %f \n\r", began, abs(mag_data.x - x0), abs(mag_data.y - y0));
	}
	printf("Calibration Completed: X_MAX = %f , Y_MAX = %f , X_MIN = %f and Y_MIN = %f \n\r", max_x, max_y, min_x, min_y);
}

/* Function to transform the magnetometer reading in angle(rad/s).*/
float processMagAngle()
{
	// printf("starting ProcessMagAngle()\n\r");
	float mag_lpf = 0;
	Timer t1;
	for (int i = 0; i < 20; i++)
	{
		//printf("\r\n");
		//wait(0.1);
		t1.start();
		__disable_irq();
		mag.getAxis(mag_data);
		__enable_irq();
		t1.stop();
		x = 2 * (mag_data.x - min_x) / float(max_x - min_x) - 1;
		y = 2 * (mag_data.y - min_y) / float(max_y - min_y) - 1;
		mag_lpf = mag_lpf + (-atan2(y, x));
		wait(0.015);
	}
	// wait(20*0.01);
	// printf("Finished ProcessMagAngle() %d \n\r",t1.read_us());
	return mag_lpf / 20;
}