![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
diff -r 000000000000 -r b703833f6795 Motor/Motor.cpp --- /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; + } + +}
diff -r 000000000000 -r b703833f6795 Motor/Motor.h --- /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
diff -r 000000000000 -r b703833f6795 PID/PID.cpp --- /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_; + +}
diff -r 000000000000 -r b703833f6795 PID/PID.h --- /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 */
diff -r 000000000000 -r b703833f6795 QEI/QEI.cpp --- /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_++; + +}
diff -r 000000000000 -r b703833f6795 QEI/QEI.h --- /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 */
diff -r 000000000000 -r b703833f6795 SHARPIR.cpp --- /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
diff -r 000000000000 -r b703833f6795 SHARPIR.h --- /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
diff -r 000000000000 -r b703833f6795 SpaceSensor.cpp --- /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); +}
diff -r 000000000000 -r b703833f6795 SpaceSensor.h --- /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
diff -r 000000000000 -r b703833f6795 main.cpp --- /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; +}
diff -r 000000000000 -r b703833f6795 mbed.bld --- /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