follow wall 1

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
khaledelmadawi
Date:
Tue Apr 08 13:23:06 2014 +0000
Commit message:
follow wall 1

Changed in this revision

Motor/Motor.cpp Show annotated file Show diff for this revision Revisions of this file
Motor/Motor.h Show annotated file Show diff for this revision Revisions of this file
PID/PID.cpp Show annotated file Show diff for this revision Revisions of this file
PID/PID.h Show annotated file Show diff for this revision Revisions of this file
QEI/QEI.cpp Show annotated file Show diff for this revision Revisions of this file
QEI/QEI.h Show annotated file Show diff for this revision Revisions of this file
SHARPIR.cpp Show annotated file Show diff for this revision Revisions of this file
SHARPIR.h Show annotated file Show diff for this revision Revisions of this file
SpaceSensor.cpp Show annotated file Show diff for this revision Revisions of this file
SpaceSensor.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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.cpp	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,60 @@
+/* mbed simple H-bridge motor controller
+ * Copyright (c) 2007-2010, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Motor.h"
+
+Motor::Motor(PinName pwm, PinName fwd, PinName rev):
+        _pwm(pwm), _fwd(fwd), _rev(rev) {
+
+    // Set initial condition of PWM
+    _pwm.period(0.001);
+    _pwm = 0;
+
+    // Initial condition of output enables
+    _fwd = 0;
+    _rev = 0;
+}
+
+void Motor::speed(float speed) {
+    _fwd = (speed > 0.0);
+    _rev = (speed < 0.0);
+    _pwm = abs(speed);
+}
+
+void Motor::period(float period){
+
+    _pwm.period(period);
+
+}
+
+void Motor::brake(int highLow){
+
+    if(highLow == BRAKE_HIGH){
+        _fwd = 1;
+        _rev = 1;
+    }
+    else if(highLow == BRAKE_LOW){
+        _fwd = 0;
+        _rev = 0;
+    }
+
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.h	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,75 @@
+/* mbed simple H-bridge motor controller
+ * Copyright (c) 2007-2010, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef MBED_MOTOR_H
+#define MBED_MOTOR_H
+
+#include "mbed.h"
+
+#define BRAKE_HIGH 1
+#define BRAKE_LOW  0
+
+/** Interface to control a standard DC motor 
+ * with an H-bridge using a PwmOut and 2 DigitalOuts
+ */
+class Motor {
+public:
+
+    /** Create a motor control interface    
+     *
+     * @param pwm A PwmOut pin, driving the H-bridge enable line to control the speed
+     * @param fwd A DigitalOut, set high when the motor should go forward
+     * @param rev A DigitalOut, set high when the motor should go backwards
+     */
+    Motor(PinName pwm, PinName fwd, PinName rev);
+    
+    /** Set the speed of the motor
+     * 
+     * @param speed The speed of the motor as a normalised value between -1.0 and 1.0
+     */
+    void speed(float speed);
+    
+    /** Set the period of the pwm duty cycle.
+     *
+     * Wrapper for PwmOut::period()
+     *
+     * @param seconds - Pwm duty cycle in seconds.
+     */
+    void period(float period);
+    
+    /** Brake the H-bridge to GND or VCC.
+     * 
+     * Defaults to breaking to VCC.
+     *
+     * Brake to GND => inA = inB = 0
+     * Brake to VCC => inA = inB = 1
+     */
+    void brake(int highLow = BRAKE_HIGH);
+
+protected:
+    PwmOut _pwm;
+    DigitalOut _fwd;
+    DigitalOut _rev;
+
+};
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID.cpp	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,324 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ * 
+ * A PID controller is a widely used feedback controller commonly found in
+ * industry.
+ *
+ * This library is a port of Brett Beauregard's Arduino PID library:
+ *
+ *  http://www.arduino.cc/playground/Code/PIDLibrary
+ *
+ * The wikipedia article on PID controllers is a good place to start on
+ * understanding how they work:
+ *
+ *  http://en.wikipedia.org/wiki/PID_controller
+ *
+ * For a clear and elegant explanation of how to implement and tune a
+ * controller, the controlguru website by Douglas J. Cooper (who also happened
+ * to be Brett's controls professor) is an excellent reference:
+ *
+ *  http://www.controlguru.com/
+ */
+
+/**
+ * Includes
+ */
+#include "PID.h"
+
+PID::PID(float Kc, float tauI, float tauD, float interval) {
+
+    usingFeedForward = false;
+    inAuto           = false;
+
+    //Default the limits to the full range of I/O: 3.3V
+    //Make sure to set these to more appropriate limits for
+    //your application.
+    setInputLimits(0.0, 3.3);
+    setOutputLimits(0.0, 3.3);
+
+    tSample_ = interval;
+
+    setTunings(Kc, tauI, tauD);
+
+    setPoint_             = 0.0;
+    processVariable_      = 0.0;
+    prevProcessVariable_  = 0.0;
+    controllerOutput_     = 0.0;
+    prevControllerOutput_ = 0.0;
+
+    accError_ = 0.0;
+    bias_     = 0.0;
+    
+    realOutput_ = 0.0;
+
+}
+
+void PID::setInputLimits(float inMin, float inMax) {
+
+    //Make sure we haven't been given impossible values.
+    if (inMin >= inMax) {
+        return;
+    }
+
+    //Rescale the working variables to reflect the changes.
+    prevProcessVariable_ *= (inMax - inMin) / inSpan_;
+    accError_            *= (inMax - inMin) / inSpan_;
+
+    //Make sure the working variables are within the new limits.
+    if (prevProcessVariable_ > 1) {
+        prevProcessVariable_ = 1;
+    } else if (prevProcessVariable_ < 0) {
+        prevProcessVariable_ = 0;
+    }
+
+    inMin_  = inMin;
+    inMax_  = inMax;
+    inSpan_ = inMax - inMin;
+
+}
+
+void PID::setOutputLimits(float outMin, float outMax) {
+
+    //Make sure we haven't been given impossible values.
+    if (outMin >= outMax) {
+        return;
+    }
+
+    //Rescale the working variables to reflect the changes.
+    prevControllerOutput_ *= (outMax - outMin) / outSpan_;
+
+    //Make sure the working variables are within the new limits.
+    if (prevControllerOutput_ > 1) {
+        prevControllerOutput_ = 1;
+    } else if (prevControllerOutput_ < 0) {
+        prevControllerOutput_ = 0;
+    }
+
+    outMin_  = outMin;
+    outMax_  = outMax;
+    outSpan_ = outMax - outMin;
+
+}
+
+void PID::setTunings(float Kc, float tauI, float tauD) {
+
+    //Verify that the tunings make sense.
+    if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
+        return;
+    }
+
+    //Store raw values to hand back to user on request.
+    pParam_ = Kc;
+    iParam_ = tauI;
+    dParam_ = tauD;
+
+    float tempTauR;
+
+    if (tauI == 0.0) {
+        tempTauR = 0.0;
+    } else {
+        tempTauR = (1.0 / tauI) * tSample_;
+    }
+
+    //For "bumpless transfer" we need to rescale the accumulated error.
+    if (inAuto) {
+        if (tempTauR == 0.0) {
+            accError_ = 0.0;
+        } else {
+            accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
+        }
+    }
+
+    Kc_   = Kc;
+    tauR_ = tempTauR;
+    tauD_ = tauD / tSample_;
+
+}
+
+void PID::reset(void) {
+
+    float scaledBias = 0.0;
+
+    if (usingFeedForward) {
+        scaledBias = (bias_ - outMin_) / outSpan_;
+    } else {
+        scaledBias = (realOutput_ - outMin_) / outSpan_;
+    }
+
+    prevControllerOutput_ = scaledBias;
+    prevProcessVariable_  = (processVariable_ - inMin_) / inSpan_;
+
+    //Clear any error in the integral.
+    accError_ = 0;
+
+}
+
+void PID::setMode(int mode) {
+
+    //We were in manual, and we just got set to auto.
+    //Reset the controller internals.
+    if (mode != 0 && !inAuto) {
+        reset();
+    }
+
+    inAuto = (mode != 0);
+
+}
+
+void PID::setInterval(float interval) {
+
+    if (interval > 0) {
+        //Convert the time-based tunings to reflect this change.
+        tauR_     *= (interval / tSample_);
+        accError_ *= (tSample_ / interval);
+        tauD_     *= (interval / tSample_);
+        tSample_   = interval;
+    }
+
+}
+
+void PID::setSetPoint(float sp) {
+
+    setPoint_ = sp;
+
+}
+
+void PID::setProcessValue(float pv) {
+
+    processVariable_ = pv;
+
+}
+
+void PID::setBias(float bias){
+
+    bias_ = bias;
+    usingFeedForward = 1;
+
+}
+
+float PID::compute() {
+
+    //Pull in the input and setpoint, and scale them into percent span.
+    float scaledPV = (processVariable_ - inMin_) / inSpan_;
+
+    if (scaledPV > 1.0) {
+        scaledPV = 1.0;
+    } else if (scaledPV < 0.0) {
+        scaledPV = 0.0;
+    }
+
+    float scaledSP = (setPoint_ - inMin_) / inSpan_;
+    if (scaledSP > 1.0) {
+        scaledSP = 1;
+    } else if (scaledSP < 0.0) {
+        scaledSP = 0;
+    }
+
+    float error = scaledSP - scaledPV;
+
+    //Check and see if the output is pegged at a limit and only
+    //integrate if it is not. This is to prevent reset-windup.
+    if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
+        accError_ += error;
+    }
+
+    //Compute the current slope of the input signal.
+    float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
+
+    float scaledBias = 0.0;
+
+    if (usingFeedForward) {
+        scaledBias = (bias_ - outMin_) / outSpan_;
+    }
+
+    //Perform the PID calculation.
+    controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
+
+    //Make sure the computed output is within output constraints.
+    if (controllerOutput_ < 0.0) {
+        controllerOutput_ = 0.0;
+    } else if (controllerOutput_ > 1.0) {
+        controllerOutput_ = 1.0;
+    }
+
+    //Remember this output for the windup check next time.
+    prevControllerOutput_ = controllerOutput_;
+    //Remember the input for the derivative calculation next time.
+    prevProcessVariable_  = scaledPV;
+
+    //Scale the output from percent span back out to a real world number.
+    return ((controllerOutput_ * outSpan_) + outMin_);
+
+}
+
+float PID::getInMin() {
+
+    return inMin_;
+
+}
+
+float PID::getInMax() {
+
+    return inMax_;
+
+}
+
+float PID::getOutMin() {
+
+    return outMin_;
+
+}
+
+float PID::getOutMax() {
+
+    return outMax_;
+
+}
+
+float PID::getInterval() {
+
+    return tSample_;
+
+}
+
+float PID::getPParam() {
+
+    return pParam_;
+
+}
+
+float PID::getIParam() {
+
+    return iParam_;
+
+}
+
+float PID::getDParam() {
+
+    return dParam_;
+
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID.h	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,213 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ * 
+ * A PID controller is a widely used feedback controller commonly found in
+ * industry.
+ *
+ * This library is a port of Brett Beauregard's Arduino PID library:
+ *
+ *  http://www.arduino.cc/playground/Code/PIDLibrary
+ *
+ * The wikipedia article on PID controllers is a good place to start on
+ * understanding how they work:
+ *
+ *  http://en.wikipedia.org/wiki/PID_controller
+ *
+ * For a clear and elegant explanation of how to implement and tune a
+ * controller, the controlguru website by Douglas J. Cooper (who also happened
+ * to be Brett's controls professor) is an excellent reference:
+ *
+ *  http://www.controlguru.com/
+ */
+
+#ifndef PID_H
+#define PID_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define MANUAL_MODE 0
+#define AUTO_MODE   1
+
+/**
+ * Proportional-integral-derivative controller.
+ */
+class PID {
+
+public:
+
+    /**
+     * Constructor.
+     *
+     * Sets default limits [0-3.3V], calculates tuning parameters, and sets
+     * manual mode with no bias.
+     *
+     * @param Kc - Tuning parameter
+     * @param tauI - Tuning parameter
+     * @param tauD - Tuning parameter
+     * @param interval PID calculation performed every interval seconds.
+     */
+    PID(float Kc, float tauI, float tauD, float interval);
+
+    /**
+     * Scale from inputs to 0-100%.
+     *
+     * @param InMin The real world value corresponding to 0%.
+     * @param InMax The real world value corresponding to 100%.
+     */
+    void setInputLimits(float inMin , float inMax);
+
+    /**
+     * Scale from outputs to 0-100%.
+     *
+     * @param outMin The real world value corresponding to 0%.
+     * @param outMax The real world value corresponding to 100%.
+     */
+    void setOutputLimits(float outMin, float outMax);
+
+    /**
+     * Calculate PID constants.
+     *
+     * Allows parameters to be changed on the fly without ruining calculations.
+     *
+     * @param Kc - Tuning parameter
+     * @param tauI - Tuning parameter
+     * @param tauD - Tuning parameter
+     */
+    void setTunings(float Kc, float tauI, float tauD);
+
+    /**
+     * Reinitializes controller internals. Automatically
+     * called on a manual to auto transition.
+     */
+    void reset(void);
+    
+    /**
+     * Set PID to manual or auto mode.
+     *
+     * @param mode        0 -> Manual
+     *             Non-zero -> Auto
+     */
+    void setMode(int mode);
+    
+    /**
+     * Set how fast the PID loop is run.
+     *
+     * @param interval PID calculation peformed every interval seconds.
+     */
+    void setInterval(float interval);
+    
+    /**
+     * Set the set point.
+     *
+     * @param sp The set point as a real world value.
+     */
+    void setSetPoint(float sp);
+    
+    /**
+     * Set the process value.
+     *
+     * @param pv The process value as a real world value.
+     */
+    void setProcessValue(float pv);
+    
+    /**
+     * Set the bias.
+     *
+     * @param bias The bias for the controller output.
+     */
+    void setBias(float bias);
+
+    /**
+     * PID calculation.
+     *
+     * @return The controller output as a float between outMin and outMax.
+     */
+    float compute(void);
+
+    //Getters.
+    float getInMin();
+    float getInMax();
+    float getOutMin();
+    float getOutMax();
+    float getInterval();
+    float getPParam();
+    float getIParam();
+    float getDParam();
+
+private:
+
+    bool usingFeedForward;
+    bool inAuto;
+
+    //Actual tuning parameters used in PID calculation.
+    float Kc_;
+    float tauR_;
+    float tauD_;
+    
+    //Raw tuning parameters.
+    float pParam_;
+    float iParam_;
+    float dParam_;
+    
+    //The point we want to reach.
+    float setPoint_;         
+    //The thing we measure.
+    float processVariable_;  
+    float prevProcessVariable_;
+    //The output that affects the process variable.
+    float controllerOutput_; 
+    float prevControllerOutput_;
+
+    //We work in % for calculations so these will scale from
+    //real world values to 0-100% and back again.
+    float inMin_;
+    float inMax_;
+    float inSpan_;
+    float outMin_;
+    float outMax_;
+    float outSpan_;
+
+    //The accumulated error, i.e. integral.
+    float accError_;
+    //The controller output bias.
+    float bias_;
+
+    //The interval between samples.
+    float tSample_;          
+
+    //Controller output as a real world value.
+    volatile float realOutput_;
+
+};
+
+#endif /* PID_H */
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI/QEI.cpp	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,289 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Quadrature Encoder Interface.
+ *
+ * A quadrature encoder consists of two code tracks on a disc which are 90
+ * degrees out of phase. It can be used to determine how far a wheel has
+ * rotated, relative to a known starting position.
+ *
+ * Only one code track changes at a time leading to a more robust system than
+ * a single track, because any jitter around any edge won't cause a state
+ * change as the other track will remain constant.
+ *
+ * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
+ * and paper code tracks consisting of alternating black and white sections;
+ * alternatively, complete disk and PCB emitter/receiver encoder systems can
+ * be bought, but the interface, regardless of implementation is the same.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |  ^  |     |     |     |     |
+ *            ---+  ^  +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  +-----+     +-----+     +-----+
+ * Channel B     ^  |     |     |     |     |     |
+ *            ------+     +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  ^
+ *               90deg
+ *
+ * The interface uses X2 encoding by default which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * channel A.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^     ^     ^     ^     ^
+ *               ^     ^     ^     ^     ^
+ * Pulse count 0 1     2     3     4     5  ...
+ *
+ * This interface can also use X4 encoding which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * either channel.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ *
+ * It defaults
+ *
+ * An optional index channel can be used which determines when a full
+ * revolution has occured.
+ *
+ * If a 4 pules per revolution encoder was used, with X4 encoding,
+ * the following would be observed.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  +--+  ^  ^  +--+  ^
+ *               ^  ^  ^  |  |  ^  ^  |  |  ^
+ * Index      ------------+  +--------+  +-----------
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ * Rev.  count 0          1           2
+ *
+ * Rotational position in degrees can be calculated by:
+ *
+ * (pulse count / X * N) * 360
+ *
+ * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
+ * of pulses per revolution.
+ *
+ * Linear position can be calculated by:
+ *
+ * (pulse count / X * N) * (1 / PPI)
+ *
+ * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
+ * pulses per revolution, and PPI is pulses per inch, or the equivalent for
+ * any other unit of displacement. PPI can be calculated by taking the
+ * circumference of the wheel or encoder disk and dividing it by the number
+ * of pulses per revolution.
+ */
+
+/**
+ * Includes
+ */
+#include "QEI.h"
+
+QEI::QEI(PinName channelA,
+         PinName channelB,
+         PinName index,
+         int pulsesPerRev,
+         Encoding encoding) : channelA_(channelA), channelB_(channelB),
+        index_(index) {
+
+    pulses_       = 0;
+    revolutions_  = 0;
+    pulsesPerRev_ = pulsesPerRev;
+    encoding_     = encoding;
+
+    //Workout what the current state is.
+    int chanA = channelA_.read();
+    int chanB = channelB_.read();
+
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+    prevState_ = currState_;
+
+    //X2 encoding uses interrupts on only channel A.
+    //X4 encoding uses interrupts on      channel A,
+    //and on channel B.
+    channelA_.rise(this, &QEI::encode);
+    channelA_.fall(this, &QEI::encode);
+
+    //If we're using X4 encoding, then attach interrupts to channel B too.
+    if (encoding == X4_ENCODING) {
+        channelB_.rise(this, &QEI::encode);
+        channelB_.fall(this, &QEI::encode);
+    }
+    //Index is optional.
+    if (index !=  NC) {
+        index_.rise(this, &QEI::index);
+    }
+
+}
+
+void QEI::reset(void) {
+
+    pulses_      = 0;
+    revolutions_ = 0;
+
+}
+
+int QEI::getCurrentState(void) {
+
+    return currState_;
+
+}
+
+int QEI::getPulses(void) {
+
+    return pulses_;
+
+}
+
+int QEI::getRevolutions(void) {
+
+    return revolutions_;
+
+}
+
+// +-------------+
+// | X2 Encoding |
+// +-------------+
+//
+// When observing states two patterns will appear:
+//
+// Counter clockwise rotation:
+//
+// 10 -> 01 -> 10 -> 01 -> ...
+//
+// Clockwise rotation:
+//
+// 11 -> 00 -> 11 -> 00 -> ...
+//
+// We consider counter clockwise rotation to be "forward" and
+// counter clockwise to be "backward". Therefore pulse count will increase
+// during counter clockwise rotation and decrease during clockwise rotation.
+//
+// +-------------+
+// | X4 Encoding |
+// +-------------+
+//
+// There are four possible states for a quadrature encoder which correspond to
+// 2-bit gray code.
+//
+// A state change is only valid if of only one bit has changed.
+// A state change is invalid if both bits have changed.
+//
+// Clockwise Rotation ->
+//
+//    00 01 11 10 00
+//
+// <- Counter Clockwise Rotation
+//
+// If we observe any valid state changes going from left to right, we have
+// moved one pulse clockwise [we will consider this "backward" or "negative"].
+//
+// If we observe any valid state changes going from right to left we have
+// moved one pulse counter clockwise [we will consider this "forward" or
+// "positive"].
+//
+// We might enter an invalid state for a number of reasons which are hard to
+// predict - if this is the case, it is generally safe to ignore it, update
+// the state and carry on, with the error correcting itself shortly after.
+void QEI::encode(void) {
+
+    int change = 0;
+    int chanA  = channelA_.read();
+    int chanB  = channelB_.read();
+
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+
+    if (encoding_ == X2_ENCODING) {
+
+        //11->00->11->00 is counter clockwise rotation or "forward".
+        if ((prevState_ == 0x3 && currState_ == 0x0) ||
+                (prevState_ == 0x0 && currState_ == 0x3)) {
+
+            pulses_++;
+
+        }
+        //10->01->10->01 is clockwise rotation or "backward".
+        else if ((prevState_ == 0x2 && currState_ == 0x1) ||
+                 (prevState_ == 0x1 && currState_ == 0x2)) {
+
+            pulses_--;
+
+        }
+
+    } else if (encoding_ == X4_ENCODING) {
+
+        //Entered a new valid state.
+        if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
+            //2 bit state. Right hand bit of prev XOR left hand bit of current
+            //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
+            change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
+
+            if (change == 0) {
+                change = -1;
+            }
+
+            pulses_ -= change;
+        }
+
+    }
+
+    prevState_ = currState_;
+
+}
+
+void QEI::index(void) {
+
+    revolutions_++;
+
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI/QEI.h	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,244 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Quadrature Encoder Interface.
+ *
+ * A quadrature encoder consists of two code tracks on a disc which are 90
+ * degrees out of phase. It can be used to determine how far a wheel has
+ * rotated, relative to a known starting position.
+ *
+ * Only one code track changes at a time leading to a more robust system than
+ * a single track, because any jitter around any edge won't cause a state
+ * change as the other track will remain constant.
+ *
+ * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
+ * and paper code tracks consisting of alternating black and white sections;
+ * alternatively, complete disk and PCB emitter/receiver encoder systems can
+ * be bought, but the interface, regardless of implementation is the same.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |  ^  |     |     |     |     |
+ *            ---+  ^  +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  +-----+     +-----+     +-----+
+ * Channel B     ^  |     |     |     |     |     |
+ *            ------+     +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  ^
+ *               90deg
+ *
+ * The interface uses X2 encoding by default which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * channel A.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^     ^     ^     ^     ^
+ *               ^     ^     ^     ^     ^
+ * Pulse count 0 1     2     3     4     5  ...
+ *
+ * This interface can also use X4 encoding which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * either channel.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ *
+ * It defaults
+ *
+ * An optional index channel can be used which determines when a full
+ * revolution has occured.
+ *
+ * If a 4 pules per revolution encoder was used, with X4 encoding,
+ * the following would be observed.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  +--+  ^  ^  +--+  ^
+ *               ^  ^  ^  |  |  ^  ^  |  |  ^
+ * Index      ------------+  +--------+  +-----------
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ * Rev.  count 0          1           2
+ *
+ * Rotational position in degrees can be calculated by:
+ *
+ * (pulse count / X * N) * 360
+ *
+ * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
+ * of pulses per revolution.
+ *
+ * Linear position can be calculated by:
+ *
+ * (pulse count / X * N) * (1 / PPI)
+ *
+ * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
+ * pulses per revolution, and PPI is pulses per inch, or the equivalent for
+ * any other unit of displacement. PPI can be calculated by taking the
+ * circumference of the wheel or encoder disk and dividing it by the number
+ * of pulses per revolution.
+ */
+
+#ifndef QEI_H
+#define QEI_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define PREV_MASK 0x1 //Mask for the previous state in determining direction
+//of rotation.
+#define CURR_MASK 0x2 //Mask for the current state in determining direction
+//of rotation.
+#define INVALID   0x3 //XORing two states where both bits have changed.
+
+/**
+ * Quadrature Encoder Interface.
+ */
+class QEI {
+
+public:
+
+    typedef enum Encoding {
+
+        X2_ENCODING,
+        X4_ENCODING
+
+    } Encoding;
+
+    /**
+     * Constructor.
+     *
+     * Reads the current values on channel A and channel B to determine the
+     * initial state.
+     *
+     * Attaches the encode function to the rise/fall interrupt edges of
+     * channels A and B to perform X4 encoding.
+     *
+     * Attaches the index function to the rise interrupt edge of channel index
+     * (if it is used) to count revolutions.
+     *
+     * @param channelA mbed pin for channel A input.
+     * @param channelB mbed pin for channel B input.
+     * @param index    mbed pin for optional index channel input,
+     *                 (pass NC if not needed).
+     * @param pulsesPerRev Number of pulses in one revolution.
+     * @param encoding The encoding to use. Uses X2 encoding by default. X2
+     *                 encoding uses interrupts on the rising and falling edges
+     *                 of only channel A where as X4 uses them on both
+     *                 channels.
+     */
+    QEI(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+
+    /**
+     * Reset the encoder.
+     *
+     * Sets the pulses and revolutions count to zero.
+     */
+    void reset(void);
+
+    /**
+     * Read the state of the encoder.
+     *
+     * @return The current state of the encoder as a 2-bit number, where:
+     *         bit 1 = The reading from channel B
+     *         bit 2 = The reading from channel A
+     */
+    int getCurrentState(void);
+
+    /**
+     * Read the number of pulses recorded by the encoder.
+     *
+     * @return Number of pulses which have occured.
+     */
+    int getPulses(void);
+
+    /**
+     * Read the number of revolutions recorded by the encoder on the index channel.
+     *
+     * @return Number of revolutions which have occured on the index channel.
+     */
+    int getRevolutions(void);
+
+private:
+
+    /**
+     * Update the pulse count.
+     *
+     * Called on every rising/falling edge of channels A/B.
+     *
+     * Reads the state of the channels and determines whether a pulse forward
+     * or backward has occured, updating the count appropriately.
+     */
+    void encode(void);
+
+    /**
+     * Called on every rising edge of channel index to update revolution
+     * count by one.
+     */
+    void index(void);
+
+    Encoding encoding_;
+
+    InterruptIn channelA_;
+    InterruptIn channelB_;
+    InterruptIn index_;
+
+    int          pulsesPerRev_;
+    int          prevState_;
+    int          currState_;
+
+    volatile int pulses_;
+    volatile int revolutions_;
+
+};
+
+#endif /* QEI_H */
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SHARPIR.cpp	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,97 @@
+/* mbed SHARPIR distance sensor
+ * Copyright (c) 2010 Tomas Johansen
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+
+#include "mbed.h"
+#include "SHARPIR.h"
+
+
+/* This function is currently only working for the Sharp GP2Y0A02YK0F sensor
+ * which measures from around 20cm to 150cm. To adapt this to other Sharp IR 
+ * sensors, you have to calculate the variables reg and exp.
+ * 
+ * To quickly calculate reg and exp, use microsoft excel to plot the graph 
+ * provided in the datasheet. You should create a scatterplot (except that 
+ * the y-axis is [cm], and x-axis is [V]).
+ *
+ * When you get the values, right click the line in the graph and select 
+ * "Add Trendline". Select "power". Also, in the trendline options, check 
+ * that you want to see the function. It will then be printed in your 
+ * scatterplot.
+ *
+ * This is the function: Reg*x^exp.
+ *
+ *
+ * Example:
+ *
+ * SHARPIR sensor(p20);
+ * sensor.calibrate(57.373, 1.3166, 0.45, 2.5);
+ * while(1){
+ *     serial.printf("cm: %f ", sensor.cm()); 
+ *     wait_ms(50);
+ * }
+ *
+ * You can also use this method to manually plot values you've measured with 
+ * the "volt" function, which in the end should give a result similar to the 
+ * values provided below.
+ *
+ * Feel free to contact me with improvements for the source code, and
+ * especially for values that would work for other sensors.
+ */
+
+
+SHARPIR::SHARPIR(PinName AnalogPort)
+        : _analogin(AnalogPort) {
+    higherrange=2.5;
+    lowerrange=0.45;
+    reg=57.373; //60.495
+    exp=-1.3166; //-1.1904
+}
+
+void calibrate(double reg, float exp, double lowerrange, double higherrange) { //sets new reg and exp value
+}
+
+float SHARPIR::volt() {
+    return(_analogin.read()*3.3); //analogin function returns a percentage which describes how much of 3.3v it's reading, therefor multiply it by 3,3 to get the correct analogin voltage.
+}
+
+float SHARPIR::cm() {
+    float v;
+    v=volt();
+    if (v>higherrange) //sensor is out of higher range
+        return(reg*pow(v, exp));//0
+    else if (v<lowerrange)
+        return(-1.0); //sensor is out of lower range
+    else
+        return(reg*pow(v, exp));
+}
+
+float SHARPIR::inch() {
+    float v;
+    v=volt();
+    if (v>higherrange) //sensor is out of higher range
+        return(0);
+    else if (v<lowerrange)
+        return(-1.0); //sensor is out of range
+    else
+        return(0.393700787*reg*pow(v, exp));
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SHARPIR.h	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,27 @@
+/* mbed SHARPIR distance sensor
+ * Copyright (c) 2010 Tomas Johansen
+ * Released under the MIT License: http://mbed.org/license/mit
+ */
+  
+#ifndef MBED_SHARPIR_H
+#define MBED_SHARPIR_H
+
+#include "mbed.h"
+
+class SHARPIR  {
+public:
+    SHARPIR(PinName AnalogPort);
+    float cm();
+    float inch();
+    float volt();
+    void calibrate(double reg, float exp, double lowerrange, double higherrange);
+
+private:
+    AnalogIn _analogin;
+    double lowerrange;
+    double higherrange;
+    double reg;
+    float  exp; //trendline excel + datasheet
+};
+
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpaceSensor.cpp	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,741 @@
+
+#include "mbed.h"
+#include "SpaceSensor.h"
+SpaceSensor::SpaceSensor(PinName p_tx, PinName p_rx): _SpaceSensor(p_tx, p_rx) {
+    begin(115200);
+    st = new char[128];
+    vecf= new float[15];
+}
+/**********************************************************************************************************************/    
+/*4.3.1 Commands for Reading Filtered Sensor Data*/
+/**********************************************************************************************************************/    
+void SpaceSensor::ReadfilteredtaredQuaternion(float *Quaternion){
+    _SpaceSensor.puts(":0\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(Quaternion,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadfilteredtaredEulerAngles(float *EulerAngles){
+    _SpaceSensor.puts(":1\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(EulerAngles,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadfilteredtaredRotationMatrix(float *RotationMatrix){
+    _SpaceSensor.puts(":2\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(RotationMatrix,st);
+    st[0]='\n';
+
+}
+void SpaceSensor::ReadfilteredtaredAxisAngle(float *Axis,float *Angle){
+    _SpaceSensor.puts(":3\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){Axis[i]=vecf[i];}
+    Angle[0]=vecf[3];
+    st[0]='\n';
+    
+}
+void SpaceSensor::ReadfilteredtaredTwoVector(float *Forward,float *Down){
+    _SpaceSensor.puts(":4\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){Forward[i]=vecf[i];}
+    for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
+    st[0]='\n';
+}
+void SpaceSensor::Readfilteredgyrorates(float *Quaternion){
+    _SpaceSensor.puts(":5\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(Quaternion,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadfiltereduntaredQuaternion(float *Quaternion){
+    _SpaceSensor.puts(":6\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(Quaternion,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadfiltereduntaredEulerAngles(float *EulerAngles){
+    _SpaceSensor.puts(":7\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(EulerAngles,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadfiltereduntaredRotationMatrix(float *RotationMatrix){
+    _SpaceSensor.puts(":8\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(RotationMatrix,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadfiltereduntaredAxisAngle(float *Axis,float *Angle){
+    _SpaceSensor.puts(":9\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){Axis[i]=vecf[i];}
+    Angle[0]=vecf[3];
+    st[0]='\n';
+}
+void SpaceSensor::ReadfiltereduntaredTwoVector(float *Forward,float *Down){
+    _SpaceSensor.puts(":10\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){Forward[i]=vecf[i];}
+    for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
+    st[0]='\n';    
+}
+void SpaceSensor::ReadfilteredtaredForwardandDownVectors(float *Forward,float *Down){
+    _SpaceSensor.puts(":11\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){Forward[i]=vecf[i];}
+    for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
+    st[0]='\n';    
+}
+void SpaceSensor::ReadfilteredNorthEarthVectors(float *North,float *Earth){
+    _SpaceSensor.puts(":12\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){North[i]=vecf[i];}
+    for(int i=0;i<3;i++){Earth[i]=vecf[i+3];}
+    st[0]='\n';    
+}
+
+/**********************************************************************************************************************/    
+    /*4.3.2 Commands for Interfacing with Electronic Systems*/
+/**********************************************************************************************************************/    
+void SpaceSensor::Setinterrupttype(int mode){
+    sprintf( st ,":29,%d\n",mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::Readinterrupttype(int *interrupttype){
+    _SpaceSensor.puts(":30\n");
+    read_string(vecf,st);
+    interrupttype[0]=(int)vecf[0];
+    interrupttype[1]=(int)vecf[1];
+}
+void SpaceSensor::Readinterruptstatus(int *interruptstatus){
+    _SpaceSensor.puts(":31\n");
+    read_string(vecf,st);
+    interruptstatus[0]=(int)vecf[0];
+}
+/**********************************************************************************************************************/    
+    /*4.3.3 Commands for Reading Normalized Sensor Data*/
+/**********************************************************************************************************************/    
+void SpaceSensor::Readall(float *gyro,float *accelerometer,float *compass){
+    _SpaceSensor.puts(":32\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){gyro[i]=vecf[i];}
+    for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];}
+    for(int i=0;i<3;i++){compass[i]=vecf[i+6];}
+    st[0]='\n';    
+}
+void SpaceSensor::Readgyros(float *gyro){
+    _SpaceSensor.puts(":33\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(gyro,st);
+    st[0]='\n';    
+}
+void SpaceSensor::Readaccelerometer(float *accelerometer){
+    _SpaceSensor.puts(":34\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(accelerometer,st);
+    st[0]='\n';    
+}
+void SpaceSensor::Readcompass(float *compass){
+    _SpaceSensor.puts(":35\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(compass,st);
+    st[0]='\n';    
+}
+void SpaceSensor::ReadtemperatureC(float *temp){
+    _SpaceSensor.puts(":36\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(temp,st);
+    st[0]='\n';    
+}
+void SpaceSensor::ReadtemperatureF(float *temp){
+    _SpaceSensor.puts(":37\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(temp,st);
+    st[0]='\n';    
+}
+void SpaceSensor::Readconfidencefactor(float *confidencefactor){
+    _SpaceSensor.puts(":38\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(confidencefactor,st);
+    st[0]='\n';    
+}
+void SpaceSensor::Readaccelerometerunnormalized(float *accelunnormalized){
+    _SpaceSensor.puts(":39\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(accelunnormalized,st);
+    st[0]='\n';    
+}
+void SpaceSensor::Readcompassunnormalized(float *compassunnormalized){
+    _SpaceSensor.puts(":30\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(compassunnormalized,st);
+    st[0]='\n';    
+}
+/**********************************************************************************************************************/    
+    /*4.3.4 Commands for Reading Raw Sensor Data*/
+/**********************************************************************************************************************/    
+void SpaceSensor::Readallraw(float *gyro,float *accelerometer,float *compass){
+    _SpaceSensor.puts(":64\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){gyro[i]=vecf[i];}
+    for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];}
+    for(int i=0;i<3;i++){compass[i]=vecf[i+6];}
+    st[0]='\n';    
+
+}
+void SpaceSensor::Readgyroraw(float *gyro){
+    _SpaceSensor.puts(":65\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(gyro,st);
+    st[0]='\n';    
+}
+void SpaceSensor::Readaccelerometerraw(float *accelerometer){
+    _SpaceSensor.puts(":66\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(accelerometer,st);
+    st[0]='\n';    
+}
+void SpaceSensor::Readcompassraw(float *compass){
+    _SpaceSensor.puts(":67\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(compass,st);
+    st[0]='\n';    
+}
+/**********************************************************************************************************************/    
+    /*4.3.5 Commands for Setting Filter Parameters*/
+/**********************************************************************************************************************/    
+void SpaceSensor::Tarewithcurrentorientation(){
+    _SpaceSensor.puts(":96\n");
+}
+void SpaceSensor::Tarewithquaternion(float Quaternion[4]){
+    sprintf( st ,":97,%f,%f,%f,%f\n",Quaternion[0],Quaternion[1],Quaternion[2],Quaternion[3]);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::Tarewithrotationmatrix(float RotationMatrix[9]){
+    sprintf( st ,":98,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",RotationMatrix[0],RotationMatrix[1],RotationMatrix[2],RotationMatrix[3],RotationMatrix[4],RotationMatrix[5],RotationMatrix[6],RotationMatrix[7],RotationMatrix[8]);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetStaticRhoModeAccelerometer(float rho){
+    sprintf( st ,":99,%f\n",rho);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetConfidenceRhoModeAccelerometer(float min,float max){
+    sprintf( st ,":100,%f,%f\n",min,max);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetStaticRhoModeCompass(float rho){
+    sprintf( st ,":101,%f\n",rho);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetConfidenceRhoModeCompass(float min, float max){
+    sprintf( st ,":102,%f,%f\n",min,max);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetDesiredUpdateRate(int t){
+    sprintf( st ,":103,%d\n",t);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetMultiReferenceVectorsWithCurrentOrientation(){
+    _SpaceSensor.puts(":l04\n");
+}
+void SpaceSensor::SetReferenceVectorMode(int mode){
+    sprintf( st ,":105,%d\n",mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetOversampleRate(int rate){
+    sprintf( st ,":106,%d\n",rate);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::EnableDisablegyros(int mode){
+    sprintf( st ,":107,%d\n",mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::EnableDisableAccelerometer(int mode){
+    sprintf( st ,":108,%d\n",mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::EnableDisableCompass(int mode){
+    sprintf( st ,":109,%d\n",mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::ResetMultiReferenceVectorsToZero(){
+    _SpaceSensor.puts(":110\n");
+}
+void SpaceSensor::SetMultiReferenceResolution(int mode){
+    sprintf( st ,":111,%d\n",mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetCompassMultiReferenceVector(int Index,float Vector[3]){
+    sprintf( st ,":112,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetCompassMultiReferenceCheckVector(int Index,float Vector[3]){
+    sprintf( st ,":113,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetAccelMultiReferenceVector(int Index,float Vector[3]){
+    sprintf( st ,":114,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetAccelMultiReferenceCheckVector(int Index,float Vector[3]){
+    sprintf( st ,":115,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetAxisDirections(int direction){
+    sprintf( st ,":116,%d\n",direction);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetRunningAveragePercent(float Percent){
+    sprintf( st ,":117,%f\n",Percent);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetCompassReferenceVector(float Vector[3]){
+    sprintf( st ,":118,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetAccelerometerReferenceVector(float Vector[3]){
+    sprintf( st ,":119,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::ResetKalmanFilter(){
+    _SpaceSensor.puts(":120\n");
+}
+void SpaceSensor::SetAccelerometerRange(int Accelrange){
+    sprintf( st ,":121,%d\n",Accelrange);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetMultiReferenceWeightPower(float WeightPower){
+    sprintf( st ,":122,%f\n",WeightPower);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::EnableDisableFilter(int Mode){
+    sprintf( st ,":123,%d\n",Mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetRunningAverageMode(int Mode){
+    sprintf( st ,":124,%d\n",Mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetGyroscopeRange(int mode){
+    sprintf( st ,":125,%d\n",mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetCompassRange(int mode){
+    sprintf( st ,":126,%d\n",mode);
+    _SpaceSensor.puts(st);
+}
+/**********************************************************************************************************************/    
+    /*4.3.6 Commands for Reading Filter Parameters*/
+/**********************************************************************************************************************/    
+void SpaceSensor::ReadTareOrientationQuaternion(float *Quaternion){
+    _SpaceSensor.puts(":128\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(Quaternion,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadTareOrientationRotationMatrix(float *RotationMatrix){
+    _SpaceSensor.puts(":129\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(RotationMatrix,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadRhoDataAccelerometer(int *Rhomode,float *minroh,float *maxroh){
+    _SpaceSensor.puts(":130\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Rhomode[0]=(int)vecf[0];
+    minroh[0]=vecf[1];
+    maxroh[0]=vecf[2];
+    st[0]='\n';
+}
+void SpaceSensor::ReadRhoDataCompass(int *Rhomode,float *minroh,float *maxroh){
+    _SpaceSensor.puts(":131\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Rhomode[0]=(int)vecf[0];
+    minroh[0]=vecf[1];
+    maxroh[0]=vecf[2];
+    st[0]='\n';
+}
+void SpaceSensor::ReadCurrentUpdateRate(int *rate){
+    _SpaceSensor.puts(":132\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    rate[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadCompassReferenceVector(float *Vector){
+    _SpaceSensor.puts(":133\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(Vector,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadAccelerometerReferenceVector(float *Vector){
+    _SpaceSensor.puts(":134\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(Vector,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadReferenceVectorMode(int *Mode){
+    _SpaceSensor.puts(":135\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadCompassMultiReferenceVector(int Index,float *Vector){
+    sprintf( st ,":136,%d\n",Index);
+    _SpaceSensor.puts(st);
+    _SpaceSensor.scanf("%s", st);
+    read_string(Vector,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadCompassMultiReferenceCheckVector(int Index,float *Vector){
+    sprintf( st ,":137,%d\n",Index);
+    _SpaceSensor.puts(st);
+    _SpaceSensor.scanf("%s", st);
+    read_string(Vector,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadAccelMultiReferenceVector(int Index,float *Vector){
+    sprintf( st ,":138,%d\n",Index);
+    _SpaceSensor.puts(st);
+    _SpaceSensor.scanf("%s", st);
+    read_string(Vector,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadAccelMultiReferenceCheckVector(int Index,float *Vector){
+    sprintf( st ,":139,%d\n",Index);
+    _SpaceSensor.puts(st);
+    _SpaceSensor.scanf("%s", st);
+    read_string(Vector,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadGyroEnabledState(int *Mode){
+    _SpaceSensor.puts(":140\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadAccelerometerEnabledState(int *Mode){
+    _SpaceSensor.puts(":141\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadCompassEnabledState(int *Mode){
+    _SpaceSensor.puts(":142\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadAxisDirections(int *direction){
+    _SpaceSensor.puts(":143\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    direction[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadOversampleRate(int *Rate){
+    _SpaceSensor.puts(":144\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Rate[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadRunningAveragePercent(float *Percent){
+    _SpaceSensor.puts(":145\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(Percent,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadDesiredUpdateRate(float *rate){
+    _SpaceSensor.puts(":146\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(rate,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadKalmanFilterCovarianceMatrix(float *CovarianceMatrix){
+    _SpaceSensor.puts(":147\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(CovarianceMatrix,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadAccelerometerRange(int *Accelrange){
+    _SpaceSensor.puts(":148\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Accelrange[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadMultiReferenceWeightPower(float *WeightPower){
+    _SpaceSensor.puts(":149\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(WeightPower,st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadMultiReferenceResolution(int *Resolution){
+    _SpaceSensor.puts(":150\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Resolution[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadNumberOfmultiReferenceCells(int *NumberOfCells){
+    _SpaceSensor.puts(":151\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    NumberOfCells[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadFilterEnableState(int *Mode){
+    _SpaceSensor.puts(":152\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadRunningAverageMode(int *Mode){
+    _SpaceSensor.puts(":153\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadGyroscopeRange(int *mode){
+    _SpaceSensor.puts(":154\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    mode[0]=(int)vecf[0];
+    st[0]='\n';
+}
+void SpaceSensor::ReadCompassRange(int *mode){
+    _SpaceSensor.puts(":155\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    mode[0]=(int)vecf[0];
+    st[0]='\n';
+}
+/**********************************************************************************************************************/    
+    /*4.3.7 Commands for Calibration*/
+/**********************************************************************************************************************/    
+void SpaceSensor::SetCompassCalibrationParameters(float Bias[3],float Matrix[9]){
+    sprintf( st ,":160,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],Matrix[0],Matrix[1],Matrix[2],Matrix[3],Matrix[4],Matrix[5],Matrix[6],Matrix[7],Matrix[8]);
+    _SpaceSensor.puts(st);
+    st[0]='\n';
+}
+void SpaceSensor::SetAccelerometerCalibrationParameters(float Bias[3],float Matrix[9]){
+    sprintf( st ,":161,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],Matrix[0],Matrix[1],Matrix[2],Matrix[3],Matrix[4],Matrix[5],Matrix[6],Matrix[7],Matrix[8]);
+    _SpaceSensor.puts(st);
+    st[0]='\n';
+}
+void SpaceSensor::ReadCompassCalibrationParameters(float *Bias,float *Matrix){
+    _SpaceSensor.puts(":162\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){Bias[i]=vecf[i];}
+    for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];}
+    st[0]='\n';
+}
+void SpaceSensor::ReadAccelerometerCalibrationParameters(float *Bias,float *Matrix){
+    _SpaceSensor.puts(":163\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){Bias[i]=vecf[i];}
+    for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];}
+    st[0]='\n';
+}
+void SpaceSensor::ReadGyroCalibrationParameters(float *Bias, float *range){
+    _SpaceSensor.puts(":164\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    for(int i=0;i<3;i++){Bias[i]=vecf[i];}
+    for(int i=0;i<3;i++){range[i]=vecf[i+3];}
+    st[0]='\n';
+}
+void SpaceSensor::BeginGyroAutocalibration(){
+    _SpaceSensor.puts(":165\n");
+}
+void SpaceSensor::SetGyroCalibrationParameters(float Bias[3], float range[3]){
+    sprintf( st ,":166,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],range[0],range[1],range[2]);
+    _SpaceSensor.puts(st);
+    st[0]='\n';
+}
+/**********************************************************************************************************************/    
+    /*4.3.8 General Commands*/
+/**********************************************************************************************************************/    
+void SpaceSensor::ReadSoftwareVersion(char *version){
+    _SpaceSensor.puts(":223\n");
+    _SpaceSensor.scanf("%s", version);
+}
+void SpaceSensor::RestoreFactorySettings(){
+    _SpaceSensor.puts(":224\n");
+}
+void SpaceSensor::CommitSettings(){
+    _SpaceSensor.puts(":225\n");
+}
+void SpaceSensor::SoftwareReset(){
+    _SpaceSensor.puts(":226\n");
+}
+void SpaceSensor::EnableWatchdogTimer(int timer){
+    sprintf( st ,":227,%d\n",timer);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::DisableWatchdogTimer(){
+    _SpaceSensor.puts(":228\n");
+}
+void SpaceSensor::EnterFirmwareUpdateMode(){
+    _SpaceSensor.puts(":229\n");
+}
+void SpaceSensor::ReadHardwareVersion(char *version){
+    _SpaceSensor.puts(":230\n");
+    _SpaceSensor.scanf("%s", version);
+}
+void SpaceSensor::SetUARTBaudRate(int Baudrate){
+    sprintf( st ,":231,%d\n",Baudrate);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::GetUARTBaudRate(int *Baudrate){
+    _SpaceSensor.puts(":232\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Baudrate[0]=(int)vecf[0];
+}
+void SpaceSensor::SetUSBmode(int mode){
+    sprintf( st ,":233,%d\n",mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::GetUSBmode(int *mode){
+    _SpaceSensor.puts(":234\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    mode[0]=(int)vecf[0];
+}
+void SpaceSensor::SetClockSpeed(int clock){
+    sprintf( st ,":235,%d\n",clock);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::GetClockSpeed(int *clock){
+    _SpaceSensor.puts(":236\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    clock[0]=(int)vecf[0];
+}
+void SpaceSensor::GetSerialnumber(int *serialnum){
+    _SpaceSensor.puts(":237\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    serialnum[0]=(int)vecf[0];
+}
+void SpaceSensor::SetLEDColor(float color[3]){
+    sprintf( st ,":238,%f,%f,%f\n",color[0],color[1],color[2]);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::GetLEDColor(float *color){
+    _SpaceSensor.puts(":239\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(color,st);
+}
+void SpaceSensor::EnableDisableJoystick(int Mode){
+    sprintf( st ,":240,%d\n",Mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::EnableDisablemouse(int Mode){
+    sprintf( st ,":241,%d\n",Mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::ReadJoystickEnabledState(int *Mode){
+    _SpaceSensor.puts(":242\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+}
+void SpaceSensor::ReadMouseEnabledState(int *Mode){
+    _SpaceSensor.puts(":243\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+}
+void SpaceSensor::SetControlMode(float Controlclass,float controlindex,float handlerindex){
+    sprintf( st ,":244,%f,%f,%f\n",Controlclass,controlindex,handlerindex);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::SetControlData(float Controlclass,float controlindex,float datapointindex,float datapoint ){
+    sprintf( st ,":245,%f,%f,%f,%f\n",Controlclass,controlindex,datapointindex,datapoint);
+    _SpaceSensor.puts(st);
+}  
+void SpaceSensor::ReadControlMode(float *Handlerindex, float Controlclass,float controlindex){
+    sprintf( st ,":246,%f,%f\n",Controlclass,controlindex);
+    _SpaceSensor.puts(st);
+    _SpaceSensor.scanf("%s", st);
+    read_string(Handlerindex,st);
+}
+void SpaceSensor::ReadControlData(float *Datapoint,float ControlClass,float controlIndex,float dataPointIndex){
+    sprintf( st ,":247,%f,%f,%f\n",ControlClass,controlIndex,dataPointIndex);
+    _SpaceSensor.puts(st);
+    _SpaceSensor.scanf("%s", st);
+    read_string(Datapoint,st);
+}
+void SpaceSensor::SetMouseAbsoluteRelative(int Mode){
+    sprintf( st ,":251,%d\n",Mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::ReadMouseAbsoluteRelative(int *Mode){
+    _SpaceSensor.puts(":252\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+}
+void SpaceSensor::SetjoystickAndMousePresentRemoved(int Mode){
+    sprintf( st ,":253,%d\n",Mode);
+    _SpaceSensor.puts(st);
+}
+void SpaceSensor::ReadjoystickAndMousePresentRemoved(int *Mode){
+    _SpaceSensor.puts(":254\n");
+    _SpaceSensor.scanf("%s", st);
+    read_string(vecf,st);
+    Mode[0]=(int)vecf[0];
+}
+
+/**********************************************************************************************************************/    
+/*Private functions*/    
+/**********************************************************************************************************************/    
+void SpaceSensor::read_string(float *vector,char *c){
+                    char var[10];                    
+                    int var_size=0,j=0,i=0;
+                    while(1){
+                             
+                             if(j==strlen(c)){
+                                             vector[i]=atof(var);
+                                             break;
+                                            
+                                            }
+                             if(c[j]==','){
+                                            vector[i]=atof(var);
+                                            i++;
+                                            j++;
+                                            var_size=0;
+                                            }
+                             var[var_size]=c[j];
+                             j++;
+                             var_size++;
+    
+                             }                               
+                    }
+    
+void SpaceSensor::begin(long baud) {
+    _SpaceSensor.baud(baud);
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpaceSensor.h	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,200 @@
+#ifndef MBED_SpaceSsensor_H
+#define MBED_SpaceSsensor_H
+//#endif
+#include "mbed.h"
+#include "iostream"
+#include "math.h"
+using namespace std;
+
+class SpaceSensor {
+public:
+    SpaceSensor(PinName p_tx, PinName p_rx);
+/**********************************************************************************************************************/    
+    /*4.3.1 Commands for Reading Filtered Sensor Data
+    These commands return sensor data which has been filtered using a Kalman filter. None of these commands take any
+    parameters, they only return data.
+    */
+/**********************************************************************************************************************/
+    void ReadfilteredtaredQuaternion(float *Quaternion);
+    void ReadfilteredtaredEulerAngles(float *EulerAngles);
+    void ReadfilteredtaredRotationMatrix(float *RotationMatrix);
+    void ReadfilteredtaredAxisAngle(float *Axis,float *Angle);
+    void ReadfilteredtaredTwoVector(float *Forward,float *Down);
+    
+    void Readfilteredgyrorates(float *Quaternion);
+    
+    void ReadfiltereduntaredQuaternion(float *Quaternion);
+    void ReadfiltereduntaredEulerAngles(float *EulerAngles);
+    void ReadfiltereduntaredRotationMatrix(float *RotationMatrix);
+    void ReadfiltereduntaredAxisAngle(float *Axis,float *Angle);
+    void ReadfiltereduntaredTwoVector(float *Forward,float *Down);
+    
+    void ReadfilteredtaredForwardandDownVectors(float *Forward,float *Down);
+    void ReadfilteredNorthEarthVectors(float *North,float *Earth);
+/**********************************************************************************************************************/    
+    /*4.3.2 Commands for Interfacing with Electronic Systems*/
+/**********************************************************************************************************************/    
+    void Setinterrupttype(int mode);
+    void Readinterrupttype(int *interrupttype);
+    void Readinterruptstatus(int *interruptstatus);
+/**********************************************************************************************************************/    
+    /*4.3.3 Commands for Reading Normalized Sensor Data
+    These commands return sensor data which has been converted from a raw form. In the case of the normalized
+    accelerometer and compass, the data returned are unit vectors, and as such, have no magnitude data associated with
+    them. For data that represents real world quantities, both the unnormalized accelerometer command and unnormalized
+    compass command return data in units of g and gauss respectively. Normalized gyro data is returned in radians/sec.
+    */
+/**********************************************************************************************************************/    
+    void Readall(float *gyro,float *accelerometer,float *compass);
+    void Readgyros(float *gyro);
+    void Readaccelerometer(float *accelerometer);
+    void Readcompass(float *compass);
+    void ReadtemperatureC(float *temp);
+    void ReadtemperatureF(float *temp);
+    void Readconfidencefactor(float *confidencefactor);
+    void Readaccelerometerunnormalized(float *accelunnormalized);
+    void Readcompassunnormalized(float *compassunnormalized);
+/**********************************************************************************************************************/    
+    /*4.3.4 Commands for Reading Raw Sensor Data
+    These commands return sensor data just as it was when it was read from each sensor. None of these commands take any
+    parameters, they only return data.
+    */
+/**********************************************************************************************************************/    
+    void Readallraw(float *gyro,float *accelerometer,float *compass);
+    void Readgyroraw(float *gyro);
+    void Readaccelerometerraw(float *accelerometer);
+    void Readcompassraw(float *compass);
+/**********************************************************************************************************************/    
+    /*4.3.5 Commands for Setting Filter Parameters
+    These commands allow the configuration of parameters associated with the Kalman filter. Most of these commands
+    take parameters, none return any data.
+    */
+/**********************************************************************************************************************/    
+    void Tarewithcurrentorientation();
+    void Tarewithquaternion(float Quaternion[4]);
+    void Tarewithrotationmatrix(float RotationMatrix[9]);
+    void SetStaticRhoModeAccelerometer(float rho);
+    void SetConfidenceRhoModeAccelerometer(float min,float max);
+    void SetStaticRhoModeCompass(float rho);
+    void SetConfidenceRhoModeCompass(float min, float max);
+    void SetDesiredUpdateRate(int t);
+    void SetMultiReferenceVectorsWithCurrentOrientation();
+    void SetReferenceVectorMode(int mode);
+    void SetOversampleRate(int rate);
+    void EnableDisablegyros(int mode);
+    void EnableDisableAccelerometer(int mode);
+    void EnableDisableCompass(int mode);
+    void ResetMultiReferenceVectorsToZero();
+    void SetMultiReferenceResolution(int mode);
+    void SetCompassMultiReferenceVector(int Index,float Vector[3]);
+    void SetCompassMultiReferenceCheckVector(int Index,float Vector[3]);
+    void SetAccelMultiReferenceVector(int Index,float Vector[3]);
+    void SetAccelMultiReferenceCheckVector(int Index,float Vector[3]);
+    void SetAxisDirections(int direction);
+    void SetRunningAveragePercent(float Percent);
+    void SetCompassReferenceVector(float Vector[3]);
+    void SetAccelerometerReferenceVector(float Vector[3]);
+    void ResetKalmanFilter();
+    void SetAccelerometerRange(int Accelrange);
+    void SetMultiReferenceWeightPower(float WeightPower);
+    void EnableDisableFilter(int Mode);
+    void SetRunningAverageMode(int Mode);
+    void SetGyroscopeRange(int mode);
+    void SetCompassRange(int mode);
+/**********************************************************************************************************************/    
+    /*4.3.6 Commands for Reading Filter Parameters
+    These commands allow the reading of parameters associated with the Kalman filter. All these commands return data,
+    and accept no parameters.
+    */
+/**********************************************************************************************************************/    
+    void ReadTareOrientationQuaternion(float *Quaternion);
+    void ReadTareOrientationRotationMatrix(float *RotationMatrix);
+    void ReadRhoDataAccelerometer(int *Rhomode,float *minroh,float *maxroh);
+    void ReadRhoDataCompass(int *Rhomode,float *minroh,float *maxroh);
+    void ReadCurrentUpdateRate(int *rate);
+    void ReadCompassReferenceVector(float *Vector);
+    void ReadAccelerometerReferenceVector(float *Vector);
+    void ReadReferenceVectorMode(int *Mode);
+    void ReadCompassMultiReferenceVector(int Index,float *Vector);
+    void ReadCompassMultiReferenceCheckVector(int Index,float *Vector);
+    void ReadAccelMultiReferenceVector(int Index,float *Vector);
+    void ReadAccelMultiReferenceCheckVector(int Index,float *Vector);
+    void ReadGyroEnabledState(int *Mode);
+    void ReadAccelerometerEnabledState(int *Mode);
+    void ReadCompassEnabledState(int *Mode);
+    void ReadAxisDirections(int *direction);
+    void ReadOversampleRate(int *Rate);
+    void ReadRunningAveragePercent(float *Percent);
+    void ReadDesiredUpdateRate(float *rate);
+    void ReadKalmanFilterCovarianceMatrix(float *CovarianceMatrix);
+    void ReadAccelerometerRange(int *Accelrange);
+    void ReadMultiReferenceWeightPower(float *WeightPower);
+    void ReadMultiReferenceResolution(int *Resolution);
+    void ReadNumberOfmultiReferenceCells(int *NumberOfCells);
+    void ReadFilterEnableState(int *Mode);
+    void ReadRunningAverageMode(int *Mode);
+    void ReadGyroscopeRange(int *mode);
+    void ReadCompassRange(int *mode);
+/**********************************************************************************************************************/    
+    /*4.3.7 Commands for Calibration
+    These commands allow the configuration and reading of calibration parameters and enabling of calibration modes.
+    */
+/**********************************************************************************************************************/    
+    void SetCompassCalibrationParameters(float Bias[3],float Matrix[9]);
+    void SetAccelerometerCalibrationParameters(float Bias[3],float Matrix[9]);
+    void ReadCompassCalibrationParameters(float *Bias,float *Matrix);
+    void ReadAccelerometerCalibrationParameters(float *Bias,float *Matrix);
+    void ReadGyroCalibrationParameters(float *Bias, float *range);
+    void BeginGyroAutocalibration();
+    void SetGyroCalibrationParameters(float Bias[3], float range[3]);
+/**********************************************************************************************************************/    
+    /*4.3.8 General Commands
+    These commands are for the configuration of the sensor as a whole as opposed to configuration of the filter or sensors.
+    */
+/**********************************************************************************************************************/    
+    void ReadSoftwareVersion(char *version);
+    void RestoreFactorySettings();
+    void CommitSettings();
+    void SoftwareReset();
+    void EnableWatchdogTimer(int timer);
+    void DisableWatchdogTimer();
+    void EnterFirmwareUpdateMode();
+    void ReadHardwareVersion(char *version);
+    void SetUARTBaudRate(int Baudrate);
+    void GetUARTBaudRate(int *Baudrate);
+    void SetUSBmode(int mode);
+    void GetUSBmode(int *mode);
+    void SetClockSpeed(int clock);
+    void GetClockSpeed(int *clock);
+    void GetSerialnumber(int *serialnum);
+    void SetLEDColor(float color[3]);
+    void GetLEDColor(float *color);
+    void EnableDisableJoystick(int Mode); 
+    void EnableDisablemouse(int Mode);
+    void ReadJoystickEnabledState(int *Mode);
+    void ReadMouseEnabledState(int *Mode);
+    void SetControlMode(float Controlclass,float controlindex,float handlerindex);
+    void SetControlData(float Controlclass,float controlindex,float datapointindex,float datapoint );  
+    void ReadControlMode(float *Handlerindex, float Controlclass,float controlindex);
+    void ReadControlData(float *Datapoint,float ControlClass,float controlIndex,float dataPointIndex);
+    void SetMouseAbsoluteRelative(int Mode);
+    void ReadMouseAbsoluteRelative(int *Mode);
+    void SetjoystickAndMousePresentRemoved(int Mode);
+    void ReadjoystickAndMousePresentRemoved(int *Mode);
+/**********************************************************************************************************************/    
+/*
+ clases needed to interface with the sensor
+ */
+/**********************************************************************************************************************/    
+    
+    
+    private:
+    Serial _SpaceSensor;
+    char *st;
+    float *vecf;
+    char var[10];
+    void begin(long baud);
+    void read_string(float *vector,char *c);
+    };
+    
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,520 @@
+/*/least updates:
+this program is based on exploration 7 which is PI controller on theta only.
+##done steps:
+-------------
+1)avoiding obstacle.
+2)switching modes must be in the navigation function.
+3)case if Obs_opp=0.
+4)line 413 if Robs=0 or Lobs=0 must be =1.5
+
+##next steps to be done:
+-----------------------
+1)position of Xtow.
+2)dot product check line 431&459.
+3)exiting conditions check.
+*///
+/**
+ * this algorithm is Exploration algo.
+ */
+#include "Motor.h"
+#include "QEI.h"
+#include"math.h"
+#include "SpaceSensor.h"
+#include "PID.h"
+#include "SHARPIR.h"
+
+//#define PULSES_PER_REVOLUTION 624
+#define RATE_PULSES 0.01        //PID loop timing
+
+//PID tuning constants.
+#define L_KC 1.5048 //Forward left motor Kc
+#define L_TI 0.1    //Forward left motor Ti
+#define L_TD 0.0    //Forward left motor Td
+#define R_KC 1.2716 //Forward right motor Kc
+#define R_TI 0.1    //Forward right motor Ti
+#define R_TD 0.0    //Forward right motor Td
+
+//PID input/output limits.
+#define L_INPUT_MIN 0     //Forward left motor minimum input
+#define L_INPUT_MAX 3000  //Forward left motor maximum input
+#define L_OUTPUT_MIN 0.0  //Forward left motor minimum output
+#define L_OUTPUT_MAX 0.9  //Forward left motor maximum output
+
+#define R_INPUT_MIN 0     //Forward right motor input minimum
+#define R_INPUT_MAX 3000  //Forward right motor input maximum
+#define R_OUTPUT_MIN 0.0  //Forward right motor output minimum
+#define R_OUTPUT_MAX 0.9  //Forward right motor output maximum
+
+#define PPDFactor 0.83638870665653517347249697967419 //pulse per Distance Factor
+#define LPPD 84.5374/PPDFactor //Left Pulse Per Distance
+#define RPPD 84.0804/PPDFactor //Left Pulse Per Distance
+
+#define delta 55
+
+#define navigationtimeinterrupt 1
+//functions identification
+void read_string();
+void motor_intialize();
+void initializePid(void);
+void PID_QEI_calculations();
+void PWM_cal();
+void Distance_calculation();
+void Get_Window(int j,float *vec,float *vec2);
+void Energy_cal(float *Energy,float *W,float Threshold1[3]);
+void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3]);
+void move(float Lperiod,float Rperiod);
+void navigation_model();
+void RotateLeft(float Lperiod,float Rperiod);
+void RotateRight(float Lperiod,float Rperiod);
+void Stop();
+void rotation();
+void errorfun();
+void GTG_mode();
+void FollowWall();
+void First_FW();
+void Wall_Following();
+void min_Obs_opp();
+
+DigitalOut myled(LED1);
+Serial xbee(p9, p10); //Creates a variable for serial comunication through pin 28 and 27
+SHARPIR IR_front(p20); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
+SHARPIR IR_right(p19); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
+SHARPIR IR_left(p18); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
+
+Motor leftFrontMotor(p21, p16, p15);  //pwm, inB, inA
+Motor rightFrontMotor(p22, p12, p11); //pwm, inA, inB
+Motor leftBackMotor(p23, p8, p7);  //pwm, inB, inA
+Motor rightBackMotor(p24, p6, p5); //pwm, inA, inB
+
+QEI leftBackQei(p25, p26, NC, 151);  //chanA, chanB, index, ppr
+QEI rightBackQei(p29, p30, NC, 147); //chanB, chanA, index, ppr
+//Tuning parameters calculated from step tests;
+//see http://mbed.org/cookbook/PID for examples.
+PID leftPid(L_KC, L_TI, L_TD, RATE_PULSES);  //Kc, Ti=0.01, Td=0.005
+PID rightPid(R_KC, R_TI, R_TD, RATE_PULSES ); //Kc, Ti, Td
+SpaceSensor SS(p13, p14);
+//Left motor working variables.
+int leftPulses     = 0;
+int leftPrevPulses = 0;
+float leftVelocity = 0.0;
+
+//Right motor working variables.
+int rightPulses     = 0;
+int rightPrevPulses = 0;
+float rightVelocity = 0.0;
+
+//angle parameters.
+float sfgyro=2;
+float FIRtheta[3],FIR_W[3],FIR_E[3],sigmoid,error,errorI=0,LTP,RTP;
+float FIR_Thresh[3]= {0.00145,0.000674,0.006452};
+float EulerAngles[3];
+float compass[3];
+
+bool theta_update;
+float LPWM,RPWM;
+float dis=0,rightDis,leftDis;
+
+// PID set points parameters.
+int LQEI_SetPoint=3000,RQEI_SetPoint=3000;
+
+//read string parameters
+float X,Y,R,Theta_base_st,Fobstacles_dis,Lobstacles_dis,Robstacles_dis;
+char Ar_st[30];
+char X_coord[10];
+char Y_coord[10];
+char st[128];
+
+//navigation model parameters
+float prev_dis[3];
+float post_dis[3];
+float current_theta,rot_theta;
+
+//Obasticle avoidance Parameters
+float current_Obs_dis,Obs_h,gradient_Obs;
+int Obs_factor;
+bool FWM,GTGM,ccw,cw;
+float Obs_adj,Obs_opp,Obs_hyp,Obs_dis,Obs_alpha_opp,Obs_alpha,FWtheta,Xtow;
+float GTGtheta,dot_product_cond;
+float h_Obsinv,error_inv;
+//time parameters
+Timer t;
+Ticker timer,timer2;
+
+void get_theta()
+{
+    angle_cal(FIRtheta,FIR_W,FIR_E,FIR_Thresh);
+    SS.ReadfilteredtaredEulerAngles(EulerAngles);
+    SS.Readcompass(compass);
+    Fobstacles_dis=IR_front.cm();//low range will return -1.0& for far range will return 0.
+    Lobstacles_dis=IR_left.cm();
+    Robstacles_dis=IR_right.cm();
+}
+
+int main()
+{
+    motor_intialize();
+    initializePid();
+    //Velocity to mantain in pulses per second.
+    leftPid.setSetPoint(LQEI_SetPoint);//set this number by experementally//
+    rightPid.setSetPoint(RQEI_SetPoint);//set this number by experementally//
+    FWM=false;
+    GTGM=true;
+    ccw=false;
+    cw=false;
+    while(1) { //big fat loop
+        char datain;
+        xbee.puts("press B to begin...");
+        datain=xbee.getc();
+        if(datain=='b'||datain=='B') {
+            t.reset();
+            t.start();
+            xbee.puts("enter coordinates in the form of BX'x'Y'y'E...\r\n");
+            read_string();
+            rot_theta=Theta_base_st;
+            navigation_model();
+            timer.attach(&get_theta, 0.2);
+            timer2.attach(&navigation_model, navigationtimeinterrupt);
+
+            if(theta_update)
+                rotation();
+            rot_theta=0;
+            FIRtheta[0]=FIRtheta[0]-Theta_base_st;
+            errorI=0;
+            while(1) {//////////////////////////////////////////////////////////////////////////////////////////////////////////////in diffrent modes
+                GTG_mode();
+                if(abs(R-(dis+prev_dis[0]))<3)break;
+            }//whileloop
+            timer.detach();
+            timer2.detach();
+            Stop();
+            sprintf( st ,"final:distance%f lp:%d rp:%d %f\r\n",abs((dis+prev_dis[0])),leftPulses,rightPulses,t.read());
+            xbee.puts(st);
+
+        }//if condition loop enter b
+    }//big fat loop
+}//main loop
+
+void read_string()
+{
+    int i=0;
+    char c;
+    c=xbee.getc();
+    if(c=='B') {
+        while(1) {
+            c=xbee.getc();
+            if(c=='E')break;
+            Ar_st[i]=c;
+            i++;
+            myled != myled;
+
+        }
+
+        int count=0,j=0;
+        if(Ar_st[j]=='X') {
+            while(1) {
+                j++;
+                if(Ar_st[j]=='Y')break;
+                X_coord[count]=Ar_st[j];
+                count++;
+            }
+            X=atof(X_coord);
+            count=0;
+        }
+        if(Ar_st[j]=='Y') {
+            while(1) {
+                j++;
+                Y_coord[count]=Ar_st[j];
+                count++;
+                if(j==i)break;
+            }
+            Y=atof(Y_coord);
+            count=0;
+        }
+    }
+    xbee.puts("complete");
+    R=sqrt(X*X+Y*Y);
+    Theta_base_st=atan2(X,Y);
+    sprintf( st ,"Rover will move %f cm & %f cm \r\n",X,Y);
+    xbee.puts(st);
+    sprintf( st ,"Rover will move %f cm & %f Degree\r\n",R,Theta_base_st);
+    xbee.puts(st);
+
+}
+void motor_intialize()
+{
+    leftFrontMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
+    rightFrontMotor.period(0.00005);
+    leftBackMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
+    rightBackMotor.period(0.00005);
+}
+
+void initializePid(void)
+{
+    leftPid.setInputLimits(L_INPUT_MIN, L_INPUT_MAX);
+    leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX);
+    leftPid.setMode(AUTO_MODE);
+    rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX);
+    rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX);
+    rightPid.setMode(AUTO_MODE);
+
+}
+
+void PID_QEI_calculations()
+{
+    leftPulses = leftBackQei.getPulses();
+    leftVelocity = (leftPulses - leftPrevPulses) / RATE_PULSES;
+    leftPrevPulses = leftPulses;//Use the absolute value of velocity as the PID controller works in the % (unsigned) domain and will get confused with -ve values.
+    leftPid.setProcessValue(fabs(leftVelocity));
+
+    rightPulses = rightBackQei.getPulses();
+    rightVelocity = (rightPulses - rightPrevPulses) / RATE_PULSES;
+    rightPrevPulses = rightPulses;
+    rightPid.setProcessValue(fabs(rightVelocity));
+}
+void errorfun()
+{
+    error=current_theta-FIRtheta[0];
+    errorI=error+errorI;
+    sprintf( st ,"error:%f errprI:%f\r\n",error,errorI);
+    xbee.puts(st);
+    float h;
+    h=6*error+0.01*errorI;
+    sigmoid=4/(1+exp(h))-2;
+    if(sigmoid<0) {
+        RTP=1+sigmoid;
+        LTP=1;
+    } else if(sigmoid>0) {
+        RTP=1;
+        LTP=1-sigmoid;
+    } else {
+        RTP=1;
+        LTP=1;
+    }
+}
+void PWM_cal()
+{
+    errorfun();
+    RPWM=/*rightPid.compute()*/ 0.9*RTP;
+    LPWM=/*leftPid.compute()*/ 0.9*LTP;
+}
+void Distance_calculation()
+{
+    leftPulses  = leftBackQei.getPulses();
+    rightPulses = rightBackQei.getPulses();
+    leftDis  = (leftPulses)*2*3.14159*3/LPPD;
+    rightDis = (rightPulses)*2*3.14159*3/RPPD;
+    dis= (rightDis+leftDis)/2;
+}
+void Get_Window(int j,float *vec,float *vec2)
+{
+    float vector1[3]= {0,0,0};
+    for(int i=0; i<3; i++) {
+        vec[i]=0;
+        vec2[i]=0;
+    }
+    int k=0;
+    while(k<j) {
+        SS.Readgyros(vector1);
+        //SS.Readaccelerometer(vector2);
+        for(int i=0; i<3; i++) {
+            vec[i]=vector1[i]*vector1[i]+vec[i];
+        }
+        for(int i=0; i<3; i++) {
+            vec2[i]=vector1[i]+vec2[i];
+        }
+        k++;
+    }
+    for(int i=0; i<3; i++) {
+        vec[i]=vec[i]/(j);
+    }
+    for(int i=0; i<3; i++) {
+        vec2[i]=vec2[i]/(j);
+    }
+}
+void Energy_cal(float *Energy,float *W,float Threshold1[3])
+{
+    Get_Window(10,Energy,W);
+    for(int i=0; i<3; i++) {
+        if(Energy[i]<Threshold1[i]*sfgyro)W[i]=0;
+    }
+    for(int i=0; i<3; i++)W[i]=W[i];
+}
+void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3])
+{
+    Energy_cal(E,theta_dot,Threshold);
+    theta[0]=theta_dot[0]*(0.2)+theta[0];
+    theta[1]=theta_dot[1]*(0.2)+theta[1];
+    theta[2]=theta_dot[2]*(0.2)+theta[2];
+}
+void move(float Lperiod,float Rperiod)
+{
+    leftFrontMotor.speed(Lperiod);
+    leftBackMotor.speed(Lperiod);
+
+    rightFrontMotor.speed(Rperiod);
+    rightBackMotor.speed(Rperiod);
+}
+void Stop()
+{
+    leftFrontMotor.brake();
+    rightFrontMotor.brake();
+    leftBackMotor.brake();
+    rightBackMotor.brake();
+}
+
+void RotateLeft(float Lperiod,float Rperiod)
+{
+    leftFrontMotor.speed(-Lperiod);
+    leftBackMotor.speed(-Lperiod);
+
+    rightFrontMotor.speed(Rperiod);
+    rightBackMotor.speed(Rperiod);
+}
+
+void RotateRight(float Lperiod,float Rperiod)
+{
+    leftFrontMotor.speed(Lperiod);
+    leftBackMotor.speed(Lperiod);
+
+    rightFrontMotor.speed(-Rperiod);
+    rightBackMotor.speed(-Rperiod);
+}
+//////////////////////////////////////////////////////////////////////////////////////not used yet
+void rotation()
+{
+    timer2.detach();
+    if(current_theta-FIRtheta[0]>FIR_Thresh[0]) {
+        xbee.puts("rotate right\r\n");
+        while(FIRtheta[0]<current_theta) { //new_disctrete..........................................................
+            PID_QEI_calculations();
+            RotateRight(leftPid.compute(),rightPid.compute());
+            Distance_calculation();
+        }
+    } else if(current_theta-FIRtheta[0]<-1*FIR_Thresh[0]) {
+        xbee.puts("rotate left\r\n");
+        while(FIRtheta[0]>current_theta) {
+            PID_QEI_calculations();
+            RotateLeft(leftPid.compute(),rightPid.compute());
+            Distance_calculation();
+        }
+    } else {
+        xbee.puts("will not rotate\r\n");
+    }
+    theta_update=false;
+    timer2.attach(&navigation_model, navigationtimeinterrupt);
+}
+void GTG_mode()
+{
+    if(FWM)Wall_Following();
+    PID_QEI_calculations();
+    PWM_cal();
+    move(LPWM,RPWM);
+    Distance_calculation();
+/*    sprintf( st ,"\r\n\r\nR:%f prev:%f theta:%f GTG:%f FIR:%f Base:%f S:%f LTP:%f RTP:%f\r\n",R,prev_dis[0],current_theta*180/3.14,(current_theta-GTGtheta)*180/3.14,FIRtheta[0]*180/3.14,Theta_base_st*180/3.14,sigmoid,LTP,RTP);
+    xbee.puts(st);
+    sprintf( st ,"\r\n\r\nF:%f R:%f L:%f OD:%f OA:%f GO:%f ccw:%i cw:%i dot:%f Xtow:%f\r\n",Fobstacles_dis,Robstacles_dis,Lobstacles_dis,Obs_dis,Obs_alpha*180/3.14,gradient_Obs,(int)(dot_product_cond>0),(int)(post_dis[0]<Xtow),dot_product_cond,Xtow);
+    xbee.puts(st);
+*/
+    sprintf( st ," theta:%f FIR:%f OA:%f GO:%f 1st:%i 2nd:%i 3rd:%i\r\n",current_theta*180/3.14,FIRtheta[0]*180/3.14,Obs_alpha*180/3.14,gradient_Obs,(int)(dot_product_cond>0),(int)(post_dis[0]<Xtow),(int)(abs(current_Obs_dis)>60));
+    xbee.puts(st);
+    sprintf( st ,"DA:%f GTGA:%f OD:%f F:%f R:%f L:%f COD:%f\r\n",(current_theta-GTGtheta)*180/3.14,GTGtheta*180/3.14,Obs_dis,Fobstacles_dis,Robstacles_dis,Lobstacles_dis,current_Obs_dis);
+    xbee.puts(st);
+
+}
+void FollowWall()
+{
+    if(Fobstacles_dis<delta&&Fobstacles_dis>0&&!(cw&&Robstacles_dis==-1||ccw&&Lobstacles_dis==-1))First_FW();
+    //else if(FWM)Wall_Following();
+
+}
+void Wall_Following()
+{
+    xbee.puts("\r\nWF\r\n");
+    if(Obs_dis>0) { //clockwise
+        current_Obs_dis=Robstacles_dis;
+        cw=true;
+        ccw=false;
+        if(current_Obs_dis==0||current_Obs_dis==-1)
+            current_Obs_dis=150;//check conditions in this line
+    } else if(Obs_dis<0) { //anti clock wise
+        current_Obs_dis=-1*Lobstacles_dis;
+        cw=false;
+        ccw=true;
+        if(current_Obs_dis==0||current_Obs_dis==1)current_Obs_dis=-150;//check conditions in this line
+    }
+
+    Obs_h=current_Obs_dis-Obs_dis;
+    gradient_Obs=2/(1+exp(-0.05*Obs_h))-1;
+    //current_theta=current_theta+3.14*(gradient_Obs);//from here we shall begin.
+    h_Obsinv=-log(4/(gradient_Obs+2)-1);
+    error_inv=(h_Obsinv-0.01*errorI)/6;
+    current_theta=error_inv+current_theta;
+//we should write here exiting condition.
+    GTGtheta=atan2(post_dis[2],post_dis[1]);
+    dot_product_cond=cos(current_theta-GTGtheta);
+    if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) {
+        FWM=false;
+        GTGM=true;
+    }
+}
+void First_FW()
+{
+    xbee.puts("\r\nFFW\r\n");
+    Xtow=post_dis[0];//check if there is a proper plase to put Xtow more than this
+    Obs_adj=Fobstacles_dis;
+    min_Obs_opp();
+    Obs_hyp=sqrt(Obs_adj*Obs_adj+Obs_opp*Obs_opp);
+    Obs_dis=Obs_adj*Obs_opp/Obs_hyp;
+
+    Obs_alpha_opp=sqrt(Obs_adj*Obs_adj-Obs_dis*Obs_dis);
+    Obs_alpha=atan2(Obs_alpha_opp,Obs_dis);
+
+    if(Obs_alpha<3.1415/2)
+        Obs_alpha= Obs_alpha-1.57079;
+    else if(Obs_alpha>3.1415/2)
+        Obs_alpha= 3.1415-Obs_alpha;
+    if(Obs_alpha<0.0698&&Obs_alpha>-1*0.0698)
+        Obs_alpha=1.57079;
+    current_theta=current_theta+Obs_alpha;//FWtheta;
+    if(Obs_dis==0)Obs_dis=Obs_adj;//done.
+
+    FWM=true;
+    GTGM=false;
+    //exiting conditions.
+    GTGtheta=atan2(post_dis[2],post_dis[1]);
+    dot_product_cond=cos(current_theta-GTGtheta);//dot product check.
+    if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) {
+        FWM=false;
+        GTGM=true;
+    }
+}
+void min_Obs_opp()
+{
+    if(Robstacles_dis<0&&Lobstacles_dis<0)Obs_opp=0;
+    else if(Robstacles_dis<0&&Lobstacles_dis>0)Obs_opp=-1*Lobstacles_dis;
+    else if(Robstacles_dis>0&&Lobstacles_dis<0)Obs_opp=Robstacles_dis;
+    else if(Robstacles_dis>0&&Lobstacles_dis>0)Obs_opp = Lobstacles_dis>Robstacles_dis ? Robstacles_dis : -1*Lobstacles_dis;
+}
+void navigation_model()   //edit here
+{       
+    prev_dis[1]=prev_dis[1]+dis*cos(FIRtheta[0]);
+    prev_dis[2]=prev_dis[2]+dis*sin(FIRtheta[0]);
+    prev_dis[0]=sqrt(prev_dis[1]*prev_dis[1]+prev_dis[2]*prev_dis[2]);
+
+    post_dis[1]=R*cos(rot_theta)-prev_dis[1];
+    post_dis[2]=R*sin(rot_theta)-prev_dis[2];
+    post_dis[0]=sqrt(post_dis[1]*post_dis[1]+post_dis[2]*post_dis[2]);
+    if(FWM&&!GTGM||Fobstacles_dis<delta&&Fobstacles_dis>0&&post_dis[0]>Fobstacles_dis)//check conditions in this line
+        FollowWall();
+    if(GTGM&&!FWM) {
+        current_theta=atan2(post_dis[2],post_dis[1]);
+        Obs_alpha=0;
+        ccw=false;
+        cw=false;
+        xbee.puts("\r\nGTG\r\n");
+    }
+    leftBackQei.reset();
+    rightBackQei.reset();
+    dis=0;
+    theta_update=true;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b3110cd2dd17
\ No newline at end of file