Revision 0:8f5db5085df7, committed 2020-09-21

Comitter:

starling

Date:

Mon Sep 21 21:42:07 2020 +0000

Commit message:

12 mar 2020

Changed in this revision

diff -r 000000000000 -r 8f5db5085df7 Control/PIDController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/PIDController.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,111 @@
+#include "PIDController.hpp"
+
+PIDController::PIDController(Motor* motor, PwmOut* servo, FXAS21002* gyro) {
+    this->motor = motor;
+    this->servo = servo;
+    this->gyro = gyro;
+
+    initializeController();
+
+    target_angle = 0;
+    saved_velocity = 0;
+    gyro_reference = 0;
+
+    interrupt_time = 0.02;
+    Ts = interrupt_time*1000;
+}
+
+void PIDController::initializeController() {
+    P = INITIAL_P;
+    I = INITIAL_I;
+    D = INITIAL_D;
+    N = INITIAL_N;
+
+    for (int i = 0; i < 2; i++)
+	{
+		e[i] = 0;
+		ui[i] = 0;
+		ud[i] = 0;
+	}
+}
+
+void PIDController::PIDTrigger() {
+    printf("PIDCntroller \r\n");
+    PIDController::RunPID();
+}
+
+void PIDController::PIDAttach() {
+    
+    pid_trigger.attach(std::bind(&PIDController::PIDTrigger, this), 1.0);
+}
+
+void PIDController::PIDDetach() {
+    pid_trigger.detach();
+}
+
+void PIDController::setInterruptTime(float interrupt_time) {
+	this->interrupt_time = interrupt_time;
+}
+
+void PIDController::setPIDConstants(float P, float I, float D, float N) {
+    this->P = P;
+    this->I = I;
+    this->D = D;
+    this->N = N;
+}
+
+void PIDController::setTargetAngle(float target_angle) {
+    this->target_angle = target_angle;
+    if (target_angle > PI)
+        this->target_angle = target_angle - 2 * PI;
+    else if (target_angle < -PI)
+        this->target_angle = target_angle + 2 * PI;
+}
+
+void PIDController::addTargetAngle(float add_angle) {
+    this->target_angle = this->target_angle + add_angle;
+}
+
+void PIDController::Control() {
+	setServoPWM(u*100/(PI/8), *servo);
+    if(abs(e[0]) >= ERROR_THRESH && motor->getVelocity() > MINIMUM_CURVE_VELOCITY) {
+        saved_velocity = motor->getVelocity();
+        motor->setVelocity(MINIMUM_CURVE_VELOCITY);
+    }
+    else if(abs(e[0]) < ERROR_THRESH && motor->getVelocity() != saved_velocity) {
+        motor->setVelocity(saved_velocity);
+    }
+}
+/* PID controller for angular position */
+void PIDController::RunPID() {
+    float feedback = gyro->get_angle() - gyro_reference;
+    	e[0]= e[0] - 2*PI;
+    if(e[0] < -PI)
+    	e[0] = e[0] + 2*PI;
+
+    /* Proportinal Part */
+    up[0] = e[0]*P;  
+
+    /* Integral Part */
+    ui[1] = ui[0];
+    if(abs(u) < PI/8){
+    	ui[0] = (P*I*Ts)*e[1] + ui[1];    
+    }
+    else if(u > 0)
+    	ui[0] = PI/8 - up[0];
+    else if(u < 0)
+    	ui[0] = -PI/8 - up[0]; 
+
+    /* Derivative Part */
+    ud[1] = ud[0];
+    ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1); 
+    
+    /** Sum of all parts **/ 
+    u = up[0] + ud[0] + ui[0];
+
+    Control();
+}
+
+void PIDController::setGyroReference(float gyro_ref) {
+	this->gyro_reference = gyro_ref;
+}

diff -r 000000000000 -r 8f5db5085df7 Control/PIDController.hpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/PIDController.hpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,74 @@
+#ifndef __PIDCONTROLLER__
+#define __PIDCONTROLLER__
+
+#include "mbed.h"
+#include "Motor.hpp"
+#include "FXAS21002.h"
+#include "FXOS8700Q.h"
+#include "rtos.h"
+
+#define PI 3.141593
+
+#define INITIAL_P 0.452531214933414
+#define INITIAL_I 5.45748932024049
+#define INITIAL_D 0.000233453623255507
+#define INITIAL_N 51.0605584484153
+
+#define ERROR_THRESH    PI/5
+#define MINIMUM_CURVE_VELOCITY 19
+
+class PIDController
+{
+    private:
+    
+        PwmOut* servo;
+        Motor* motor;
+        FXAS21002* gyro;
+      
+        float target_angle;
+        float P;
+        float I;
+        float D;
+        float N;
+        float e[2];
+        float up[2];
+        float ui[2];
+        float ud[2];
+        float u;
+        float saved_velocity;
+        float gyro_reference;
+    
+        Ticker pid_trigger; 
+        float interrupt_time;
+        float Ts;
+    
+        void PIDTrigger();
+    
+        void Control();
+    
+        void RunPID();
+        
+    public:
+        PIDController(Motor* motor, PwmOut* servo, FXAS21002* gyro);
+    
+        void initializeController();
+        // Atach PIDController.
+        void PIDAttach();
+    
+        // Detach PIDController.
+        void PIDDetach();
+    
+        void setInterruptTime(float interrupt_time);
+    
+        void setPIDConstants(float P, float I, float D, float N);
+    
+        void setTargetAngle(float target_angle);
+    
+        void addTargetAngle(float add_angle);
+        
+        void setGyroReference(float gyro_ref);
+        
+
+};
+
+#endif //__PIDCONTROLLER__
\ No newline at end of file

diff -r 000000000000 -r 8f5db5085df7 EthernetInterface.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EthernetInterface.lib	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/mbed_official/code/EthernetInterface/#4d7bff17a592

diff -r 000000000000 -r 8f5db5085df7 LED/LED.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LED/LED.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,100 @@
+#include "LED.hpp"
+
+LED::LED(PinName red_main, PinName green_main, PinName blue_main, 
+		 PinName red_nxp, PinName green_nxp, PinName blue_nxp):
+	red_led_main(red_main),
+	green_led_main(green_main),
+	blue_led_main(blue_main),
+	red_led_nxp(red_nxp),
+	green_led_nxp(green_nxp),
+	blue_led_nxp(blue_nxp)
+	{
+		RGB_LED_ON = 0;                    //active Low
+		RGB_LED_OFF = 1;                   //active Low
+		MAIN_RGB_LED_ON = 1;                    //active HIGH	
+		MAIN_RGB_LED_OFF = 0; 	
+	}
+
+void LED::turn_leds_off_nxp() {
+	red_led_nxp = RGB_LED_OFF;
+	green_led_nxp = RGB_LED_OFF;
+	blue_led_nxp = RGB_LED_OFF;
+}
+
+void LED::set_leds_color_nxp(int color) {
+	turn_leds_off_nxp();
+
+	switch(color) {
+		case RED:
+			turn_leds_off_nxp();
+			red_led_nxp = RGB_LED_ON;
+			break;
+		case GREEN:
+			turn_leds_off_nxp();
+			green_led_nxp = RGB_LED_ON;
+			break;
+		case BLUE:
+			turn_leds_off_nxp();
+			blue_led_nxp = RGB_LED_ON;
+			break;
+		case BLACK:
+			turn_leds_off_nxp();
+			break;
+		case WHITE:
+			red_led_nxp = RGB_LED_ON;
+			green_led_nxp = RGB_LED_ON;
+			blue_led_nxp = RGB_LED_ON;
+			break;
+		case PURPLE:
+			turn_leds_off_nxp();
+			red_led_nxp = RGB_LED_ON;
+			blue_led_nxp = RGB_LED_ON;
+			break;
+		case YELLOW:
+			turn_leds_off_nxp();
+			red_led_nxp = RGB_LED_ON;
+			green_led_nxp = RGB_LED_ON;
+			break;
+		case AQUA:
+			turn_leds_off_nxp();
+			blue_led_nxp = RGB_LED_ON;
+			green_led_nxp = RGB_LED_ON;
+			break;
+		default:
+			break;
+	}
+	
+}
+
+void LED::turn_leds_off_main() {
+	red_led_main = MAIN_RGB_LED_OFF;
+	green_led_main = MAIN_RGB_LED_OFF;
+	blue_led_main = MAIN_RGB_LED_OFF;
+}
+
+void LED::set_leds_color_main(int color) {
+	switch(color) {
+		case RED:
+			turn_leds_off_main();
+			red_led_main = MAIN_RGB_LED_ON;
+			break;
+		case GREEN:
+			turn_leds_off_main();
+			green_led_main = MAIN_RGB_LED_ON;
+			break;
+		case BLUE:
+			turn_leds_off_main();
+			blue_led_main = MAIN_RGB_LED_ON;
+			break;
+		case BLACK:
+			turn_leds_off_main();
+			break;
+		case WHITE:
+			red_led_main = MAIN_RGB_LED_ON;
+			green_led_main = MAIN_RGB_LED_ON;
+			blue_led_main = MAIN_RGB_LED_ON;
+			break;
+		default:
+			break;
+	}
+}
\ No newline at end of file

diff -r 000000000000 -r 8f5db5085df7 LED/LED.hpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LED/LED.hpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,46 @@
+#ifndef __LED__
+#define __LED__
+
+#include "mbed.h"
+
+
+class LED {
+    private:
+    	int RGB_LED_ON;                    //active Low
+		int RGB_LED_OFF;                   //active Low
+		int MAIN_RGB_LED_ON;                    //active HIGH	
+		int MAIN_RGB_LED_OFF; 
+
+        // Leds Objects
+        DigitalOut red_led_main;
+        DigitalOut green_led_main;
+        DigitalOut blue_led_main;
+        
+        DigitalOut red_led_nxp;
+        DigitalOut green_led_nxp;
+        DigitalOut blue_led_nxp;
+
+
+    public:
+        enum{
+        	BLACK,
+        	RED,
+        	GREEN,
+        	BLUE,
+            WHITE,
+        	PURPLE,
+        	YELLOW,
+        	AQUA};
+            
+        LED(PinName red_main, PinName green_main, PinName blue_main, 
+		    PinName red_nxp, PinName green_nxp, PinName blue_nxp);
+
+        void set_leds_color_nxp(int color);
+        void turn_leds_off_nxp();
+
+        void set_leds_color_main(int color);
+        void turn_leds_off_main();
+        
+
+};
+#endif //__LED__
\ No newline at end of file

diff -r 000000000000 -r 8f5db5085df7 MotionSensor.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionSensor.lib	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/components/code/MotionSensor/#4d6e28d4a18a

diff -r 000000000000 -r 8f5db5085df7 Motor/Motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,88 @@
+
+#include "Motor.hpp"
+
+#define PI 3.141592653589793238462
+#define PWM_PERIOD 13.5                 // ms
+#define BRAKE_CONSTANT 100              // Brake force(max = 100)
+#define BRAKE_WAIT 1.662                // seconds
+#define MINIMUM_VELOCITY 15
+
+Motor::Motor(PinName motor_pin): motor(motor_pin){}
+
+    
+void Motor::startJogging(float jog_dc, float jog_p){
+    jog_duty_cycle = jog_dc;
+    jog_period = jog_p;
+    interruption.attach(this,&Motor::motorJogging, jog_duty_cycle*jog_period);
+}
+    
+void Motor::stopJogging(void){
+    interruption.detach();
+    setMotorPWM(velocity,motor);
+}
+    
+void Motor::motorJogging(void) {
+    interruption.detach();
+    if(!alternate_motor){
+        setMotorPWM(velocity, motor);
+        interruption.attach(this,&Motor::motorJogging, jog_duty_cycle*jog_period);
+    }
+    else{
+         setMotorPWM(10, motor);
+         interruption.attach(this,&Motor::motorJogging, (1-jog_duty_cycle)*jog_period);
+    }
+    alternate_motor = !alternate_motor;
+}
+
+void Motor::brakeMotor(float brake_intensity, float brake_wait){
+    stopJogging();
+    if(velocity >= 0){
+        setMotorPWM(-brake_intensity, motor);
+        wait(brake_wait);
+        velocity = 0;
+        setMotorPWM(velocity,motor);
+    }
+    else {
+        setVelocity(0);
+    }
+}
+
+void Motor::reverseMotor(int speed){
+    for(int i=0 ; i >= -speed; i--){
+        setMotorPWM((float)i,motor);
+        wait_ms(13.5);    
+    }
+    for(int i=-speed ; i <= 0; i++){
+        setMotorPWM((float)i,motor);
+        wait_ms(13.5);    
+    }
+    for(int i=0 ; i >= -speed; i--){
+        setMotorPWM((float)i,motor);
+        wait_ms(13.5);    
+    }
+}
+
+void Motor::setSmoothVelocity(int new_velocity){
+    if( velocity > new_velocity){
+        for(; velocity >= new_velocity; velocity--){
+            setMotorPWM(velocity,motor);
+            wait_ms(PWM_PERIOD);
+            }
+        velocity++;
+    }
+    else if(velocity < new_velocity){
+        for(; velocity <= new_velocity; velocity++){
+            setMotorPWM(velocity,motor);
+            wait_ms(PWM_PERIOD);
+            }
+        velocity--;
+    }
+}
+
+void Motor::setVelocity(int new_velocity){
+ setMotorPWM(new_velocity,motor);
+ velocity = new_velocity;
+}
+float Motor::getVelocity(){
+    return velocity;
+}
\ No newline at end of file

diff -r 000000000000 -r 8f5db5085df7 Motor/Motor.hpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.hpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,32 @@
+#ifndef MOTOR_HEADER
+#define MOTOR_HEADER
+
+#include "mbed.h"
+#include "CarPWM.h"
+
+class Motor{
+    
+    protected:
+        Ticker interruption;
+        bool alternate_motor;
+        float velocity;
+        float jog_duty_cycle;
+        float jog_period;
+        PwmOut motor;
+    
+    public:
+        
+        void startJogging(float jog_dc, float jog_p);
+        void stopJogging(void);
+        void brakeMotor(float brake_intensity, float brake_wait);
+        void reverseMotor(int speed);
+        void setVelocity(int new_velocity);
+        float getVelocity();
+        void setSmoothVelocity(int new_velocity);
+        Motor(PinName motor_pin);
+        
+    private:
+         void motorJogging(void);
+     
+};
+#endif
\ No newline at end of file

diff -r 000000000000 -r 8f5db5085df7 PWM/CarPWM.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/CarPWM.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,38 @@
+#include "CarPWM.h"
+#include "mbed.h"
+
+#define MINIMUM_MOTOR_WIDTH 1160 // microseconds
+#define ZERO_MOTOR_WIDTH 1505    // microseconds
+#define MAXIMUM_MOTOR_WIDTH 1910 // microseconds
+#define MINIMUM_SERVO_WIDTH 1016 // microseconds
+#define ZERO_SERVO_WIDTH 1470    // microseconds
+#define MAXIMUM_SERVO_WIDTH 1858 // microseconds
+#define PERIOD 13500             // miliseconds
+#define MOTOR_RANGE_MAX 100
+#define SERVO_ANGLE_MAX 100
+
+void initialize(PwmOut motor, PwmOut servo){
+    motor.period_us(PERIOD);
+    servo.period_us(PERIOD);
+    }
+void setServoPWM(float angle, PwmOut servo){
+    if(angle > SERVO_ANGLE_MAX)
+        angle = SERVO_ANGLE_MAX;
+    else if(angle < -SERVO_ANGLE_MAX)
+        angle = - SERVO_ANGLE_MAX;
+    float servo_width = ZERO_SERVO_WIDTH + (angle/SERVO_ANGLE_MAX)*(MAXIMUM_SERVO_WIDTH - ZERO_SERVO_WIDTH); 
+    servo.pulsewidth_us(servo_width);
+   // Serial pc(USBTX, USBRX);
+   // pc.printf("\nServo: %f ",servo_width);
+}
+void setMotorPWM(float speed, PwmOut motor){
+    if(speed > MOTOR_RANGE_MAX)
+        speed = MOTOR_RANGE_MAX;
+    else if(speed < -MOTOR_RANGE_MAX)
+        speed = - MOTOR_RANGE_MAX;
+    float motor_speed = ZERO_MOTOR_WIDTH + (speed/MOTOR_RANGE_MAX)*(MAXIMUM_MOTOR_WIDTH - ZERO_MOTOR_WIDTH); 
+    motor.pulsewidth_us(motor_speed);
+  //  Serial pc(USBTX, USBRX);
+  //  pc.printf("Motor: %f ",motor_speed);
+
+}
\ No newline at end of file

diff -r 000000000000 -r 8f5db5085df7 PWM/CarPWM.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/CarPWM.h	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,10 @@
+#include "mbed.h"
+#ifndef CARPWM_H
+#define CARPWM_H
+
+void initialize(PwmOut motor, PwmOut servo);
+void setServoPWM(float angle, PwmOut servo);
+void setMotorPWM(float width, PwmOut motor);
+
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 8f5db5085df7 Protocol/Protocol/protocol.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/Protocol/protocol.h	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,154 @@
+/**
+@file protocol.h
+@brief Protocol definitions.
+*/
+
+/*
+Copyright 2016 Erik Perillo <erik.perillo@gmail.com>
+
+This file is part of piranha-ptc.
+
+This is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef __PIRANHA_PROTOCOL_H__
+#define __PIRANHA_PROTOCOL_H__
+
+//@{
+///PID parameters range.
+#define PID_PARAMS_MIN 0.0
+#define PID_PARAMS_MAX 100.0
+//@}
+
+//@{
+///Ground velocity range.
+#define GND_VEL_MIN -100.0
+#define GND_VEL_MAX 100.0
+//@}
+
+//@{
+///Angle reference range (in radians).
+#define PI 3.141593
+#define ABS_ANG_REF_MIN -PI
+#define ABS_ANG_REF_MAX PI
+//@}
+
+//@{
+///Angle reference from robot range (in radians).
+#define REL_ANG_REF_MIN -PI
+#define REL_ANG_REF_MAX PI
+//@}
+
+//@{
+///Angle reference from magnetometer range (in radians).
+#define MAG_ANG_REF_MIN -PI
+#define MAG_ANG_REF_MAX PI
+//@}
+
+//@{
+///Break intensity
+#define BRAKE_INTENSITY_MIN 0.0
+#define BRAKE_INTENSITY_MAX 100.0
+//@}
+
+//@{
+///Jogging speed ratio.
+#define BRAKE_PERIOD_MIN 0.0
+#define BRAKE_PERIOD_MAX 100.0
+//@}
+
+//@{
+///Jogging speed period (in seconds).
+#define JOG_VEL_PERIOD_MIN 0.0
+#define JOG_VEL_PERIOD_MAX 300.0
+//@}
+
+//@{
+///Jogging speed ratio.
+#define JOG_VEL_RATIO_MIN 0.0
+#define JOG_VEL_RATIO_MAX 1.0
+//@}
+
+//@{
+///Magnetometer calibration values.
+#define MAG_CALIB_MIN -750.0
+#define MAG_CALIB_MAX 750.0
+//@}
+
+///Messages to send via protocol.
+enum
+{
+	///Do nothing.
+	NONE,
+
+	///Brake the robot.
+	BRAKE,
+
+	///Reset gyroscope.
+	GYRO_ZERO,
+
+	///Set zero axis to current angle measure.
+	ANG_SET,
+
+	///Reset angle zero axis.
+	ANG_RST,
+
+	///Turn on leds
+	LED_ON,
+
+	///Turn off leds
+	LED_OFF,
+
+	///Set new angle reference relative to zero axis.
+	ABS_ANG_REF,
+
+	///Set new angle reference relative to robot axis.
+	REL_ANG_REF,
+
+	///Set new angle reference relative to north using magnetometer.
+	MAG_ANG_REF,
+
+	///Set new ground velocity for robot.
+	GND_VEL,
+
+	///Set new jogging speed for robot.
+	JOG_VEL,
+
+	///Magnetometer calibration (min_x, max_x, min_y, max_y).
+	MAG_CALIB,
+
+	///Send PID control parameters (P, I, D, N).
+	PID_PARAMS
+};
+
+#define MSG_HEADER_SIZE 1
+#define MSG_VAL_SIZE 2
+#define MSG_MAX_NUM_VALS 4
+#define MSG_BUF_LEN (MSG_HEADER_SIZE + MSG_VAL_SIZE*MSG_MAX_NUM_VALS)
+#define MSG_HEADER_IDX 0
+#define MSG_VALS_START_IDX (MSG_HEADER_IDX + 1)
+
+#define SENDER_PORT 7532
+#define SENDER_IFACE_ADDR "192.168.7.2"
+#define SENDER_NETMASK_ADDR "255.255.255.0"
+#define SENDER_GATEWAY_ADDR "0.0.0.0"
+
+#define RECEIVER_PORT 7533
+#define RECEIVER_IFACE_ADDR "192.168.7.3"
+#define RECEIVER_NETMASK_ADDR "255.255.255.0"
+#define RECEIVER_GATEWAY_ADDR "0.0.0.0"
+
+#endif
+

diff -r 000000000000 -r 8f5db5085df7 Protocol/Protocol/receiver.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/Protocol/receiver.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,159 @@
+/*
+Copyright 2016 Erik Perillo <erik.perillo@gmail.com>
+
+This file is part of piranha-ptc.
+
+This is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "receiver.h"
+#include "EthernetInterface.h"
+
+float Receiver::un_scale(uint16_t value, float min, float max)
+{
+	return ((float)value)/((1 << 16) - 1)*(max - min) + min;
+}
+
+uint8_t Receiver::get_header()
+{
+	return this->message[MSG_HEADER_IDX];
+}
+
+uint16_t Receiver::get_raw_val(int pos)
+{
+	uint16_t value = 0;
+
+	value |= this->message[MSG_VALS_START_IDX + 2*pos];
+	value |= this->message[MSG_VALS_START_IDX + 2*pos + 1] << 8;
+
+	return value;
+}
+
+float Receiver::get_val(float min, float max, int pos)
+{
+	uint16_t raw_val;
+
+	raw_val = this->get_raw_val(pos);
+	return this->un_scale(raw_val, min, max);
+}
+
+void Receiver::get_vals(float min, float max, float* vals, int size)
+{
+	uint16_t raw_val;
+
+	for(int i=0; i<size; i++)
+	{
+		raw_val = this->get_raw_val(i);
+		vals[i] = this->un_scale(raw_val, min, max);
+	}
+}
+
+bool Receiver::receive()
+{
+	return this->sock.receiveFrom(this->sender_addr, this->message, 
+		sizeof(this->message)) > 0;
+}
+
+Receiver::Receiver()
+{
+	;
+}
+
+Receiver::Receiver(Endpoint sender_addr, const UDPSocket& sock):
+	sock(sock), sender_addr(sender_addr)
+{
+	;
+}
+
+Receiver::Receiver(Endpoint sender_addr, int sock_port, int timeout):
+	sender_addr(sender_addr)
+{
+	this->sock.bind(sock_port);
+	this->sock.set_blocking(timeout < 0, timeout);
+}
+
+void Receiver::set_sender_addr(const Endpoint& sender_addr)
+{
+	this->sender_addr = sender_addr;
+}
+
+void Receiver::set_socket(const UDPSocket& sock)
+{
+	this->sock = sock;
+}
+
+void Receiver::set_socket(int port, int timeout)
+{
+	this->sock.bind(port);
+	this->sock.set_blocking(timeout < 0, timeout);
+}
+
+Endpoint Receiver::get_sender_addr()
+{
+	return this->sender_addr;
+}
+
+UDPSocket Receiver::get_socket()
+{
+	return this->sock;
+}
+
+uint8_t Receiver::get_msg()
+{
+	return this->message[MSG_HEADER_IDX];
+}
+
+float Receiver::get_abs_ang_ref()
+{
+	return this->get_val(ABS_ANG_REF_MIN, ABS_ANG_REF_MAX);
+}
+
+float Receiver::get_rel_ang_ref()
+{
+	return this->get_val(REL_ANG_REF_MIN, REL_ANG_REF_MAX);
+}
+
+float Receiver::get_mag_ang_ref()
+{
+	return this->get_val(MAG_ANG_REF_MIN, MAG_ANG_REF_MAX);
+}
+
+float Receiver::get_gnd_vel()
+{
+	return this->get_val(GND_VEL_MIN, GND_VEL_MAX);
+}
+
+void Receiver::get_brake(float* intensity, float* period)
+{
+	*intensity = this->get_val(BRAKE_INTENSITY_MIN, BRAKE_INTENSITY_MAX);
+	*period = this->get_val(BRAKE_PERIOD_MIN, BRAKE_PERIOD_MAX, 1);
+}
+
+void Receiver::get_jog_vel(float* period, float* ratio)
+{
+	*period = this->get_val(JOG_VEL_PERIOD_MIN, JOG_VEL_PERIOD_MAX);
+	*ratio = this->get_val(JOG_VEL_RATIO_MIN, JOG_VEL_RATIO_MAX, 1);
+}
+
+void Receiver::get_pid_params(float* params)
+{
+	this->get_vals(PID_PARAMS_MIN, PID_PARAMS_MAX, params, 4);
+}
+
+void Receiver::get_mag_calib(float* params)
+{
+	this->get_vals(MAG_CALIB_MIN, MAG_CALIB_MAX, params, 4);
+}
+

diff -r 000000000000 -r 8f5db5085df7 Protocol/Protocol/receiver.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/Protocol/receiver.h	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,77 @@
+/**
+@file receiver.h
+@brief Receiver side functions declarations.
+*/
+
+/*
+Copyright 2016 Erik Perillo <erik.perillo@gmail.com>
+
+This file is part of piranha-ptc.
+
+This is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef __PIRANHA_RCV_PROTOCOL_H__
+#define __PIRANHA_RCV_PROTOCOL_H__
+
+#include "protocol.h"
+#include "mbed.h"
+#include "EthernetInterface.h"
+
+#define TEST
+
+class Receiver
+{
+        
+	#ifdef TEST
+	public:
+	#else
+	protected:
+	#endif
+	UDPSocket sock;
+	char message[MSG_BUF_LEN];
+	Endpoint sender_addr;	
+
+	float un_scale(uint16_t value, float min, float max);
+	uint8_t get_header();
+	uint16_t get_raw_val(int pos=0);
+	float get_val(float min, float max, int pos=0);
+	void get_vals(float min, float max, float* vals, int size);
+
+	public:
+	Receiver();
+	Receiver(Endpoint sender_addr, const UDPSocket& sock);
+	Receiver(Endpoint sender_addr, int sock_port=RECEIVER_PORT, int timeout=1);
+
+	void set_sender_addr(const Endpoint& sender_addr);
+	void set_socket(const UDPSocket& sock);
+	void set_socket(int port=RECEIVER_PORT, int timeout=1);
+	Endpoint get_sender_addr();
+	UDPSocket get_socket();
+
+	bool receive();
+	uint8_t get_msg();
+	float get_abs_ang_ref();
+	float get_rel_ang_ref();
+	float get_mag_ang_ref();
+	float get_gnd_vel();
+	void get_brake(float* intensity, float* period);
+	void get_jog_vel(float* period, float* ratio);
+	void get_pid_params(float* params);
+	void get_mag_calib(float* params);
+};
+
+#endif
+

diff -r 000000000000 -r 8f5db5085df7 SensorsLibrary/FXAS21002.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXAS21002.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,106 @@
+ /* Copyright (c) 2015 NXP Semiconductors. MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "FXAS21002.h"
+#include "mbed.h"
+FXAS21002::FXAS21002(PinName sda, PinName scl) : gyroi2c(sda,scl)
+{
+   
+}
+
+void FXAS21002::set_gyro(gyro_mode mode) // Protected method
+{ 
+    char d[2];
+    d[0] = FXAS21002_CTRL_REG1;                       //Puts device in standby mode
+    d[1] = 0x08;
+    gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2);
+
+    d[0] = FXAS21002_CTRL_REG0;                       //Sets FSR and Sensitivity
+    d[1] = mode + 0x80;
+    gyroi2c.write(FXAS21002_I2C_ADDRESS, d, 2);
+
+    d[0] = FXAS21002_CTRL_REG1;                       //Puts device in active mode
+    d[1] = 0x0A;
+    gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2);
+}
+
+
+void FXAS21002::stop_measure(void)
+{
+    interrupt.detach(); 
+    angle = 0;   
+}
+
+float FXAS21002::get_angle(void)
+{
+    return angle;
+}
+
+void FXAS21002::integrate_gyro_angle(void)
+{
+    float gyro_data[3];
+    acquire_gyro_data_dps(gyro_data);
+    
+    angle = angle + (gyro_data[2]-GYRO_OFFSET)*(period/1000000);
+    if(angle > 180)
+        angle = angle - 360;
+    if(angle < -180)
+        angle = angle + 360;
+}
+
+void FXAS21002::start_measure(float period_us)
+{
+    period = period_us;
+    interrupt.attach_us(this,&FXAS21002::integrate_gyro_angle,period);
+    angle = 0;
+}
+
+void FXAS21002::gyro_config(void)
+{
+    set_gyro(MODE_2);           //Default implementation
+    sensitivity = 0.03125;
+}
+ 
+void FXAS21002::gyro_config(gyro_mode mode)
+{
+    set_gyro(mode);
+    switch(mode)
+    {
+        case MODE_1: sensitivity = 0.0625; break;
+        case MODE_2: sensitivity = 0.03125; break;
+        case MODE_3: sensitivity = 0.015625; break;
+        case MODE_4: sensitivity = 0.0078125; break;
+    }
+}
+
+void FXAS21002::acquire_gyro_data_dps(float * g_data)
+{
+  
+    char data_bytes[7];
+    gyroi2c.write(FXAS21002_I2C_ADDRESS,FXAS21002_STATUS,1,true);  // Read the 6 data bytes - LSB and MSB for X, Y and Z Axes.
+    gyroi2c.read(FXAS21002_I2C_ADDRESS,data_bytes,7);
+
+    g_data[0] =  (float)((int16_t)((data_bytes[1]*256) + (data_bytes[2]))) * sensitivity;
+    g_data[1] =  (float)((int16_t)((data_bytes[3]*256) + (data_bytes[4]))) * sensitivity;
+    g_data[2] =  (float)((int16_t)((data_bytes[5]*256) + (data_bytes[6]))) * sensitivity;
+   
+}
+
+
+     
+     
\ No newline at end of file

diff -r 000000000000 -r 8f5db5085df7 SensorsLibrary/FXAS21002.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXAS21002.h	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,71 @@
+ /* Copyright (c) 2015 NXP Semiconductors. MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef FXAS21002_H
+#define FXAS21002_H
+#include "mbed.h"
+
+#define FXAS21002_I2C_ADDRESS 0x40
+
+#define FXAS21002_STATUS 0x00
+#define FXAS21002_WHO_AM_I 0x0C
+#define FXAS21002_CTRL_REG0 0x0D
+#define FXAS21002_CTRL_REG1 0x13
+#define FXAS21002_WHO_AM_I_VALUE 0xD1
+#define GYRO_OFFSET -0.239 //-0.15 //-0.2396875
+/*  Gyroscope mechanical modes
+*   Mode    Full-scale range [Deg/s]    Sensitivity [(mDeg/s)/LSB]
+*   1       +- 2000                     62.5
+*   2       +- 1000                     31.25
+*   3       +- 500                      15.625
+*   4       +- 250                      7.8125
+*/
+enum gyro_mode
+{
+    MODE_1 = 0x00, 
+    MODE_2 = 0x01,
+    MODE_3 = 0x02,
+    MODE_4 = 0x03
+};
+
+class FXAS21002
+{
+    public:
+    
+    FXAS21002(PinName sda, PinName scl);
+      
+    void gyro_config(void);
+    void gyro_config(gyro_mode mode);
+        
+    void acquire_gyro_data_dps(float * du);
+    void start_measure(float period_us);
+    void stop_measure(void);
+    float get_angle(void);
+    
+    private:
+    void set_gyro(gyro_mode mode);
+    void integrate_gyro_angle(void);
+    Ticker interrupt;
+    I2C gyroi2c;
+    float sensitivity;
+    float angle;
+    float period;
+    
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 8f5db5085df7 SensorsLibrary/FXOS8700Q.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXOS8700Q.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,294 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "FXOS8700Q.h"
+#define UINT14_MAX        16383
+
+
+FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
+    // activate the peripheral
+    uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+    m_i2c.frequency(400000);
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG1;
+    data[1] = 0x1F;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG2;
+    data[1] = 0x20;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_XYZ_DATA_CFG;
+    data[1] = 0x00;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_CTRL_REG1;
+    data[1] = 0x18;//0x1D;
+    writeRegs(data, 2);
+}
+
+FXOS8700Q_acc::~FXOS8700Q_acc() { }
+
+void FXOS8700Q_acc::enable(void) {
+    uint8_t data[2];
+    readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+    data[1] |= 0x01;
+    data[0] = FXOS8700Q_CTRL_REG1;
+    writeRegs(data, 2);
+}
+
+void FXOS8700Q_acc::disable(void) {
+    uint8_t data[2];
+    readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+    data[1] &= 0xFE;
+    data[0] = FXOS8700Q_CTRL_REG1;
+    writeRegs(data, 2);
+}
+
+
+
+uint32_t FXOS8700Q_acc::whoAmI() {
+    uint8_t who_am_i = 0;
+    readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
+    return (uint32_t) who_am_i;
+}
+
+uint32_t FXOS8700Q_acc::dataReady(void) {
+    uint8_t stat = 0;
+    readRegs(FXOS8700Q_STATUS, &stat, 1);
+    return (uint32_t) stat;
+}
+
+uint32_t FXOS8700Q_acc::sampleRate(uint32_t f) {
+    return(50); // for now sample rate is fixed at 50Hz
+}
+
+void FXOS8700Q_acc::getX(float * x) {
+    *x = (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0f);
+}
+
+void FXOS8700Q_acc::getY(float * y) {
+    *y = (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0f);
+}
+
+void FXOS8700Q_acc::getZ(float * z) {
+    *z = (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0f);
+}
+
+void FXOS8700Q_acc::getX(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_OUT_X_MSB);
+}
+
+void FXOS8700Q_acc::getY(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_OUT_Y_MSB);
+}
+
+void FXOS8700Q_acc::getZ(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_OUT_Z_MSB);
+}
+
+
+void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) {
+    int16_t acc, t[3];
+    uint8_t res[6];
+   readRegs(FXOS8700Q_OUT_X_MSB, res, 6);
+
+    acc = (res[0] << 6) | (res[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    t[0] = acc;
+    acc = (res[2] << 6) | (res[3] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    t[1] = acc;
+    acc = (res[4] << 6) | (res[5] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    t[2] = acc;
+    data.x = ((float) t[0]) / 4096.0f;
+    data.y = ((float) t[1]) / 4096.0f;
+    data.z = ((float) t[2]) / 4096.0f;
+}
+
+
+void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) {
+    int16_t acc;
+    uint8_t res[6];
+    readRegs(FXOS8700Q_OUT_X_MSB, res, 6);
+
+    acc = (res[0] << 6) | (res[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    data.x = acc;
+    acc = (res[2] << 6) | (res[3] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    data.y = acc;
+    acc = (res[4] << 6) | (res[5] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    data.z = acc;
+}
+
+void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) {
+    char t[1] = {addr};
+    m_i2c.write(m_addr, t, 1, true);
+    m_i2c.read(m_addr, (char *)data, len);
+}
+
+void FXOS8700Q_acc::writeRegs(uint8_t * data, int len) {
+    m_i2c.write(m_addr, (char *)data, len);
+}
+
+
+int16_t FXOS8700Q_acc::getAccAxis(uint8_t addr) {
+    int16_t acc;
+    uint8_t res[2];
+    readRegs(addr, res, 2);
+
+    acc = (res[0] << 6) | (res[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+
+    return acc;
+}
+
+
+
+FXOS8700Q_mag::FXOS8700Q_mag(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
+    // activate the peripheral
+    uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+    m_i2c.frequency(400000);
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG1;
+    data[1] = 0x1F;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG2;
+    data[1] = 0x20;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_XYZ_DATA_CFG;
+    data[1] = 0x00;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_CTRL_REG1;
+    data[1] = 0x18;//0x1D;
+    writeRegs(data, 2);
+}
+
+FXOS8700Q_mag::~FXOS8700Q_mag() { }
+
+void FXOS8700Q_mag::enable(void) {
+    uint8_t data[2];
+    readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+    data[1] |= 0x01;
+    data[0] = FXOS8700Q_CTRL_REG1;
+    writeRegs(data, 2);
+}
+
+void FXOS8700Q_mag::disable(void) {
+    uint8_t data[2];
+    readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+    data[1] &= 0xFE;
+    data[0] = FXOS8700Q_CTRL_REG1;
+    writeRegs(data, 2);
+}
+
+
+
+uint32_t FXOS8700Q_mag::whoAmI() {
+    uint8_t who_am_i = 0;
+    readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
+    return (uint32_t) who_am_i;
+}
+
+uint32_t FXOS8700Q_mag::dataReady(void) {
+    uint8_t stat = 0;
+    readRegs(FXOS8700Q_STATUS, &stat, 1);
+    return (uint32_t) stat;
+}
+
+uint32_t FXOS8700Q_mag::sampleRate(uint32_t f) {
+    return(50); // for now sample rate is fixed at 50Hz
+}
+
+void FXOS8700Q_mag::getX(float * x) {
+    *x = (float(getAccAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f);
+}
+
+void FXOS8700Q_mag::getY(float * y) {
+    *y = (float(getAccAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f);
+}
+
+void FXOS8700Q_mag::getZ(float * z) {
+    *z = (float(getAccAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f);
+}
+
+void FXOS8700Q_mag::getX(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_M_OUT_X_MSB);
+}
+
+void FXOS8700Q_mag::getY(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_M_OUT_Y_MSB);
+}
+
+void FXOS8700Q_mag::getZ(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_M_OUT_Z_MSB);
+}
+
+
+void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) {
+    int16_t t[3];
+    uint8_t res[6];
+   readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
+
+    t[0] = (res[0] << 8) | res[1];
+    t[1] = (res[2] << 8) | res[3];
+    t[2] = (res[4] << 8) | res[5];
+
+    data.x = ((float) t[0]) * 0.1f;
+    data.y = ((float) t[1]) * 0.1f;
+    data.z = ((float) t[2]) * 0.1f;
+}
+
+
+void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) {
+    int16_t acc;
+    uint8_t res[6];
+    readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
+
+    data.x = (res[0] << 8) | res[1];
+    data.y = (res[2] << 8) | res[3];
+    data.z = (res[4] << 8) | res[5];
+}
+
+void FXOS8700Q_mag::readRegs(int addr, uint8_t * data, int len) {
+    char t[1] = {addr};
+    m_i2c.write(m_addr, t, 1, true);
+    m_i2c.read(m_addr, (char *)data, len);
+}
+
+void FXOS8700Q_mag::writeRegs(uint8_t * data, int len) {
+    m_i2c.write(m_addr, (char *)data, len);
+}
+
+
+int16_t FXOS8700Q_mag::getAccAxis(uint8_t addr) {
+    int16_t acc;
+    uint8_t res[2];
+    readRegs(addr, res, 2);
+
+    acc = (res[0] << 8) | res[1];
+
+    return acc;
+}

diff -r 000000000000 -r 8f5db5085df7 SensorsLibrary/FXOS8700Q.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXOS8700Q.h	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,148 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef FXOS8700Q_H
+#define FXOS8700Q_H
+
+#include "mbed.h"
+#include "MotionSensor.h"
+// FXOS8700CQ I2C address
+#define FXOS8700CQ_SLAVE_ADDR0 (0x1E<<1) // with pins SA0=0, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR1 (0x1D<<1) // with pins SA0=1, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR2 (0x1C<<1) // with pins SA0=0, SA1=1
+#define FXOS8700CQ_SLAVE_ADDR3 (0x1F<<1) // with pins SA0=1, SA1=1
+// FXOS8700CQ internal register addresses
+#define FXOS8700Q_STATUS 0x00
+#define FXOS8700Q_OUT_X_MSB 0x01
+#define FXOS8700Q_OUT_Y_MSB 0x03
+#define FXOS8700Q_OUT_Z_MSB 0x05
+#define FXOS8700Q_M_OUT_X_MSB 0x33
+#define FXOS8700Q_M_OUT_Y_MSB 0x35
+#define FXOS8700Q_M_OUT_Z_MSB 0x37
+#define FXOS8700Q_WHOAMI 0x0D
+#define FXOS8700Q_XYZ_DATA_CFG 0x0E
+#define FXOS8700Q_CTRL_REG1 0x2A
+#define FXOS8700Q_M_CTRL_REG1 0x5B
+#define FXOS8700Q_M_CTRL_REG2 0x5C
+#define FXOS8700Q_WHOAMI_VAL 0xC7
+
+
+/**
+* MMA8451Q accelerometer example
+*
+* @code
+* #include "mbed.h"
+* #include "FXOS8700Q.h"
+* 
+* 
+* int main(void) {
+* 
+* FXOS8700Q combo( A4, A5, FXOS8700Q_I2C_ADDRESS0);
+* PwmOut rled(LED_RED);
+* PwmOut gled(LED_GREEN);
+* PwmOut bled(LED_BLUE);
+* 
+*     while (true) {       
+*         rled1.0 - combo(acc.getAccX());
+*         gled1.0 - combo(acc.getAccY());
+*         bled1.0 - combo(acc.getAccZ());
+*         wait(0.1);
+*     }
+* }
+* @endcode
+*/
+
+class FXOS8700Q_acc : public MotionSensor
+{
+public:
+  /**
+  * FXOS8700Q constructor
+  *
+  * @param sda SDA pin
+  * @param sdl SCL pin
+  * @param addr addr of the I2C peripheral
+  */
+  
+  FXOS8700Q_acc(PinName sda, PinName scl, int addr);
+
+  /**
+  * FXOS8700Q destructor
+  */
+  ~FXOS8700Q_acc();
+
+    void enable(void);
+    void disable(void);
+    uint32_t sampleRate(uint32_t frequency);
+    uint32_t whoAmI(void);
+    uint32_t dataReady(void);
+    void getX(int16_t * x);
+    void getY(int16_t * y);
+    void getZ(int16_t * z);
+    void getX(float * x);
+    void getY(float * y);
+    void getZ(float * z);
+    void getAxis(MotionSensorDataCounts &data);
+    void getAxis(MotionSensorDataUnits &data);
+  
+  void readRegs(int addr, uint8_t * data, int len);
+  
+private:
+  I2C m_i2c;
+  int m_addr;
+
+  void writeRegs(uint8_t * data, int len);
+  int16_t getAccAxis(uint8_t addr);
+
+};
+
+class FXOS8700Q_mag : public MotionSensor
+{
+public:
+  FXOS8700Q_mag(PinName sda, PinName scl, int addr);
+
+  /**
+  * FXOS8700Q destructor
+  */
+  ~FXOS8700Q_mag();
+
+    void enable(void);
+    void disable(void);
+    uint32_t sampleRate(uint32_t fequency);
+    uint32_t whoAmI(void);
+    uint32_t dataReady(void);
+    void getX(int16_t * x);
+    void getY(int16_t * y);
+    void getZ(int16_t * z);
+    void getX(float * x);
+    void getY(float * y);
+    void getZ(float * z);
+    void getAxis(MotionSensorDataCounts &data);
+    void getAxis(MotionSensorDataUnits &data);
+  
+  void readRegs(int addr, uint8_t * data, int len);
+  
+private:
+  I2C m_i2c;
+  int m_addr;
+
+  void writeRegs(uint8_t * data, int len);
+  int16_t getAccAxis(uint8_t addr);
+
+};
+
+#endif

diff -r 000000000000 -r 8f5db5085df7 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,249 @@
+#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;
+}

diff -r 000000000000 -r 8f5db5085df7 mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#162b12aea5f2

diff -r 000000000000 -r 8f5db5085df7 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/99a22ba036c9
\ No newline at end of file