quadcopter cufe
follow wall 1
Revision 0:b703833f6795, committed 2014-04-08
- Comitter:
- khaledelmadawi
- Date:
- Tue Apr 08 13:23:06 2014 +0000
- Commit message:
- follow wall 1
Changed in this revision
--- /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