Go to the documentation of this file.00001 #ifndef ENCODER_STATE_CALCULATOR_H
00002 #define ENCODER_STATE_CALCULATOR_H
00003
00004 #include <stdint.h>
00005 #include <cmath>
00006 #include <boost/shared_ptr.hpp>
00007 #include <boost/math/constants/constants.hpp>
00008 #include <stdexcept>
00009
00010 class EncoderStateCalculator
00011 {
00012 public:
00013 EncoderStateCalculator(const double& jointGearRatio, const double& pwmFrequency, const double& pwmDeadTime, const double& busVoltage, const double& motorCurrentLimit, const double& phaseResistance,
00014 const double& backEmfConstant, const double& jointKinematicDirection, const double& encoderMountingDirection,
00015 const double& encoderGlitchScalar, const double& encoderVelocityBlendStop, const double& encoderVelocityBlendStart);
00016 virtual ~EncoderStateCalculator();
00017
00018 double getPosition(const double& encoderPositionReference, const int32_t& incrementalEncoderReference, const int32_t& incrementalEncoderNow);
00019 double getVelocity(int32_t timer, const int32_t& timeout, const int32_t& encoderAccumulation, const double& measuredControlRateHz);
00020
00022 double encoderCountsPerMotorRevolution;
00023 double encoderRadiansPerCount;
00024 double encoderCountsToJointRadians;
00025 double busVoltageRatio;
00026 double maxEncoderVelocity;
00027 double encoderVelocityLowFrequency;
00028 double encoderVelocityLowFrequencyPrevious;
00029 double encoderVelocityHighFrequency;
00030 double encoderVelocityBlended;
00031 double encoderPosition;
00032 int32_t encoderAccumulationPrevious;
00033 double gammaBlend;
00034 double gammaBlendDenominator;
00035
00036 private:
00037 double applyLimits(const double& value, const double& lowerLimit, const double& upperLimit);
00038
00040 double jointGearRatio;
00041 double pwmFrequency;
00042 double pwmDeadTime;
00043 double busVoltage;
00044 double motorCurrentLimit;
00045 double phaseResistance;
00046 double backEmfConstant;
00047 double jointKinematicDirection;
00048 double encoderMountingDirection;
00049 double encoderGlitchScalar;
00050 double encoderVelocityBlendStop;
00051 double encoderVelocityBlendStart;
00052
00054 double encoderTickTimeout;
00055 double clockFrequency;
00056 double backEmfConstantMinimum;
00057 };
00058
00059 typedef boost::shared_ptr<EncoderStateCalculator> EncoderStateCalculatorPtr;
00060
00061 class MotorEncoderStateCalculator : public EncoderStateCalculator
00062 {
00063 public:
00064 MotorEncoderStateCalculator(const double& jointGearRatio, const double& pwmFrequency, const double& pwmDeadTime, const double& busVoltage, const double& motorCurrentLimit, const double& phaseResistance,
00065 const double& backEmfConstant, const double& jointKinematicDirection, const double& encoderMountingDirection, const double& encoderCountsPerRevolution,
00066 const double& encoderGlitchScalar, const double& encoderVelocityBlendStop, const double& encoderVelocityBlendStart);
00067 virtual ~MotorEncoderStateCalculator();
00068
00069 private:
00070 double encoderCountsPerRevolution;
00071 };
00072
00073 typedef boost::shared_ptr<MotorEncoderStateCalculator> MotorEncoderStateCalculatorPtr;
00074
00075 class HallsEncoderStateCalculator : public EncoderStateCalculator
00076 {
00077 public:
00078 HallsEncoderStateCalculator(const double& jointGearRatio, const double& pwmFrequency, const double& pwmDeadTime, const double& busVoltage, const double& motorCurrentLimit, const double& phaseResistance,
00079 const double& backEmfConstant, const double& jointKinematicDirection, const double& encoderMountingDirection, const double& motorPoleCount,
00080 const double& encoderGlitchScalar, const double& encoderVelocityBlendStop, const double& encoderVelocityBlendStart);
00081 virtual ~HallsEncoderStateCalculator();
00082 private:
00083 double motorPoleCount;
00084 };
00085
00086 typedef boost::shared_ptr<HallsEncoderStateCalculator> HallsEncoderStateCalculatorPtr;
00087
00088 #endif