Revision 0:56b8c86181b1, committed 2013-05-21

Comitter:

jose_claudiojr

Date:

Tue May 21 14:12:13 2013 +0000

Commit message:

Quadcopter code with accelerometer and gyroscope.

Changed in this revision

--- /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;
+}

--- /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

--- /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;
+}

--- /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

--- /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;
+}

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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;
+}

--- /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

--- /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

--- /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

--- /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;
+}

--- /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

--- /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;
+ 
+}

--- /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

--- /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;
+}

--- /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

--- /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;
+}

--- /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 */

--- /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;
+ 
+}

--- /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 */

--- /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]);
+    }
+}

--- /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

--- /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

--- /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 */

--- /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;
+}

--- /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

--- /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

--- /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

--- /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