Revision 0:88faaa1afb83, committed 2016-04-11

Comitter:

drelliak

Date:

Mon Apr 11 05:20:40 2016 +0000

Child:

1:3f923c2862c9

Commit message:

Trekking Controller

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionSensor.lib	Mon Apr 11 05:20:40 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/components/code/MotionSensor/#4d6e28d4a18a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/CarPWM.cpp	Mon Apr 11 05:20:40 2016 +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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/CarPWM.h	Mon Apr 11 05:20:40 2016 +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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/protocol.h	Mon Apr 11 05:20:40 2016 +0000
@@ -0,0 +1,55 @@
+/*
+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__
+
+#define PI 3.141593
+
+//pid values range
+#define PID_PARAMS_MIN -100.0
+#define PID_PARAMS_MAX 100.0
+
+//ground speed range
+#define GND_SPEED_MIN -100.0
+#define GND_SPEED_MAX 100.0
+
+//angle reference range (in radians)
+#define ANG_REF_MIN -PI
+#define ANG_REF_MAX PI
+
+//messages to send via protocol
+enum
+{
+    //do nothing
+    NONE,
+    //brake the robot
+    BRAKE,
+    //reset angle measure
+    ANG_RST,
+    //set new angle reference
+    ANG_REF,
+    //set new ground speed for robot
+    GND_SPEED,
+    //send pid control parameters
+    PID_PARAMS
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/receiver.cpp	Mon Apr 11 05:20:40 2016 +0000
@@ -0,0 +1,63 @@
+/*
+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"
+
+
+uint16_t read(Serial& serial)
+{
+	int i;
+	uint16_t value = 0;
+	char chr;
+	
+	for(i=0; i<2; i++)
+		while(true)
+			if(serial.readable())
+			{
+				chr = serial.getc();
+				value = value | chr << i*8;	
+				break;
+			}
+
+	return value;
+}
+
+float un_scale(uint16_t value, float min, float max)
+{
+	return ((float)value)/((1 << 16) - 1)*(max - min) + min;
+}
+
+float get_param(Serial& serial, float min, float max)
+{
+	uint16_t value;
+	
+	value = read(serial);
+
+	return un_scale(value, min, max);
+}
+
+void get_pid_params(Serial& serial, float* kp, float* ki, float* kd, float* n)
+{
+	*kp = get_pid_param(serial);
+	*ki = get_pid_param(serial);
+	*kd = get_pid_param(serial);
+	*n = get_pid_param(serial);
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/receiver.h	Mon Apr 11 05:20:40 2016 +0000
@@ -0,0 +1,56 @@
+/*
+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"
+
+
+/**
+ Reads serial and converts a string of (one or two) bytes into an unsigned int.
+ Least-significant bytes must come first.
+ Assumes big-endian representation of numbers on architecture.
+*/
+uint16_t read(Serial& serial);
+
+/**
+ Converts unsigned int into float following unit-normalization rules.
+*/
+float un_scale(uint16_t value, float min, float max);
+
+/**
+ Applies un_scale to value read on serial. 
+*/
+float get_param(Serial& serial, float min, float max);
+
+/**
+ Gets all 4 pid parameters from serial stream.
+*/
+void get_pid_params(Serial& serial, float* kp, float* ki, float* kd, float* n);
+
+//macros just to make life easier
+#define get_pid_param(serial) get_param(serial, PID_PARAMS_MIN, PID_PARAMS_MAX)
+#define get_gnd_speed(serial) get_param(serial, GND_SPEED_MIN, GND_SPEED_MAX)
+#define get_ang_ref(serial) get_param(serial, ANG_REF_MIN, ANG_REF_MAX)
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXAS21002.cpp	Mon Apr 11 05:20:40 2016 +0000
@@ -0,0 +1,61 @@
+ /* 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::gyro_config(void)
+ {
+   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 FS =+/- 2000 dps
+   d[1] = 0x01;
+   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::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]))) * 0.03125;//0.0625;
+   g_data[1] =  (float)((int16_t)((data_bytes[3]*256) + (data_bytes[4]))) * 0.03125;//0.0625;
+   g_data[2] =  (float)((int16_t)((data_bytes[5]*256) + (data_bytes[6]))) * 0.03125;//0.0625;
+   
+ }
+
+
+     
+     
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXAS21002.h	Mon Apr 11 05:20:40 2016 +0000
@@ -0,0 +1,46 @@
+ /* 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
+
+class FXAS21002
+{
+    public:
+    
+    FXAS21002(PinName sda, PinName scl);
+      
+    void gyro_config(void);
+        
+    void acquire_gyro_data_dps(float * du);
+    
+    private:
+    I2C gyroi2c;
+    
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXOS8700Q.cpp	Mon Apr 11 05:20:40 2016 +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;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXOS8700Q.h	Mon Apr 11 05:20:40 2016 +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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 11 05:20:40 2016 +0000
@@ -0,0 +1,231 @@
+#include "FXAS21002.h"
+#include "FXOS8700Q.h"  
+#include "mbed.h"
+#include "CarPWM.h"
+#include "receiver.h"
+
+
+#define PI 3.141592653589793238462
+#define Ts 0.02                         // Seconds
+#define INITIAL_P 0.452531214933414
+#define INITIAL_I 5.45748932024049
+#define INITIAL_D 0.000233453623255507
+#define INITIAL_N 51.0605584484153
+#define GYRO_OFFSET 0.0152
+#define END_THRESH 4
+#define START_THRESH 10
+#define MINIMUM_VELOCITY 15
+Serial ser(USBTX, USBRX);    // Initialize Serial port
+PwmOut motor(PTD1);         // Motor connected to pin PTD1
+PwmOut servo(PTD3);         // Servo connected to pin PTD3
+
+FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
+FXAS21002 gyro(PTE25,PTE24);
+
+
+
+// PID controller parameters and functions
+float e[2], u, up[1],ui[2], ud[2]; // The vector coeficient means a time delay, for exemple e[a] = e(k-a) -> z^(-a)e(k)
+float P, I, D, N, reference = 0;
+void controlAnglePID(float P, float I, float D, float N);
+void initializeController();
+
+// Gyroscope variables and functions
+float gyro_data[3], gyro_angle;
+Timer t; 
+void processGyroAngle();
+void startGyro();
+void stopGyro();
+
+// Magnetometer variables and functions
+float max_x, max_y, min_x, min_y,x,y;
+MotionSensorDataUnits mag_data;
+float processMagAngle();
+void magCal();
+
+// State variables
+float sensor, velocity;
+void readProtocol();
+void brakeMotor();
+// Test functions
+void debug();
+
+int main(){
+    // Initializing serial communication
+    ser.baud(9600);
+    ser.format(8, SerialBase::None, 1);
+    // Initializing controller
+    printf("Initializing controller....\r\n\r\n");
+    initializeController();
+    printf("Controller Initialized. \r\n");
+    // Calibrating magnetometer and setting the initial position
+    magCal();
+    gyro_angle = processMagAngle();
+    // Start moving the robot and integrating the gyroscope
+    velocity = MINIMUM_VELOCITY;
+    setMotorPWM(velocity,motor);
+    startGyro();
+    // main loop
+        while (true){
+        processGyroAngle();
+        controlAnglePID(P,I,D,N);
+        //debug();
+        if(t.read_us() < Ts*1000000)
+            wait_us(Ts*1000000 - t.read_us());
+        if(ser.readable())
+            readProtocol();
+        }
+}
+
+void readProtocol(){
+    char msg = ser.getc();
+    switch(msg)
+    {
+        case NONE:
+            //ser.printf("sending red signal to led\r\n");
+            return;
+            break;          
+        case BRAKE:
+            //ser.printf("sending green signal to led\r\n");
+            brakeMotor();
+            break;
+        case ANG_RST:
+            //ser.printf("sending blue signal to led\r\n");
+            stopGyro();
+            gyro_angle = 0;
+            startGyro();
+            return;
+            break;
+        case ANG_REF:
+            reference = get_ang_ref(ser);     
+            break;
+        case GND_SPEED:
+            velocity = get_gnd_speed(ser);       
+            setMotorPWM(velocity,motor);
+            break;
+        case PID_PARAMS:
+            ser.putc('p');
+            get_pid_params(ser, &P, &I, &D, &N);     
+            break;
+        default:
+           // ser.flush();
+
+    }
+}
+/* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0.          */
+void initializeController(){
+    for(int i =0; i<2; i++){
+        e[i] = 0;
+        ui[i] = 0;
+        ud[i] = 0;
+    }
+    P= INITIAL_P;
+    I= INITIAL_I;
+    D= INITIAL_D;
+    N= INITIAL_N;
+}
+
+/* Start the Gyroscope timer and set the initial configuration */
+void startGyro(){
+    gyro.gyro_config(); 
+    t.start();
+}
+
+/* Stop and reset the Gyroscope */
+void stopGyro(){
+    t.stop();
+    t.reset();
+    gyro_angle = 0;
+}
+
+/* Integrate the Gyroscope to get the angular position (Deegre/seconds) */
+void processGyroAngle(){
+    gyro.acquire_gyro_data_dps(gyro_data);
+    t.stop();
+    gyro_angle = gyro_angle + (gyro_data[2] + GYRO_OFFSET)*(double)(t.read_us())/1000000;
+    t.reset();
+    t.start();
+    sensor = gyro_angle;
+    if(sensor > 180)
+        sensor = sensor - 360;
+    if(sensor < -180)
+        sensor = sensor + 360;
+}
+
+/* PID controller for angular position */
+void controlAnglePID(float P, float I, float D, float N){ 
+    /* Getting error */
+    e[1] = e[0];
+    e[0] = reference - (sensor*PI/180);
+    if(e[0] > PI)
+      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); 
+    /** Controller **/ 
+    u = up[0] + ud[0] + ui[0];
+    setServoPWM(u*100/(PI/8), servo);
+}
+/* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
+void brakeMotor(){
+    if(velocity > 0)
+        setMotorPWM(-30, motor);
+    else if(velocity < 0)
+        setMotorPWM(30, motor);
+    wait(0.5);
+    setMotorPWM(0,motor);
+}
+/* Debug functions that prints the sensor and control inputs values. Since it's time consuming it should not be used    */
+/* in the main loop or the controller performance may be affected.                                                      */
+void debug(){
+    //printf("ERROR: %f Up: %f Ui: %f Ud: %f U: %f \r\n", e[0]*180/3.1415, up[0]*100/(3.1415/8), ui[0]*100/(3.1415/8), ud[0]*100/(3.1415/8),u*100/(3.1415/8));
+    //printf("Erro: %f  Sensor: %f Magnetometer: %f \r\n",e[0]*180/PI,sensor,processMagAngle(0)*180/PI);
+    printf(" %f \r\n",sensor);
+}
+
+/* Function to normalize the magnetometer reading */
+void magCal(){
+        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(){
+    mag.getAxis(mag_data);
+    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;
+    return atan2(y,x);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Apr 11 05:20:40 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/99a22ba036c9
\ No newline at end of file