Quadcopter working with accelerômeter and accelerometer, and bluetooth radio for communication

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
jose_claudiojr
Date:
Tue May 21 14:12:13 2013 +0000
Commit message:
Quadcopter code with accelerometer and gyroscope.

Changed in this revision

Controllers/Fuzzy/FuzzyController.cpp Show annotated file Show diff for this revision Revisions of this file
Controllers/Fuzzy/FuzzyController.h Show annotated file Show diff for this revision Revisions of this file
Controllers/Fuzzy/FuzzyIO.cpp Show annotated file Show diff for this revision Revisions of this file
Controllers/Fuzzy/FuzzyIO.h Show annotated file Show diff for this revision Revisions of this file
Controllers/Fuzzy/MembershipFunction.cpp Show annotated file Show diff for this revision Revisions of this file
Controllers/Fuzzy/MembershipFunction.h Show annotated file Show diff for this revision Revisions of this file
Controllers/PID.cpp Show annotated file Show diff for this revision Revisions of this file
Controllers/PID.h Show annotated file Show diff for this revision Revisions of this file
Filters/ComplementaryFilter.cpp Show annotated file Show diff for this revision Revisions of this file
Filters/ComplementaryFilter.h Show annotated file Show diff for this revision Revisions of this file
Filters/Kalman.cpp Show annotated file Show diff for this revision Revisions of this file
Filters/Kalman.h Show annotated file Show diff for this revision Revisions of this file
Filters/KalmanFilter.cpp Show annotated file Show diff for this revision Revisions of this file
Filters/KalmanFilter.h Show annotated file Show diff for this revision Revisions of this file
GetTickCount/GetTickCount.cpp Show annotated file Show diff for this revision Revisions of this file
GetTickCount/GetTickCount.h Show annotated file Show diff for this revision Revisions of this file
Hardware/Motor.cpp Show annotated file Show diff for this revision Revisions of this file
Hardware/Motor.h Show annotated file Show diff for this revision Revisions of this file
Sensors/ADXL345.cpp Show annotated file Show diff for this revision Revisions of this file
Sensors/ADXL345.h Show annotated file Show diff for this revision Revisions of this file
Sensors/Accelerometer.cpp Show annotated file Show diff for this revision Revisions of this file
Sensors/Accelerometer.h Show annotated file Show diff for this revision Revisions of this file
Sensors/Gyroscope.cpp Show annotated file Show diff for this revision Revisions of this file
Sensors/Gyroscope.h Show annotated file Show diff for this revision Revisions of this file
Sensors/L3G4200D.cpp Show annotated file Show diff for this revision Revisions of this file
Sensors/L3G4200D.h Show annotated file Show diff for this revision Revisions of this file
Serial/SerialBuffered.cpp Show annotated file Show diff for this revision Revisions of this file
Serial/SerialBuffered.h Show annotated file Show diff for this revision Revisions of this file
Util.cpp Show annotated file Show diff for this revision Revisions of this file
Util.h Show annotated file Show diff for this revision Revisions of this file
Watchdog/Watchdog.cpp Show annotated file Show diff for this revision Revisions of this file
Watchdog/Watchdog.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
stddef.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 56b8c86181b1 Controllers/Fuzzy/FuzzyController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controllers/Fuzzy/FuzzyController.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,100 @@
+#include "FuzzyController.h"
+
+FuzzyController::FuzzyController(void)
+{
+    inputs = NULL;
+    outputs = NULL;
+
+    inputsLenght = 0;
+    outputsLenght = 0;
+
+    rulesAmount = 0;
+    mfsIndexs = NULL;
+
+    outputsValues = NULL;
+}
+
+FuzzyController::~FuzzyController(void)
+{
+}
+
+void FuzzyController::addInput(FuzzyIO* input)
+{
+    inputs = (FuzzyIO**) realloc(inputs, sizeof(FuzzyIO*) * (inputsLenght + 1));
+    
+    inputs[inputsLenght] = input;
+    
+    inputsLenght++;
+}
+
+void FuzzyController::addOutput(FuzzyIO* output)
+{
+    outputs = (FuzzyIO**) realloc(outputs, sizeof(FuzzyIO*) * (outputsLenght + 1));
+    
+    outputs[outputsLenght] = output;
+    
+    outputsLenght++;
+}
+
+void FuzzyController::addRules(int rulesAmount, int** mfsIndexs)
+{
+    this->rulesAmount = rulesAmount;
+    this->mfsIndexs = mfsIndexs;
+}
+
+float* FuzzyController::compute(float* values)
+{
+    if (outputsValues == NULL)
+    {
+        inputsValues = new float[inputsLenght];
+        outputsValues = new float[outputsLenght];
+
+        num = new float[outputsLenght];
+        den = new float[outputsLenght];
+    }
+
+    for (int i = 0; i < outputsLenght; i++)
+    {
+        num[i] = 0;
+        den[i] = 0;
+    }
+
+    for (int i = 0; i < rulesAmount; i++)
+    {
+        for (int j = 0; j < inputsLenght; j++)
+        {
+            inputsValues[j] = inputs[j]->getMFs(mfsIndexs[i][j])->getValue(values[j]);
+        }
+        
+        float m = min(inputsValues);
+
+        if (m == 0)
+            continue;
+
+        for (int j = 0; j < outputsLenght; j++)
+        {
+            num[j] += m * outputs[j]->getMFs(mfsIndexs[i][inputsLenght + j])->centroid(m);
+            den[j] += m;
+        }
+    }
+
+    for (int i = 0; i < outputsLenght; i++)
+    {
+        outputsValues[i] = num[i] / den[i];
+    }
+
+    return outputsValues;
+}
+
+float FuzzyController::min(float* values)
+{
+    float minValue = values[0];
+
+    for (int i = 1; i < inputsLenght; i++)
+    {
+        if (values[i] < minValue)
+            minValue = values[i];
+    }
+
+    return minValue;
+}
diff -r 000000000000 -r 56b8c86181b1 Controllers/Fuzzy/FuzzyController.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controllers/Fuzzy/FuzzyController.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,40 @@
+#ifndef FC_H
+#define FC_H
+
+#pragma once
+
+#include "FuzzyIO.h"
+
+class FuzzyController
+{
+    public:
+        FuzzyController(void);
+        ~FuzzyController(void);
+
+        void addInput(FuzzyIO* input);
+        void addOutput(FuzzyIO* output);
+        void addRules(int rulesAmount, int** mfsIndexs);
+
+        float* compute(float* values);
+
+    private:
+        FuzzyIO** inputs;
+        FuzzyIO** outputs;
+
+        int inputsLenght;
+        int outputsLenght;
+
+        int rulesAmount;
+        int** mfsIndexs;
+
+        float* outputsValues;
+        float* inputsValues;
+        
+        float* num;
+        float* den;
+
+        float min(float* values);
+        float max(float* values);
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 Controllers/Fuzzy/FuzzyIO.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controllers/Fuzzy/FuzzyIO.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,64 @@
+#include "FuzzyIO.h"
+
+FuzzyIO::FuzzyIO(float rangeMin, float rangeMax)
+{
+    this->rangeMin = rangeMin;
+    this->rangeMax = rangeMax;
+
+    mfs = NULL;
+    mfsLenght = 0;
+}
+
+FuzzyIO::~FuzzyIO(void)
+{
+}
+
+void FuzzyIO::addMF(MembershipFunction* mf)
+{
+    mfs = (MembershipFunction**) realloc(mfs, sizeof(MembershipFunction*) * (mfsLenght + 1));
+    
+    mfs[mfsLenght] = mf;
+    
+    mfsLenght++;
+}
+
+void FuzzyIO::addAutoMFs(int amount, mfType type, float rangeMin, float rangeMax)
+{
+    if (amount <= 1)
+    {
+        MembershipFunction* mf = new MembershipFunction(type);
+        mf->setValues(rangeMin, (rangeMin - rangeMax) / 2, rangeMax);
+        
+        this->addMF(mf);
+        
+        return;
+    }
+    
+    float rangeDt = (rangeMax - rangeMin) / (amount - 1);
+    float startPoint = rangeMin - rangeDt;
+    
+    for (int i = 0; i < amount; i++)
+    {
+        MembershipFunction* mf = new MembershipFunction(type);
+        mf->setValues(startPoint, startPoint + rangeDt, startPoint + rangeDt * 2);
+        
+        startPoint = startPoint + rangeDt;
+        
+        this->addMF(mf);
+    }
+}
+
+void FuzzyIO::addAutoMFs(int amount, mfType type)
+{
+    this->addAutoMFs(amount, type, this->rangeMin, this->rangeMax);
+}
+
+MembershipFunction* FuzzyIO::getMFs(int index)
+{
+    return mfs[index];
+}
+
+int FuzzyIO::getMFsLenght()
+{
+    return this->mfsLenght;
+}
diff -r 000000000000 -r 56b8c86181b1 Controllers/Fuzzy/FuzzyIO.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controllers/Fuzzy/FuzzyIO.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,31 @@
+#ifndef FUZZYIO_H
+#define FUZZYIO_H
+
+#pragma once
+
+#include "MembershipFunction.h"
+
+class FuzzyIO
+{
+    public:
+        FuzzyIO(float rangeMin, float rangeMax);
+        ~FuzzyIO(void);
+
+        void addMF(MembershipFunction* mf);
+        void addAutoMFs(int amount, mfType type);
+        void addAutoMFs(int amount, mfType type, float rangeMin, float rangeMax);
+
+        MembershipFunction* getMFs(int index);
+        int getMFsLenght();
+    
+    private:
+        //string inputName;
+        
+        float rangeMin;
+        float rangeMax;
+        
+        MembershipFunction** mfs;
+        int mfsLenght;
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 Controllers/Fuzzy/MembershipFunction.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controllers/Fuzzy/MembershipFunction.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,130 @@
+#include "MembershipFunction.h"
+
+MembershipFunction::MembershipFunction(mfType type, float a, float b, float c, float d, float u, float v)
+{
+    this->type = type;
+    
+    this->a = a;
+    this->b = b;
+    this->c = c;
+    this->d = d;
+    
+    this->u = u;
+    this->v = v;
+}
+
+MembershipFunction::MembershipFunction(mfType type)
+{
+    this->type = type;
+    
+    a = 0;
+    b = 0;
+    c = 0;
+    d = 0;
+    
+    u = 0;
+    v = 0;
+
+    //MembershipFunction(type, 0, 0, 0, 0, 0, 0);
+}
+
+MembershipFunction::~MembershipFunction(void)
+{
+}
+
+float MembershipFunction::getValue(float x)
+{
+    switch (type)
+    {
+        case TRIMF:
+            return trimf(x);
+        break;
+
+        case TRAPMF:
+            return trapmf(x);
+        break;
+    }
+    
+    return 0;
+}
+
+void MembershipFunction::setValues(float a, float b, float c)
+{
+    this->a = a;
+    this->b = b;
+    this->c = c;
+}
+
+void MembershipFunction::setValues(float a, float b, float c, float d)
+{
+    this->a = a;
+    this->b = b;
+    this->c = c;
+    this->d = d;
+}
+
+void MembershipFunction::setValues(float u, float v)
+{
+    this->u = u;
+    this->v = v;
+}
+
+float MembershipFunction::trimf(float x)
+{
+    if (x <= a || x > c)
+        return 0;
+
+    if (x > a && x <= b)
+        return (x - a) / (b - a);
+    
+    if (x > b && x <= c)
+        return (c - x) / (c - b);
+        
+    return 0;
+}
+
+float MembershipFunction::trapmf(float x)
+{
+    if (x <= a || x > d)
+        return 0;
+
+    if (x > a && x <= b)
+        return (x - a) / (b - a);
+
+    if (x > b && x <= c)
+        return 1;
+    
+    if (x > c && x <= d)
+        return (d - x) / (d - c);
+        
+    return 0;
+}
+
+float MembershipFunction::centroid(float h)
+{
+    float p1, p2, at, bt, ct;
+
+    switch (type)
+    {
+        case TRIMF:
+            p1 = a + (b - a) * h;
+            p2 = c - (c - b) * h;
+
+            at = p2 - p1;
+            bt = c - a;
+            ct = p1 - a;
+
+            return a + (2*at*ct + at*at + ct*bt + at*bt + bt*bt) / (3 * (at + bt));
+            //return p1 + (p2 - p1) / 2.0;
+        break;
+
+        case TRAPMF:
+            p1 = a + (b - a) * h;
+            p2 = d - (d - c) * h;
+            return p1 + (p2 - p1) / 2.0;
+        break;
+    }
+
+    //return a + (c - a) / 2.0;
+    return 0;
+}
diff -r 000000000000 -r 56b8c86181b1 Controllers/Fuzzy/MembershipFunction.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controllers/Fuzzy/MembershipFunction.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,35 @@
+#ifndef MF_H
+#define MF_H
+
+#include <stdlib.h>
+
+#define NULL    0
+
+enum mfType {TRIMF, TRAPMF, GUASSMF};
+
+class MembershipFunction
+{
+    public:
+        MembershipFunction(mfType type, float a, float b, float c, float d, float u, float v);
+        MembershipFunction(mfType type);
+        virtual ~MembershipFunction(void);
+
+        float getValue(float x);
+        void setValues(float a, float b, float c);
+        void setValues(float a, float b, float c, float d);
+        void setValues(float u, float v);
+
+        float centroid(float h);
+    
+    private:
+        mfType type;
+    
+        float a, b, c, d;
+        float u, v;
+        
+        float trimf(float x);
+        float trapmf(float x);
+        float guassmf(float x);
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 Controllers/PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controllers/PID.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,77 @@
+#include "PID.h"
+
+PID::PID(float kp, float ki, float kd, float dt)
+{
+    this->kp = kp;
+    this->ki = ki;
+    this->kd = kd;
+    this->dt = dt;
+    
+    reset();
+}
+
+void PID::reset()
+{
+    lastErro = 0.0f;
+    lastSumErro = 0.0f;
+    _pid = 0.0f;
+    
+    integral = 0.0f;
+    
+}
+
+void PID::setGains(float kp, float ki, float kd)
+{
+    this->kp = kp;
+    this->ki = ki;
+    this->kd = kd;
+}
+
+float PID::compute(float setPoint, float currentPoint)
+{
+    float error = setPoint - currentPoint;
+    float derivative = (error - lastErro)/dt;
+    integral = (integral + (error*dt));
+    
+    float contribuicao = (kp*error) + (ki*integral) + (kd*derivative);
+    
+    
+  
+    //_pid = contribuicao;
+            
+    lastErro = error;
+    //lastSumErro += error;
+    
+    return contribuicao;
+}
+
+float PID::getP()
+{
+    return kp;
+}
+
+float PID::getI()
+{
+    return ki;
+}
+
+float PID::getD()
+{
+    return kd;
+}
+/*
+float PID::getuP()
+{
+    return up;
+}
+
+float PID::getuI()
+{
+    return ui;
+}
+
+float PID::getuD()
+{
+    return ud;
+}
+*/
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 Controllers/PID.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controllers/PID.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,24 @@
+#ifndef PID_H
+#define PID_H
+
+class PID
+{
+    public:
+        PID(float kp, float ki, float kd, float dt);
+        
+        void setGains(float kp, float ki, float kd);
+        void reset();
+        float compute(float setPoint, float currentPoint);
+        
+        float getP();
+        float getI();
+        float getD();
+        
+    private:
+        float kp, ki, kd, _pid;
+        float dt;
+        float lastErro, lastSumErro;
+        float integral;
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 Filters/ComplementaryFilter.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filters/ComplementaryFilter.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,28 @@
+#include "ComplementaryFilter.h"
+
+ComplementaryFilter::ComplementaryFilter(float dt, float alpha, float beta)
+{
+    this->dt    = dt;
+    this->alpha = alpha;
+    this->beta  = beta;
+}
+
+ComplementaryFilter::~ComplementaryFilter(void)
+{
+}
+
+void ComplementaryFilter::reset(void)
+{
+    this->angle = 0;
+}
+
+/*
+ * All the values of acc_rate and gyro rate can be given either in radians or degrees
+ * x_acc = (float)(x_acc_ADC – x_acc_offset) * x_acc_scale;
+ * gyro  = (float)(gyro_ADC – gyro_offset)   * gyro_scale;
+ */
+float ComplementaryFilter::update(float acc_angle, float gyro_rate)
+{
+    this->angle = ( alpha * ( this->angle + (gyro_rate * dt) ) ) + (beta * acc_angle);
+    return this->angle;
+}
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 Filters/ComplementaryFilter.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filters/ComplementaryFilter.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,23 @@
+#ifndef COMPFILTER_H
+#define COMPFILTER_H
+
+/*
+#include <stdlib.h>
+#include <math.h>
+*/
+class ComplementaryFilter
+{
+    public:
+    
+        ComplementaryFilter(float dt, float alpha, float beta);
+        ~ComplementaryFilter(void);
+        float update(float acc_rate, float gyro_rate);
+            
+    private:
+    
+        void reset(void);
+        float dt, alpha, beta;
+        float angle;
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 Filters/Kalman.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filters/Kalman.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,81 @@
+#include "Kalman.h"
+
+Kalman::Kalman() 
+{
+    /* We will set the varibles like so, these can also be tuned by the user */
+    Q_angle = 0.001;
+    Q_bias = 0.003;
+    R_measure = 0.03;
+    
+    bias = 0; // Reset bias
+    // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), 
+    // the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
+    P[0][0] = 0;
+    P[0][1] = 0;
+    P[1][0] = 0;
+    P[1][1] = 0;
+}
+// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
+double Kalman::getAngle(double newAngle, double newRate, double dt) 
+{
+    // Discrete Kalman filter time update equations - Time Update ("Predict")
+    // Update xhat - Project the state ahead
+    /* Step 1 */
+    rate = newRate - bias;
+    angle += dt * rate;
+    
+    // Update estimation error covariance - Project the error covariance ahead
+    /* Step 2 */
+    P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
+    P[0][1] -= dt * P[1][1];
+    P[1][0] -= dt * P[1][1];
+    P[1][1] += Q_bias * dt;
+    
+    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
+    // Calculate Kalman gain - Compute the Kalman gain
+    /* Step 4 */
+    S = P[0][0] + R_measure;
+    /* Step 5 */
+    K[0] = P[0][0] / S;
+    K[1] = P[1][0] / S;
+    
+    // Calculate angle and bias - Update estimate with measurement zk (newAngle)
+    /* Step 3 */
+    y = newAngle - angle;
+    /* Step 6 */
+    angle += K[0] * y;
+    bias += K[1] * y;
+    
+    // Calculate estimation error covariance - Update the error covariance
+    /* Step 7 */
+    P[0][0] -= K[0] * P[0][0];
+    P[0][1] -= K[0] * P[0][1];
+    P[1][0] -= K[1] * P[0][0];
+    P[1][1] -= K[1] * P[0][1];
+    
+    return angle;
+}
+// Used to set angle, this should be set as the starting angle
+void Kalman::setAngle(double newAngle) 
+{ 
+    angle = newAngle; 
+}
+// Return the unbiased rate
+double Kalman::getRate() 
+{ 
+    return rate; 
+}
+
+/* These are used to tune the Kalman filter */
+void Kalman::setQangle(double newQ_angle) 
+{ 
+    Q_angle = newQ_angle; 
+}
+void Kalman::setQbias(double newQ_bias) 
+{ 
+    Q_bias = newQ_bias; 
+}
+void Kalman::setRmeasure(double newR_measure) 
+{
+    R_measure = newR_measure; 
+}
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 Filters/Kalman.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filters/Kalman.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,37 @@
+#ifndef KALMAN_H
+#define KALMAN_H
+
+#include <stdlib.h>
+#include <math.h>
+
+class Kalman 
+{
+
+    public:
+        Kalman();
+        ~Kalman(void);
+        double getAngle(double newAngle, double newRate, double dt);
+        void setAngle(double newAngle);
+        double getRate();
+        
+        void setQangle(double newQ_angle);
+        void setQbias(double newQ_bias);
+        void setRmeasure(double newR_measure);
+        
+    private:
+        /* Kalman filter variables */
+        double Q_angle; // Process noise variance for the accelerometer
+        double Q_bias; // Process noise variance for the gyro bias
+        double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
+        
+        double angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix
+        double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix
+        double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
+        
+        double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
+        double K[2]; // Kalman gain - This is a 2x1 matrix
+        double y; // Angle difference - 1x1 matrix
+        double S; // Estimate error - 1x1 matrix
+};
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 Filters/KalmanFilter.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filters/KalmanFilter.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,274 @@
+#include "KalmanFilter.h"
+
+KalmanFilter::KalmanFilter()
+{
+    reset();
+}
+
+KalmanFilter::~KalmanFilter(void)
+{
+}
+
+void KalmanFilter::reset()
+{    
+    A[0][0] = 1.0;
+    A[0][1] = -0.005;
+    A[1][0] = 0.0;
+    A[1][1] = 1.0;
+    
+    B[0][0] = 0.005;
+    B[1][0] = 0.0;
+    
+    C[0][0] = 1.0;
+    C[0][1] = 0.0;
+    
+    Sz[0][0] = 17.2;
+    
+    Sw[0][0] = 0.005;
+    Sw[0][1] = 0.005;
+    Sw[1][0] = 0.005;
+    Sw[1][1] = 0.005;
+    
+    xhat[0][0] = 0.0;
+    xhat[1][0] = 0.0;
+    
+    /* Matriz P -> covariância do do erro
+     * Definindo o estado inicial da matriz de covariância do erro, 
+     * é aconselhável a definir L um número alto. 
+     * Neste caso, sabemos o estado inicial do nosso modelo, 
+     * então L é igual a ZERO.
+     */
+    
+    P[0][0] = 0; //L
+    P[0][1] = 0;
+    P[1][0] = 0;
+    P[1][1] = 0; //L
+}
+
+float KalmanFilter::update(float gyroRaw, float accelAngle)
+{
+    // Inputs.
+    float u[1][1];              // Gyroscope rate.
+    float y[1][1];              // Accelerometer angle.
+    
+    // Temp values.
+    float AP[2][2];             // This is the matrix A*P
+    float CT[2][1];             // This is the matrix C'
+    float APCT[2][1];           // This is the matrix A*P*C'
+    float CP[1][2];             // This is the matrix C*P
+    float CPCT[1][1];           // This is the matrix C*P*C'
+    float CPCTSz[1][1];         // This is the matrix C*P*C'+Sz
+    float CPCTSzInv[1][1];      // This is the matrix inv(C*P*C'+Sz)
+    float K[2][1];              // This is the Kalman gain.
+    float Cxhat[1][1];          // This is the vector C*xhat
+    float yCxhat[1][1];         // This is the vector y-C*xhat
+    float KyCxhat[2][1];        // This is the vector K*(y-C*xhat)
+    float Axhat[2][1];          // This is the vector A*xhat 
+    float Bu[2][1];             // This is the vector B*u
+    float AxhatBu[2][1];        // This is the vector A*xhat+B*u
+    float AT[2][2];             // This is the matrix A'
+    float APAT[2][2];           // This is the matrix A*P*A'
+    float APATSw[2][2];         // This is the matrix A*P*A'+Sw
+    float KC[2][2];             // This is the matrix K*C
+    float KCP[2][2];            // This is the matrix K*C*P
+    float KCPAT[2][2];          // This is the matrix K*C*P*A'
+    
+    // Fill in inputs.
+    u[0][0] = gyroRaw;
+    y[0][0] = accelAngle;
+    
+    // Update the state estimate by extrapolating estimate with gyroscope input.
+    // xhat_est = A * xhat + B * u
+    matrix_multiply((float*) A, (float*) xhat, 2, 2, 1, (float*) Axhat);
+    matrix_multiply((float*) B, (float*) u, 2, 1, 1, (float*) Bu);
+    matrix_addition((float*) Axhat, (float*) Bu, 2, 1, (float*) AxhatBu);
+    
+    // Compute the innovation.
+    // Inn = y - c * xhat;
+    matrix_multiply((float*) C, (float*) xhat, 1, 2, 1, (float*) Cxhat);
+    matrix_subtraction((float*) y, (float*) Cxhat, 1, 1, (float*) yCxhat);
+    
+    // Compute the covariance of the innovation.
+    // s = C * P * C' + Sz
+    matrix_transpose((float*) C, 1, 2, (float*) CT);
+    matrix_multiply((float*) C, (float*) P, 1, 2, 2, (float*) CP);
+    matrix_multiply((float*) CP, (float*) CT, 1, 2, 1, (float*) CPCT);
+    matrix_addition((float*) CPCT, (float*) Sz, 1, 1, (float*) CPCTSz);
+    
+    // Compute the kalman gain matrix.
+    // K = A * P * C' * inv(s)
+    matrix_multiply((float*) A, (float*) P, 2, 2, 2, (float*) AP);
+    matrix_multiply((float*) AP, (float*) CT, 2, 2, 1, (float*) APCT);
+    matrix_inversion((float*) CPCTSz, 1, (float*) CPCTSzInv);
+    matrix_multiply((float*) APCT, (float*) CPCTSzInv, 2, 1, 1, (float*) K);
+    
+    // Update the state estimate.
+    // xhat = xhat_est + K * Inn;
+    matrix_multiply((float*) K, (float*) yCxhat, 2, 1, 1, (float*) KyCxhat);
+    matrix_addition((float*) AxhatBu, (float*) KyCxhat, 2, 1, (float*) xhat);
+    
+    // Compute the new covariance of the estimation error.
+    // P = A * P * A' - K * C * P * A' + Sw
+    matrix_transpose((float*) A, 2, 2, (float*) AT);
+    matrix_multiply((float*) AP, (float*) AT, 2, 2, 2, (float*) APAT);
+    matrix_addition((float*) APAT, (float*) Sw, 2, 2, (float*) APATSw);
+    matrix_multiply((float*) K, (float*) C, 2, 1, 2, (float*) KC);
+    matrix_multiply((float*) KC, (float*) P, 2, 2, 2, (float*) KCP);
+    matrix_multiply((float*) KCP, (float*) AT, 2, 2, 2, (float*) KCPAT);
+    matrix_subtraction((float*) APATSw, (float*) KCPAT, 2, 2, (float*) P);
+    
+    // Return the estimate.
+    return xhat[0][0];
+}
+
+float KalmanFilter::getEstimatedData()
+{
+    return xhat[0][0];
+}
+
+void KalmanFilter::matrix_multiply(float* A, float* B, int m, int p, int n, float* C)
+// Matrix Multiplication Routine
+{
+    // A = input matrix (m x p)
+    // B = input matrix (p x n)
+    // m = number of rows in A
+    // p = number of columns in A = number of rows in B
+    // n = number of columns in B
+    // C = output matrix = A*B (m x n)
+    int i, j, k;
+    for (i=0;i<m;i++)
+        for(j=0;j<n;j++)
+        {
+          C[n*i+j]=0;
+          for (k=0;k<p;k++)
+            C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j];
+        }
+}
+
+void KalmanFilter::matrix_addition(float* A, float* B, int m, int n, float* C)
+// Matrix Addition Routine
+{
+    // A = input matrix (m x n)
+    // B = input matrix (m x n)
+    // m = number of rows in A = number of rows in B
+    // n = number of columns in A = number of columns in B
+    // C = output matrix = A+B (m x n)
+    int i, j;
+    for (i=0;i<m;i++)
+        for(j=0;j<n;j++)
+            C[n*i+j]=A[n*i+j]+B[n*i+j];
+}
+
+void KalmanFilter::matrix_subtraction(float* A, float* B, int m, int n, float* C)
+// Matrix Subtraction Routine
+{
+    // A = input matrix (m x n)
+    // B = input matrix (m x n)
+    // m = number of rows in A = number of rows in B
+    // n = number of columns in A = number of columns in B
+    // C = output matrix = A-B (m x n)
+    int i, j;
+    for (i=0;i<m;i++)
+        for(j=0;j<n;j++)
+            C[n*i+j]=A[n*i+j]-B[n*i+j];
+}
+
+void KalmanFilter::matrix_transpose(float* A, int m, int n, float* C)
+// Matrix Transpose Routine
+{
+    // A = input matrix (m x n)
+    // m = number of rows in A
+    // n = number of columns in A
+    // C = output matrix = the transpose of A (n x m)
+    int i, j;
+    for (i=0;i<m;i++)
+        for(j=0;j<n;j++)
+            C[m*j+i]=A[n*i+j];
+}
+
+
+int KalmanFilter::matrix_inversion(float* A, int n, float* AInverse)
+// Matrix Inversion Routine
+{
+    // A = input matrix (n x n)
+    // n = dimension of A
+    // AInverse = inverted matrix (n x n)
+    // This function inverts a matrix based on the Gauss Jordan method.
+    // The function returns 1 on success, 0 on failure.
+    int i, j, iPass, imx, icol, irow;
+    float det, temp, pivot, factor;
+    float* ac = (float*) calloc(n*n, sizeof(float));
+    det = 1;
+    for (i = 0; i < n; i++)
+    {
+        for (j = 0; j < n; j++)
+        {
+            AInverse[n*i+j] = 0;
+            ac[n*i+j] = A[n*i+j];
+        }
+        AInverse[n*i+i] = 1;
+    }
+
+    // The current pivot row is iPass.
+    // For each pass, first find the maximum element in the pivot column.
+    for (iPass = 0; iPass < n; iPass++)
+    {
+        imx = iPass;
+        for (irow = iPass; irow < n; irow++)
+        {
+            if (fabs(A[n*irow+iPass]) > fabs(A[n*imx+iPass])) imx = irow;
+        }
+
+        // Interchange the elements of row iPass and row imx in both A and AInverse.
+        if (imx != iPass)
+        {
+            for (icol = 0; icol < n; icol++)
+            {
+                temp = AInverse[n*iPass+icol];
+                AInverse[n*iPass+icol] = AInverse[n*imx+icol];
+                AInverse[n*imx+icol] = temp;
+                if (icol >= iPass)
+                {
+                    temp = A[n*iPass+icol];
+                    A[n*iPass+icol] = A[n*imx+icol];
+                    A[n*imx+icol] = temp;
+                }
+            }
+        }
+
+        // The current pivot is now A[iPass][iPass].
+        // The determinant is the product of the pivot elements.
+        pivot = A[n*iPass+iPass];
+        det = det * pivot;
+        if (det == 0)
+        {
+            free(ac);
+            return 0;
+        }
+
+        for (icol = 0; icol < n; icol++)
+        {
+            // Normalize the pivot row by dividing by the pivot element.
+            AInverse[n*iPass+icol] = AInverse[n*iPass+icol] / pivot;
+            if (icol >= iPass) A[n*iPass+icol] = A[n*iPass+icol] / pivot;
+        }
+
+        for (irow = 0; irow < n; irow++)
+        {
+            // Add a multiple of the pivot row to each row.  The multiple factor
+            // is chosen so that the element of A on the pivot column is 0.
+            if (irow != iPass) factor = A[n*irow+iPass];
+            for (icol = 0; icol < n; icol++)
+            {
+                if (irow != iPass)
+                {
+                    AInverse[n*irow+icol] -= factor * AInverse[n*iPass+icol];
+                    A[n*irow+icol] -= factor * A[n*iPass+icol];
+                }
+            }
+        }
+    }
+
+    free(ac);
+    return 1;
+}
diff -r 000000000000 -r 56b8c86181b1 Filters/KalmanFilter.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filters/KalmanFilter.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,34 @@
+#ifndef KALMANFILTER_H
+#define KALMANFILTER_H
+
+#include <stdlib.h>
+#include <math.h>
+
+class KalmanFilter
+{
+    public:
+        KalmanFilter();
+        ~KalmanFilter(void);
+        
+        void reset();
+        float update(float gyroRaw, float accelAngle);
+        float getEstimatedData();
+    
+    private:
+        float A[2][2];
+        float B[2][1];
+        float C[1][2];
+        float Sz[1][1];
+        float Sw[2][2];
+        
+        float xhat[2][1];
+        float P[2][2];
+        
+        void matrix_multiply(float* A, float* B, int m, int p, int n, float* C);
+        void matrix_addition(float* A, float* B, int m, int n, float* C);
+        void matrix_subtraction(float* A, float* B, int m, int n, float* C);
+        void matrix_transpose(float* A, int m, int n, float* C);
+        int matrix_inversion(float* A, int n, float* AInverse);
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 GetTickCount/GetTickCount.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GetTickCount/GetTickCount.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,5 @@
+volatile unsigned int TickCount;
+
+extern "C" void SysTick_Handler (void) {
+    TickCount+= 10;
+}
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 GetTickCount/GetTickCount.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GetTickCount/GetTickCount.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,31 @@
+/* mbed GetTickCount Library
+  * Copyright (c) 2010 Michael Wei
+  */ 
+  
+//shouldn't have to include, but fixes weird problems with defines
+#include "LPC1768/LPC17xx.h"
+
+#ifndef MBED_TICKCOUNT_H 
+#define MBED_TICKCOUNT_H 
+extern volatile unsigned int TickCount;
+
+inline void GetTickCount_Start(void) {
+    //CMSIS SYSTICK Config
+    SysTick_Config(SystemCoreClock / 100);               /* Generate interrupt every 10 ms */
+}
+
+inline void GetTickCount_Stop(void) {
+    SysTick->CTRL = (1 << SYSTICK_CLKSOURCE) | (0<<SYSTICK_ENABLE) | (0<<SYSTICK_TICKINT);    /* Disable SysTick IRQ and SysTick Timer */
+}
+
+inline void GetTickCount_Reset(void) {
+     SysTick->CTRL = (1 << SYSTICK_CLKSOURCE) | (0<<SYSTICK_ENABLE) | (0<<SYSTICK_TICKINT);    /* Disable SysTick IRQ and SysTick Timer */
+     TickCount = 0;
+     SysTick->CTRL = (1 << SYSTICK_CLKSOURCE) | (1<<SYSTICK_ENABLE) | (1<<SYSTICK_TICKINT);    /* Enable SysTick IRQ and SysTick Timer */
+}
+
+inline unsigned int GetTickCount(void)
+{
+    return TickCount;
+}
+#endif
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 Hardware/Motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Hardware/Motor.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,69 @@
+#include "Motor.h"
+
+Motor::Motor(PinName pwm, float _period/*, float* linCoef, float lowerSat, float upperSat*/) : PwmOut(pwm)
+{    
+    //period(_period);
+    period_us(_period);
+    
+    currentPower = 0;
+    
+    //setPower(currentPower);
+    
+    //this->linCoef = linCoef;
+    
+    //this->lowerSat = lowerSat;
+    //this->upperSat = upperSat;
+}
+
+/*Motor::Motor(PinName pwm, float periodms) : PwmOut(pwm)
+{
+    //Motor(pwm, (periodms*1000), new float[1], 0, 100);
+    period_ms(periodms);
+    
+    //currentPower = 0;
+    
+    //this->linCoef = new float[1];
+    
+    //this->lowerSat = 0;
+    //this->upperSat = 100;
+}*/
+
+void Motor::setPower(float power)
+{
+    power = clampFloat(power, 0.0, 100.0);
+    
+    //pulsewidth(PWM_MIN + (power / 100.0) * PWM_DT);
+    write( (100-(PWM_MIN + power))/100);
+    currentPower = power;
+}
+
+void Motor::setPowerLin(float power)
+{
+    setPower(power);
+    /*
+    float pwm = 0;
+    
+    power = clampFloat(power, 0.0, 100.0);
+    
+    for (int i = 0; i < 5; i++)
+    {
+        pwm += linCoef[4-i] * pow(power/100.0, i);
+    }
+    
+    setPower(lowerSat + pwm*(upperSat - lowerSat));
+    */
+}
+void Motor::arm(int pwm_ms)
+{
+    pulsewidth_ms(pwm_ms);
+    //write( (power)/100 );
+}
+void Motor::accumulatePower(float accPower)
+{
+    setPower(currentPower + accPower);
+}
+
+float Motor::getPower()
+{
+    return currentPower;
+}
diff -r 000000000000 -r 56b8c86181b1 Hardware/Motor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Hardware/Motor.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,30 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+#include "mbed.h"
+#include "Util.h"
+
+//#define PWM_MIN     0.001
+#define PWM_MIN     0.1
+#define PWM_DT      0.001
+
+class Motor : public PwmOut
+{
+    public:
+        Motor(PinName pwm, float _period/*, float* linCoef, float lowerSat, float upperSat*/);
+        //Motor(PinName pwm, float periodms);
+        
+        void setPower(float power);
+        void setPowerLin(float power);
+        void arm(int pwm_ms);
+        void accumulatePower(float accPower);
+        
+        float getPower();
+        
+    private:
+        float currentPower;
+        //float* linCoef;
+        //float lowerSat, upperSat;
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 Sensors/ADXL345.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/ADXL345.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,165 @@
+
+#include "ADXL345.h"
+
+ADXL345::ADXL345(PinName mosi, PinName miso, PinName sck, PinName cs) : _spi(mosi, miso, sck), _ncs (cs)
+{
+    _spi.frequency(2000000);
+    _spi.format(8,3);
+    _ncs = 1;
+    wait_us(500);
+    
+    //oneByteWrite(ADXL345_SPI_WRITE | (ADXL345_POWER_CTL_REG & 0x3F), 0x00);
+    oneByteWrite(ADXL345_POWER_CTL_REG, 0x08); 
+    //Measure bit -> 1
+    //A setting of 0 in the measure bit places the part into standby mode, and a setting of 1 places the part into measurement mode. 
+    //The ADXL345 powers up in standby mode with minimum power consumption. 
+
+    //oneByteWrite(ADXL345_SPI_WRITE | (ADXL345_DATA_FORMAT_REG & 0x3F), /*0x0B*/ 0x08);
+    oneByteWrite(ADXL345_DATA_FORMAT_REG, /*0x0B*/ 0x00);
+                                                                                            // |   D7      D6       D5     D4     D3       D2    [D1  D0] | 
+                                                                                            // |SELF_TEST  SPI  INT_INVERT  0  FULL_RES  Justify  Range   |
+                                                                                            // |    0       0        0      0      0        0      0   0  |
+    //FULL_RES Bit -> 0
+    //When this bit is set to a value of 1, the device is in full resolution 
+    //mode, where the output resolution increases with the grange 
+    //set by the range bits to maintain a 4 mg/LSB scale factor. When 
+    //the FULL_RES bit is set to 0, the device is in 10-bit mode, and 
+    //the range bits determine the maximum grange and scale factor. 
+
+    //setDataRate(ADXL345_100HZ);
+    setDataRate(ADXL345_200HZ);
+    
+    oneByteWrite(ADXL345_POWER_CTL_REG, 0x08);
+    
+    wait_us(500);
+}
+
+void ADXL345::setDataRate(int dataRate)
+{
+    //Get the current register contents, so we don't clobber the power bit.
+    char registerContents = oneByteRead(ADXL345_BW_RATE_REG);
+
+    registerContents &= 0x10;
+    registerContents |= dataRate;
+
+    oneByteWrite(ADXL345_BW_RATE_REG, registerContents);
+}
+
+void ADXL345::getAxes(int* axes)
+{
+    getData(axes, 3);
+}
+
+void ADXL345::getData(int* dataVector, int vectorSize)
+{
+    char* buffer = new char[vectorSize * 2];
+    
+    multiByteRead(ADXL345_DATAX0_REG, buffer, vectorSize * 2);
+    
+    for(int i = 0, j = 0; i < vectorSize; i++, j += 2)
+    {
+        dataVector[i] = (int16_t)((int)buffer[j+1] << 8 | (int)buffer[j]);
+    }
+    
+    free(buffer);
+}
+
+float ADXL345::getX()
+{
+    char lsb_byte = 0;
+    signed short msb_byte;
+    float acc;
+    
+    lsb_byte = oneByteRead(ADXL345_DATAX1_REG);
+    msb_byte = lsb_byte << 8;
+    msb_byte |= oneByteRead(ADXL345_DATAX0_REG);
+    acc = (float)msb_byte;
+    return acc;
+}
+
+float ADXL345::getY()
+{
+    char lsb_byte = 0;
+    signed short msb_byte;
+    float acc;
+    
+    lsb_byte = oneByteRead(ADXL345_DATAY1_REG);
+    msb_byte = lsb_byte << 8;
+    msb_byte |= oneByteRead(ADXL345_DATAY0_REG);
+    acc = (float)msb_byte;
+    return acc;
+}
+
+float ADXL345::getZ()
+{
+    char lsb_byte = 0;
+    signed short msb_byte;
+    float acc;
+    
+    lsb_byte = oneByteRead(ADXL345_DATAZ1_REG);
+    msb_byte = lsb_byte << 8;
+    msb_byte |= oneByteRead(ADXL345_DATAZ0_REG);
+    acc = (float)msb_byte;
+    return acc;
+}
+
+int ADXL345::oneByteRead(int address) {
+ 
+    int tx = (ADXL345_SPI_READ | (address & 0x3F));
+    int rx = 0;
+ 
+    _ncs = 0;
+    //Send address to read from.
+    _spi.write(tx);
+    //Read back contents of address.
+    rx = _spi.write(0x00);
+    _ncs = 1;
+ 
+    return rx;
+ 
+}
+ 
+void ADXL345::oneByteWrite(int address, char data) {
+ 
+    int tx = (ADXL345_SPI_WRITE | (address & 0x3F));
+ 
+    _ncs = 0;
+    //Send address to write to.
+    _spi.write(tx);
+    //Send data to be written.
+    _spi.write(data);
+    _ncs = 1;
+ 
+}
+ 
+void ADXL345::multiByteRead(int startAddress, char* buffer, int size) {
+ 
+    int tx = (ADXL345_SPI_READ | ADXL345_MULTI_BYTE | (startAddress & 0x3F));
+ 
+    _ncs = 0;
+    //Send address to start reading from.
+    _spi.write(tx);
+ 
+    for (int i = 0; i < size; i++) {
+        buffer[i] = _spi.write(0x00);
+    }
+ 
+    _ncs = 1;
+ 
+}
+ 
+void ADXL345::multiByteWrite(int startAddress, char* buffer, int size) {
+ 
+    int tx = (ADXL345_SPI_WRITE | ADXL345_MULTI_BYTE | (startAddress & 0x3F));
+ 
+    _ncs = 0;
+    //Send address to start reading from.
+    _spi.write(tx);
+ 
+    for (int i = 0; i < size; i++) {
+        buffer[i] = _spi.write(0x00);
+    }
+ 
+    _ncs = 1;
+ 
+}
diff -r 000000000000 -r 56b8c86181b1 Sensors/ADXL345.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/ADXL345.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,81 @@
+#ifndef ADXL345_H
+#define ADXL345_H
+
+#include "mbed.h"
+
+#define ADXL345_DEVID_REG          0x00
+#define ADXL345_THRESH_TAP_REG     0x1D
+#define ADXL345_OFSX_REG           0x1E
+#define ADXL345_OFSY_REG           0x1F
+#define ADXL345_OFSZ_REG           0x20
+#define ADXL345_DUR_REG            0x21
+#define ADXL345_LATENT_REG         0x22
+#define ADXL345_WINDOW_REG         0x23
+#define ADXL345_THRESH_ACT_REG     0x24
+#define ADXL345_THRESH_INACT_REG   0x25
+#define ADXL345_TIME_INACT_REG     0x26
+#define ADXL345_ACT_INACT_CTL_REG  0x27
+#define ADXL345_THRESH_FF_REG      0x28
+#define ADXL345_TIME_FF_REG        0x29
+#define ADXL345_TAP_AXES_REG       0x2A
+#define ADXL345_ACT_TAP_STATUS_REG 0x2B
+#define ADXL345_BW_RATE_REG        0x2C
+#define ADXL345_POWER_CTL_REG      0x2D
+#define ADXL345_INT_ENABLE_REG     0x2E
+#define ADXL345_INT_MAP_REG        0x2F
+#define ADXL345_INT_SOURCE_REG     0x30
+#define ADXL345_DATA_FORMAT_REG    0x31
+#define ADXL345_DATAX0_REG         0x32
+#define ADXL345_DATAX1_REG         0x33
+#define ADXL345_DATAY0_REG         0x34
+#define ADXL345_DATAY1_REG         0x35
+#define ADXL345_DATAZ0_REG         0x36
+#define ADXL345_DATAZ1_REG         0x37
+#define ADXL345_FIFO_CTL           0x38
+#define ADXL345_FIFO_STATUS        0x39
+
+//Data rate codes.
+#define ADXL345_3200HZ      0x0F
+#define ADXL345_1600HZ      0x0E
+#define ADXL345_800HZ       0x0D
+#define ADXL345_400HZ       0x0C
+#define ADXL345_200HZ       0x0B
+#define ADXL345_100HZ       0x0A
+#define ADXL345_50HZ        0x09
+#define ADXL345_25HZ        0x08
+#define ADXL345_12HZ5       0x07
+#define ADXL345_6HZ25       0x06
+
+#define ADXL345_SPI_READ    0x80
+#define ADXL345_SPI_WRITE   0x00
+#define ADXL345_MULTI_BYTE  0x60
+
+#define ADXL345_X           0x00
+#define ADXL345_Y           0x01
+#define ADXL345_Z           0x02
+
+class ADXL345
+{
+    public:
+        ADXL345(PinName mosi, PinName miso, PinName sck, PinName cs);
+        
+        void setDataRate(int dataRate);
+        void getAxes(int* axes);
+        
+        float getX();
+        float getY();
+        float getZ();
+        
+    private:
+        virtual void getData(int* dataVector, int vectorSize);
+        SPI _spi;
+        DigitalOut _ncs;
+
+        int oneByteRead(int address);
+        void oneByteWrite(int address, char data);
+        void multiByteRead(int startAddress, char* buffer, int size);
+        void multiByteWrite(int startAddress, char* buffer, int size);
+        
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 Sensors/Accelerometer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Accelerometer.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,98 @@
+
+#include "Accelerometer.h"
+
+Accelerometer::Accelerometer(ADXL345* acceleHardware, float sensitivity)
+{
+    this->acceleHardware = acceleHardware; 
+    this->sensitivity = sensitivity;   
+    updateZeroRates();
+}
+
+Accelerometer::~Accelerometer(void)
+{
+}
+
+void Accelerometer::updateZeroRates()
+{
+    //update(253, 10); // Utilizando filtro passa baixa, é necessário 253 amostras para estabilização do filtro.
+    update(200, 10, true);
+    zeroRateX = rawX;
+    zeroRateY = rawY;
+    zeroRateZ = rawZ;
+}
+
+void Accelerometer::update()
+{    
+    update(1, 1, false);
+}
+
+void Accelerometer::update(int samplesSize, int sampleDataRate, bool iswait)
+{   
+    int axes[3] = {0, 0, 0};
+    rawX = 0;
+    rawY = 0;
+    rawZ = 0;
+    
+    for (int i = 0; i < samplesSize; i++)
+    {
+        acceleHardware->getAxes(axes);
+        //O valor do ângulo real para o ângulo medido está com uma diferença de 2.5 vezes maior.
+        rawX += ((float)axes[0]) / samplesSize;
+        rawY += ((float)axes[1]) / samplesSize;
+        rawZ += ((float)axes[2]) / samplesSize;
+        
+        /*rawX += (acceleHardware->getX()) / samplesSize;
+        rawY += (acceleHardware->getY()) / samplesSize;
+        rawZ += (acceleHardware->getZ()) / samplesSize;*/
+        
+        if(wait)
+            wait_ms(sampleDataRate);
+    }
+}
+
+float Accelerometer::getAccelerationX()
+{
+    //( * Vref / 1023 – VzeroG) / Sensitivity
+    //return (((rawX - zeroRateX) / sensitivity) + 1.0); //Devido a posição da plaquinha, quando se define o zeroRateX, tem que adicionar novamente a gravidade, que está influenciando o X
+    return ((rawX - zeroRateX) / sensitivity);
+}
+
+float Accelerometer::getAccelerationY()
+{
+    return ((rawY - zeroRateY) / sensitivity);
+}
+
+float Accelerometer::getAccelerationZ()
+{
+    return (((rawZ - zeroRateZ) / sensitivity) + 1.0);
+}
+
+float Accelerometer::getRadiansAngleX()
+{
+    float x = getAccelerationX();
+    float z = getAccelerationZ();
+    
+    return (((float)atan2(x, z)) * 2.2);
+}
+
+float Accelerometer::getRadiansAngleY()
+{
+    float y = getAccelerationY();
+    float z = getAccelerationZ();
+    return (((float)atan2(y, z)) * 2.2);
+}
+
+float Accelerometer::getDegreesAngleX()
+{
+    return (getRadiansAngleX() / Accelerometer::getPI()) * 180.0;
+}
+
+float Accelerometer::getDegreesAngleY()
+{
+    return (getRadiansAngleY() / Accelerometer::getPI()) * 180.0;
+}
+
+float Accelerometer::getPI()
+{
+    return 3.14159265;
+}
diff -r 000000000000 -r 56b8c86181b1 Sensors/Accelerometer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Accelerometer.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,42 @@
+#ifndef ACCELEROMETER_H
+#define ACCELEROMETER_H
+
+#include <math.h>
+
+#include "ADXL345.h"
+
+class Accelerometer
+{
+    public:
+        Accelerometer(ADXL345* acceleHardware, float sensitivity);
+        ~Accelerometer(void);
+        
+        void updateZeroRates();
+        
+        void update();
+        void update(int samplesSize, int sampleDataRate, bool iswait);
+        
+        float getAccelerationX();
+        float getAccelerationY();
+        float getAccelerationZ();
+        
+        float getRadiansAngleX();
+        float getRadiansAngleY();
+        
+        float getDegreesAngleX();
+        float getDegreesAngleY();
+        
+        static float getPI();
+    
+    private:
+        ADXL345* acceleHardware;
+        
+        float sensitivity;        
+        float zeroRateX, zeroRateY, zeroRateZ;
+        
+        float rawX;
+        float rawY;
+        float rawZ;
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 Sensors/Gyroscope.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Gyroscope.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,108 @@
+#include "Gyroscope.h"
+
+Gyroscope::Gyroscope(L3G4200D* gyroHardware, float sensitivity, float dataRate)
+{
+    this->gyroHardware = gyroHardware;
+    this->sensitivity = sensitivity;
+    this->dataRate = dataRate;
+    
+    updateZeroRates();
+}
+
+Gyroscope::~Gyroscope(void)
+{
+}
+
+void Gyroscope::updateZeroRates()
+{
+    update(100, 10, true);
+    
+    zeroRateX = rawX;
+    zeroRateY = rawY;
+    zeroRateZ = rawZ;
+    
+    resetAngles();
+}
+
+void Gyroscope::update()
+{    
+    update(1, 1, false);
+}
+
+void Gyroscope::update(int samplesSize, int sampleDataRate, bool iswait)
+{
+    int axes[3] = {0, 0, 0};
+    
+    rawX = 0;
+    rawY = 0;
+    rawZ = 0;
+    
+    for (int i = 0; i < samplesSize; i++)
+    {
+        gyroHardware->getAxes(axes);
+        
+       //O valor do ângulo real para o ângulo medido está com uma diferença de 2.4 vezes maior.
+       rawX += ((gyroHardware->getX() * 2.55) / samplesSize);
+       rawY += ((gyroHardware->getY() * 2.55) / samplesSize);
+       rawZ += ((gyroHardware->getZ() * 2.55) / samplesSize);
+        
+        if(iswait)
+            wait_ms(sampleDataRate);
+    }
+    
+    angleX += getDegreesX() * dataRate;
+    angleY += getDegreesY() * dataRate;
+    angleZ += getDegreesZ() * dataRate;
+}
+
+void Gyroscope::resetAngles()
+{
+    angleX = 0;
+    angleY = 0;
+    angleZ = 0;
+}
+
+float Gyroscope::getDegreesX()
+{
+    return (rawX - zeroRateX) * sensitivity;
+}
+
+float Gyroscope::getDegreesY()
+{
+    return (rawY - zeroRateY) * sensitivity;
+}
+
+float Gyroscope::getDegreesZ()
+{
+    return (rawZ - zeroRateZ) * sensitivity;
+}
+
+float Gyroscope::getRadiansX()
+{
+    return getDegreesX() / 180.0 * PI;
+}
+
+float Gyroscope::getRadiansY()
+{
+    return getDegreesY() / 180.0 * PI;
+}
+
+float Gyroscope::getRadiansZ()
+{
+    return getDegreesZ() / 180.0 * PI;
+}
+
+float Gyroscope::getAngleX()
+{
+    return angleX;
+}
+
+float Gyroscope::getAngleY()
+{
+    return angleY;
+}
+
+float Gyroscope::getAngleZ()
+{
+    return angleZ;
+}
diff -r 000000000000 -r 56b8c86181b1 Sensors/Gyroscope.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Gyroscope.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,50 @@
+#ifndef GYROSCOPE_H
+#define GYROSCOPE_H
+
+#include "L3G4200D.h"
+
+#define PI  3.14159265
+
+class Gyroscope
+{
+    public:
+        Gyroscope(L3G4200D* gyroHardware, float sensitivity, float dataRate);
+        ~Gyroscope(void);
+        
+        void updateZeroRates();
+        
+        void update();
+        void update(int samplesSize, int sampleDataRate, bool iswait);
+        
+        void resetAngles();
+        
+        float getRadiansX();
+        float getRadiansY();
+        float getRadiansZ();
+        
+        float getDegreesX();
+        float getDegreesY();
+        float getDegreesZ();
+        
+        float getAngleX();
+        float getAngleY();
+        float getAngleZ();
+    
+    private:
+        L3G4200D* gyroHardware;
+    
+        float sensitivity;
+        float dataRate;
+        
+        float zeroRateX, zeroRateY, zeroRateZ;
+        
+        float rawX;
+        float rawY;
+        float rawZ;
+        
+        float angleX;
+        float angleY;
+        float angleZ;
+};
+
+#endif /* GYROSCOPE_H */
diff -r 000000000000 -r 56b8c86181b1 Sensors/L3G4200D.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/L3G4200D.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,164 @@
+
+#include "L3G4200D.h"
+
+L3G4200D::L3G4200D(PinName mosi, PinName miso, PinName sck, PinName cs) : _spi(mosi, miso, sck), _ncs (cs)
+{
+    _spi.frequency(2000000);
+    _spi.format(8,3);
+    _ncs = 1;
+    wait_us(500);
+    
+    oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG1), 0x6F);//-> Digital output data rate(ODR)=200Hz and Cut-Off 50Hz
+    oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG2), 0x00);
+    oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG3), 0x00);
+    oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG4), 0x10); // Full Scale selection = 500 dps
+    oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG5), 0x00);
+    
+    wait_us(500);
+}
+
+void L3G4200D::setPowerMode(char mode)
+{
+}
+
+int L3G4200D::getWhoAmI()
+{
+    return oneByteRead(L3G4200D_SPI_READ | (WHO_AM_I & 0x3F));
+}
+
+char L3G4200D::getTemperature()
+{
+    return (char)oneByteRead(L3G4200D_SPI_READ | (OUT_TEMP & 0x3F));
+}
+
+void L3G4200D::getAxes(int* axes)
+{
+    getData(axes, 3);
+}
+
+int L3G4200D::getAxisX()
+{
+    int axes[3];
+    
+    getData(axes, 3);
+    
+    return axes[0];
+}
+
+int L3G4200D::getAxisY()
+{
+    int axes[3];
+    
+    getData(axes, 3);
+    
+    return axes[1];
+}
+
+int L3G4200D::getAxisZ()
+{
+    int axes[3];
+    
+    getData(axes, 3);
+    
+    return axes[2];
+}
+
+void L3G4200D::getData(int* dataVector, int vectorSize)
+{
+    char* buffer = new char[vectorSize * 2];
+    
+    multiByteRead(L3G4200D_SPI_READ | L3G4200D_SPI_MULTI | (OUT_X_L & 0x3F), buffer, vectorSize * 2);
+    
+    for(int i = 0, j = 0; i < vectorSize; i++, j += 2)
+    {
+        dataVector[i] = (int16_t)((int)buffer[j+1] << 8 | (int)buffer[j]);
+    }
+    
+    free(buffer);
+}
+
+float L3G4200D::getX()
+{
+    signed short byte = 0;    
+    byte = (oneByteRead(OUT_X_H)&0xFF)<<8;
+    byte |= (oneByteRead(OUT_X_L)&0xFF);
+    return (float)byte;
+}
+
+float L3G4200D::getY()
+{
+    signed short byte = 0;    
+    byte = (oneByteRead(OUT_Y_H)&0xFF)<<8;
+    byte |= (oneByteRead(OUT_Y_L)&0xFF);
+    return (float)byte;
+}
+
+float L3G4200D::getZ()
+{
+    signed short byte = 0;    
+    byte = (oneByteRead(OUT_Z_H)&0xFF)<<8;
+    byte |= (oneByteRead(OUT_Z_L)&0xFF);
+    return (float)byte;
+}
+
+
+int L3G4200D::oneByteRead(int address) {
+ 
+    int tx = (L3G4200D_SPI_READ | (address & 0x3F));
+    int rx = 0;
+ 
+    _ncs = 0;
+    //Send address to read from.
+    _spi.write(tx);
+    //Read back contents of address.
+    rx = _spi.write(0x00);
+    _ncs = 1;
+ 
+    return rx;
+ 
+}
+ 
+void L3G4200D::oneByteWrite(int address, char data) {
+ 
+    int tx = (L3G4200D_SPI_WRITE | (address & 0x3F));
+ 
+    _ncs = 0;
+    //Send address to write to.
+    _spi.write(tx);
+    //Send data to be written.
+    _spi.write(data);
+    _ncs = 1;
+ 
+}
+ 
+void L3G4200D::multiByteRead(int startAddress, char* buffer, int size) {
+ 
+    int tx = (L3G4200D_SPI_READ | L3G4200D_SPI_MULTI | (startAddress & 0x3F));
+ 
+    _ncs = 0;
+    //Send address to start reading from.
+    _spi.write(tx);
+ 
+    for (int i = 0; i < size; i++) {
+        buffer[i] = _spi.write(0x00);
+    }
+ 
+    _ncs = 1;
+ 
+}
+ 
+void L3G4200D::multiByteWrite(int startAddress, char* buffer, int size) {
+ 
+    int tx = (L3G4200D_SPI_WRITE | L3G4200D_SPI_MULTI | (startAddress & 0x3F));
+ 
+    _ncs = 0;
+    //Send address to start reading from.
+    _spi.write(tx);
+ 
+    for (int i = 0; i < size; i++) {
+        buffer[i] = _spi.write(0x00);
+    }
+ 
+    _ncs = 1;
+ 
+}
diff -r 000000000000 -r 56b8c86181b1 Sensors/L3G4200D.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/L3G4200D.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,74 @@
+#ifndef L3G4200D_H
+#define L3G4200D_H
+
+#include "mbed.h"
+
+// Enderecos dos registradores
+#define WHO_AM_I        0x0F
+#define CTRL_REG1        0x20
+#define CTRL_REG2        0x21
+#define CTRL_REG3        0x22
+#define CTRL_REG4        0x23
+#define CTRL_REG5        0x24
+#define REFERENCE        0x25
+#define OUT_TEMP        0x26
+#define STATUS_REG        0x27
+#define OUT_X_L            0x28
+#define OUT_X_H            0x29
+#define OUT_Y_L            0x2A
+#define OUT_Y_H            0x2B
+#define OUT_Z_L            0x2C
+#define OUT_Z_H            0x2D
+#define FIFO_CTRL_REG    0x2E
+#define FIFO_SRC_REG    0x2F
+#define INT1_CFG        0x30
+#define INT1_SRC        0x31
+#define INT1_TSH_XH        0x32
+#define INT1_TSH_XL        0x33
+#define INT1_TSH_YH        0x34
+#define INT1_TSH_YL        0x35
+#define INT1_TSH_ZH        0x36
+#define INT1_TSH_ZL        0x37
+#define INT1_DURATION    0x38
+
+// Valores utilizados na comunicacao SPI para leitura e escrita
+#define L3G4200D_SPI_READ     0x80
+#define L3G4200D_SPI_WRITE    0x00
+#define L3G4200D_SPI_MULTI    0x60
+
+// Modos de operacoes
+#define POWER_DOWN    0
+#define SLEEP        1
+#define NORMAL        2 
+
+class L3G4200D
+{
+    public:
+        L3G4200D(PinName mosi, PinName miso, PinName sck, PinName cs);
+        
+        void setPowerMode(char mode);
+        
+        int getWhoAmI();
+        char getTemperature();
+        
+        void getAxes(int* axes);
+        int getAxisX();
+        int getAxisY();
+        int getAxisZ();
+        float getX();
+        float getY();
+        float getZ();
+        
+    private:
+        virtual void getData(int* dataVector, int vectorSize);        
+        SPI _spi;
+        DigitalOut _ncs;
+
+        int oneByteRead(int address);
+        void oneByteWrite(int address, char data);
+        void multiByteRead(int startAddress, char* buffer, int size);
+        void multiByteWrite(int startAddress, char* buffer, int size);
+        
+};
+
+#endif /* L3G4200D_H */
diff -r 000000000000 -r 56b8c86181b1 Serial/SerialBuffered.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serial/SerialBuffered.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,180 @@
+#include "SerialBuffered.h"
+
+/**
+ * Create a buffered serial class.
+ *
+ * @param tx A pin for transmit.
+ * @param rx A pin for receive.
+ */
+SerialBuffered::SerialBuffered(PinName tx, PinName rx) : Serial(tx, rx)
+{
+    indexContentStart = 0;
+    indexContentEnd = 0;
+    timeout = 1;
+    
+    attach(this, &SerialBuffered::handleInterrupt);
+}
+
+/**
+ * Destroy.
+ */
+SerialBuffered::~SerialBuffered()
+{
+}
+
+/**
+ * Set timeout for getc().
+ *
+ * @param ms milliseconds. (-1:Disable timeout)
+ */
+void SerialBuffered::setTimeout(int ms)
+{
+    timeout = ms;
+}
+
+/**
+ * Read requested bytes.
+ *
+ * @param bytes A pointer to a buffer.
+ * @param requested Length.
+ *
+ * @return Readed byte length.
+ */
+size_t SerialBuffered::readBytes(uint8_t *bytes, size_t requested)
+{
+    int i = 0;
+    
+    while (i < requested)
+    {
+        int c = getc();
+        
+        if (c < 0)
+        {
+            break;
+        }
+        
+        bytes[i] = c;
+        i++;
+    }
+    
+    return i;
+}
+
+/**
+ * Get a character.
+ *
+ * @return A character. (-1:timeout)
+ */
+int SerialBuffered::getc()
+{
+    timer.reset();
+    timer.start();
+    
+    while (indexContentStart == indexContentEnd)
+    {
+        wait_ms(1);
+        
+        if ((timeout > 0) && (timer.read_ms() > timeout))
+        {
+            /*
+             * Timeout occured.
+             */
+            // printf("Timeout occured.\n");
+            return EOF;
+        }
+    }
+    
+    timer.stop();
+
+    uint8_t result = buffer[indexContentStart++];
+    indexContentStart =  indexContentStart % BUFFERSIZE;
+
+    return result;
+}
+
+/**
+ * Returns 1 if there is a character available to read, otherwise.
+ */
+int SerialBuffered::readable()
+{
+    return indexContentStart != indexContentEnd;
+}
+
+void SerialBuffered::handleInterrupt()
+{
+    while (Serial::readable())
+    {
+        if (indexContentStart == ((indexContentEnd + 1) % BUFFERSIZE))
+        {
+            /*
+             * Buffer overrun occured.
+             */
+            // printf("Buffer overrun occured.\n");
+            Serial::getc();
+        }
+        else
+        {
+            buffer[indexContentEnd++] = Serial::getc();
+            indexContentEnd = indexContentEnd % BUFFERSIZE;
+        }
+    }
+}
+
+float SerialBuffered::f_readIntTo(char delimiter)
+{
+    char buffer[16];
+    int i;
+    float result;
+    for (i = 0;; i++)
+    {
+        //while (!readable());
+        
+        char number = getc();
+        
+        if (number == delimiter)
+            break;
+            
+        buffer[i] = number;     
+    }
+    
+    buffer[i-1] = '\0';
+    
+    result = atof(buffer);
+    
+    return result;
+}
+
+
+int SerialBuffered::i_readIntTo(char delimiter)
+{
+    char buffer[16];
+    int i;
+    float result;
+    for (i = 0;; i++)
+    {
+        //while (!readable());
+        
+        char number = getc();
+        
+        if (number == delimiter)
+            break;
+            
+        buffer[i] = number;     
+    }
+    
+    buffer[i-1] = '\0';
+    
+    result = atoi(buffer);
+    
+    return result;
+}
+
+void SerialBuffered::writeText(char* text)
+{
+    for (int i = 0; text[i] != '\0' && i < BUFFER_TEXT_SIZE; i++)
+    {    
+        while (!Serial::writeable());
+
+        Serial::putc(text[i]);
+    }
+}
diff -r 000000000000 -r 56b8c86181b1 Serial/SerialBuffered.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serial/SerialBuffered.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,71 @@
+#ifndef _SERIAL_BUFFERED_H_
+#define _SERIAL_BUFFERED_H_
+
+#include "mbed.h"
+
+/**
+ * Buffered serial class.
+ */
+class SerialBuffered : public Serial
+{
+    public:
+        /**
+         * Create a buffered serial class.
+         *
+         * @param tx A pin for transmit.
+         * @param rx A pin for receive.
+         */
+        SerialBuffered(PinName tx, PinName rx);
+
+        /**
+         * Destroy.
+         */
+        virtual ~SerialBuffered();
+
+        /**
+         * Get a character.
+         *
+         * @return A character. (-1:timeout)
+         */
+        int getc();
+
+        /**
+         * Returns 1 if there is a character available to read, otherwise.
+         */
+        int readable();
+
+        /**
+         * Set timeout for getc().
+         *
+         * @param ms milliseconds. (-1:Disable timeout)
+         */
+        void setTimeout(int ms);
+
+        /**
+         * Read requested bytes.
+         *
+         * @param bytes A pointer to a buffer.
+         * @param requested Length.
+         *
+         * @return Readed byte length.
+         */
+        size_t readBytes(uint8_t *bytes, size_t requested);
+        
+        float f_readIntTo(char delimiter);
+        int i_readIntTo(char delimiter);
+        void writeText(char* text);
+
+    private:
+        void handleInterrupt();
+        
+        static const int BUFFERSIZE = 2048;
+        static const int BUFFER_TEXT_SIZE = 255;
+        
+        uint8_t buffer[BUFFERSIZE];            // points at a circular buffer, containing data from m_contentStart, for m_contentSize bytes, wrapping when you get to the end
+        uint16_t indexContentStart;   // index of first bytes of content
+        uint16_t indexContentEnd;     // index of bytes after last byte of content
+        int timeout;
+        Timer timer;
+};
+
+#endif
diff -r 000000000000 -r 56b8c86181b1 Util.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Util.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,34 @@
+#include "Util.h"
+
+int clampInt(int value, int min, int max)
+{
+    if (value < min)
+        return min;
+    else if (value > max)
+        return max;
+        
+    return value;
+}
+
+float clampFloat(float value, float min, float max)
+{
+    if (value < min)
+        return min;
+    else if (value > max)
+        return max;
+        
+    return value;
+}
+/*
+float abs(float value)
+{
+    if(value < 0)
+        return ((-1)*value);
+ 
+    return value;
+}
+*/
+float norm(float val1, float val2, float val3)
+{
+    return sqrt(pow(val1,2)+pow(val2,2)+pow(val3,2));
+}
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 Util.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Util.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,10 @@
+#ifndef UTIL_H
+#define UTIL_H
+
+#include <math.h>
+
+int clampInt(int value, int min, int max);
+float clampFloat(float value, float min, float max);
+//float abs(float value);
+float norm(float val1, float val2, float val3);
+#endif /* UTIL_H */
diff -r 000000000000 -r 56b8c86181b1 Watchdog/Watchdog.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Watchdog/Watchdog.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,16 @@
+#include "Watchdog.h"
+#include "mbed.h"
+
+void Watchdog::kick(float s) 
+{
+    LPC_WDT->WDCLKSEL = 0x1;                // Set CLK src to PCLK
+    uint32_t clk = SystemCoreClock / 16;    // WD has a fixed /4 prescaler, PCLK default is /4
+    LPC_WDT->WDTC = s * (float)clk;
+    LPC_WDT->WDMOD = 0x3;                   // Enabled and Reset
+    kick();
+}
+void Watchdog::kick() 
+{
+    LPC_WDT->WDFEED = 0xAA;
+    LPC_WDT->WDFEED = 0x55;
+}
diff -r 000000000000 -r 56b8c86181b1 Watchdog/Watchdog.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Watchdog/Watchdog.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,11 @@
+#ifndef _WATCHDOG_H_
+#define _WATCHDOG_H_
+
+class Watchdog
+{
+public:
+    void kick(float s);
+    void kick();
+};
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,315 @@
+#include "stddef.h"
+   
+const float dt = 0.005;
+
+Gyroscope gyroscope(new L3G4200D(p5, p6, p7, p8), 0.0175, dt);;
+Accelerometer accelerometer(new ADXL345(p5, p6, p7, p11), 256);;
+ComplementaryFilter ComplementaryFilterX(dt, 0.9995, 0.0005);;
+ComplementaryFilter ComplementaryFilterY(dt, 0.9995, 0.0005);;
+PID pidX(0.0007f, 0.008f, 0.0004f, dt);
+PID pidY(0.0007f, 0.008f, 0.0004f, dt);
+
+bool compute_pid        = false;
+bool motors_armed       = false;
+bool start_update_loop  = false;
+
+float accX          = 0.0;
+float accY          = 0.0;
+float gyroX         = 0.0;
+float gyroY         = 0.0;
+float setPoint_x    = 0.0;
+float setPoint_y    = 0.0;
+float control_x     = 0.0;
+float control_y     = 0.0;
+
+float ComplementarAngle_x = 0.0;
+float ComplementarAngle_y = 0.0;
+
+float heading = 0.0;
+float altura = 0.0;
+
+int state = 0;
+//float motorCoefA[5] = {1.4071, -2.7620, 1.9864, 0.3664, 0.0150};
+//float motorCoefB[5] = {1.4433, -2.8580, 2.0790, 0.3207, 0.0111};
+//float motorCoefC[5] = {1.4736, -2.8773, 2.1176, 0.2639, 0.0131};
+//float motorCoefD[5] = {1.5427, -2.9893, 2.1539, 0.2750, 0.0119};
+
+//Motor motorA(p26, 42, motorCoefA, 15, 60);
+//Motor motorC(p24, 42, motorCoefC, 15, 60);
+//Motor motorB(p25, 42, motorCoefB, 15, 60);
+//Motor motorD(p23, 42, motorCoefD, 15, 60);
+Motor motorA(p26, 42);
+Motor motorC(p24, 42);
+Motor motorB(p25, 42);
+Motor motorD(p23, 42);
+
+//Dummy
+DigitalOut dummy(p21);
+
+DigitalOut powerLed(LED1);
+DigitalOut initLed(LED2);
+DigitalOut statusLed1(LED3);
+DigitalOut statusLed2(LED4);
+
+Ticker updater;
+
+volatile unsigned int sysCount;
+volatile unsigned int pwmCount;
+volatile unsigned int mainCount;
+volatile unsigned int loop_time;
+
+//SerialBuffered pc(USBTX, USBRX);
+SerialBuffered pc(p28, p27);
+char buffer[64];
+char cmd = CMD_WAITING;
+/*
+ * Sets the Motors
+ */
+void motors_setPower()
+{
+
+    motorB.setPowerLin( FLIGHT_THRESHOLD - (control_x*100) + heading + altura );
+    motorD.setPowerLin( FLIGHT_THRESHOLD + (control_x*100) + heading + altura );
+    
+    motorA.setPowerLin( FLIGHT_THRESHOLD - (control_y*100) - heading + altura );
+    motorC.setPowerLin( FLIGHT_THRESHOLD + (control_y*100) - heading + altura );
+
+}
+
+/*
+ * Update Loop
+ * Periodically Occurs every 5 milliseconds
+ */
+void update_lood()
+{
+    gyroscope.update();
+    accelerometer.update();
+
+    accX  =        accelerometer.getDegreesAngleX();
+    accY  =        accelerometer.getDegreesAngleY();
+    gyroX = (-1) * gyroscope.getDegreesX();
+    gyroY =        gyroscope.getDegreesY();
+    
+    //gyroAngle = gyroscope->getAngleY();
+    
+    ComplementarAngle_x = ComplementaryFilterX.update(accY, gyroX);
+    ComplementarAngle_y = ComplementaryFilterY.update(accX, gyroY);
+
+
+    if(compute_pid) 
+    {
+        control_x = pidX.compute(setPoint_x, ComplementarAngle_x);
+        control_y = pidY.compute(setPoint_y, ComplementarAngle_y);
+    }
+
+    motors_setPower();
+}
+
+/*
+ * PWM Loop
+ * Periodically Occurs every 1 millisecond
+ */
+void pwm_lood()
+{
+    if(pwmCount == 2 && !motors_armed) 
+    {
+        dummy = 0;
+    } 
+    else if(pwmCount == 4 && motors_armed) 
+    {
+        dummy = 0;
+    } 
+    else if(pwmCount == 40 )//tempo igual a '20' e '0' (termina os 20 mili e inicia os próximos 20 mili) 
+    { 
+        dummy = 1;
+        pwmCount = 0;
+    }
+}
+
+/*
+ * Main Loop
+ * Handle the update loop and the pwm loop
+ */
+void update()
+{
+    sysCount+=1;
+    pwmCount+=1;
+    if(sysCount >= 10 && start_update_loop) 
+    {
+        update_lood();
+        sysCount = 0;
+    }
+
+    pwm_lood();
+}
+
+bool setPointAdjustment(int currentLoopTime)
+{   
+    bool reset = false;
+    /* 2 graus por segundo
+       2 - 1
+    taxa - 0.010 -> loop_time     */
+    
+    float taxa = 0.04; 
+    
+    switch(state) /* Máquina de estados */
+    {   
+        case 0:         
+        case 6:
+        case 7: 
+        case 8:
+        case 14: 
+        case 15:  
+            if(setPoint_x < 0)
+                setPoint_x += taxa;
+            else if(setPoint_x > 0)
+                setPoint_x -= taxa; 
+                 
+            if(setPoint_y < 0)
+                setPoint_y += taxa;
+            else if(setPoint_y > 0)
+                setPoint_y -= taxa; 
+        break;
+        case 1:
+        case 2:
+            if(setPoint_x < 20)
+                setPoint_x += taxa;
+            else if(setPoint_x > 20)
+                setPoint_x -= taxa;            
+        break;
+        case 3:
+        case 4:
+        case 5:
+            if(setPoint_x < -20)
+                setPoint_x += taxa;
+            else if(setPoint_x > -20)
+                setPoint_x -= taxa;
+        break;
+        case 9:
+        case 10:
+            if(setPoint_y < 20)
+                setPoint_y += taxa;
+            else if(setPoint_y > 20)
+                setPoint_y -= taxa;            
+        break;
+        case 11:
+        case 12:
+        case 13:
+            if(setPoint_y < -20)
+                setPoint_y += taxa;
+            else if(setPoint_y > -20)
+                setPoint_y -= taxa;    
+        break;
+        
+    }
+    
+    
+    if(currentLoopTime >= 5000)
+    {
+        state++;
+        if(state > 15) 
+            state = 0;
+        reset = true;
+    }
+    return reset;
+}
+
+/*
+ * Main
+ */
+int main()
+{
+    pc.baud(PC_BAUD_RATE);
+    
+    //Leds de controle
+    powerLed = 0;
+    initLed  = 0;
+    statusLed1 = 0;
+    statusLed2 = 0;
+    
+    //Contadores
+    sysCount = 0;
+    pwmCount = 0;
+    mainCount = 0;
+    
+    //Variáveis lógicas
+    start_update_loop = false;
+    motors_armed = false;
+    /*
+    gyroscope = new Gyroscope(new L3G4200D(p5, p6, p7, p8), 0.0175, dt);
+    accelerometer = new Accelerometer(new ADXL345(p5, p6, p7, p11), 256);
+    ComplementaryFilterX = new ComplementaryFilter(dt, 0.9995, 0.0005);
+    ComplementaryFilterY = new ComplementaryFilter(dt, 0.9995, 0.0005);
+    */
+    powerLed = 1;
+
+    wait(10);
+    accelerometer.updateZeroRates();
+    gyroscope.updateZeroRates();
+
+    initLed  = 1;
+
+    motors_setPower();
+    updater.attach(&update, 0.0005);
+    wait(2);
+    motors_armed = true;
+    wait(7);
+
+    //msg_update = true;
+    compute_pid = true;
+    start_update_loop = true;
+    loop_time = 10;
+    int i = 0;
+    int j = 0;
+    while (true) 
+    {
+        wait_ms(loop_time);
+        j += loop_time;
+        if(setPointAdjustment(j))
+            j = 0;
+        
+        /*
+        if (pc.readable())
+            cmd = pc.getc();
+
+        switch (cmd) 
+        {
+            case CMD_UP:
+            heading+= 0.5;
+            sprintf((char*)buffer, "heading %f\r\n", heading);    
+            pc.writeText(buffer);
+            break;
+            
+            case CMD_DOWN:
+            heading-= 0.5;
+            sprintf((char*)buffer, "heading %f\r\n", heading);    
+            pc.writeText(buffer);
+            break;
+            
+            case CMD_UP2:
+            altura += 0.5;
+            sprintf((char*)buffer, "altura %f\r\n", altura);    
+            pc.writeText(buffer);
+            break;
+            
+            case CMD_DOWN2:
+            altura -= 0.5;
+            sprintf((char*)buffer, "altura %f\r\n", altura);    
+            pc.writeText(buffer);
+            break; 
+            
+        }
+        
+        cmd = CMD_WAITING;
+        */
+        sprintf((char*)buffer, "%f %f %f %f \n", ComplementarAngle_x, setPoint_x, ComplementarAngle_y, setPoint_y);    
+        pc.writeText(buffer);
+        
+        i += loop_time;
+        if(i >= 200)
+        {   statusLed2 = statusLed1;
+            statusLed1 = !statusLed1;
+            i = 0;
+        }
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912
\ No newline at end of file
diff -r 000000000000 -r 56b8c86181b1 stddef.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/stddef.h	Tue May 21 14:12:13 2013 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+
+#include "Util.h"
+
+#include "Sensors/L3G4200D.h"
+#include "Sensors/Gyroscope.h"
+
+#include "Sensors/ADXL345.h"
+#include "Sensors/Accelerometer.h"
+
+#include "Serial/SerialBuffered.h"
+
+//#include "Filters/KalmanFilter.h"
+#include "Filters/ComplementaryFilter.h"
+//#include "Filters/Kalman.h"
+
+#include "Controllers/PID.h"
+
+//#include "Controllers/Fuzzy/MembershipFunction.h"
+//#include "Controllers/Fuzzy/FuzzyIO.h"
+//#include "Controllers/Fuzzy/FuzzyController.h"
+
+#include "Hardware/Motor.h"
+
+//#include "Watchdog/Watchdog.h"
+
+//#include "GetTickCount/GetTickCount.h"
+// Parametros gerais de configuracoes
+
+#define PC_BAUD_RATE        115200
+//#define GPS_BAUD_RATE       9600
+//#define SONAR_BAUD_RATE     57600
+
+//#define MSG_UPDATE_TIME     5//In Mili
+
+// Parametros de protocolo
+
+//#define CMD_START         0x42//'B'
+//#define CMD_SET_PID       0x43//'C'
+//#define CMD_SET_MOTOR     0x46//'F'
+//#define CMD_GET_PID       0x47//'G'
+#define CMD_WAITING       0x48//'H'
+
+//#define CMD_MOTOR_SEND     0x49//'I'
+//#define CMD_MOTOR1_UP     0x49//'I'
+//#define CMD_MOTOR1_DOWN   0x4A//'J'
+     
+//#define CMD_MOTOR2_UP     0x4B//'K'
+//#define CMD_MOTOR2_DOWN   0x4C//'L'
+        
+//#define CMD_MOTOR3_UP     0x4D//'M'
+//#define CMD_MOTOR3_DOWN   0x4E//'N'
+        
+//#define CMD_MOTOR4_UP     0x4F//'O'
+//#define CMD_MOTOR4_DOWN   0x50//'P'
+
+#define CMD_DOWN            0x41//'A'
+#define CMD_UP              0x51//'Q'
+
+#define CMD_DOWN2           0x44//'D'
+#define CMD_UP2             0x45//'E'
+
+//#define CMD_CHART        0x52//'R'
+
+//#define CMD_MSG_END       0x7C//'|'
+
+// Parametros de voo
+
+//#define MIN_THRESHOLD     1
+//#define MAX_THRESHOLD     100
+#define FLIGHT_THRESHOLD  30
+//#define POWER_THRESHOLD   0
+//#define INITIAL_LOOPS     500//1000
+
+//Soft reset
+//extern "C" void mbed_reset();
\ No newline at end of file