EncoderPositionSensor.hpp

/***************************************************************************
  tag: Peter Soetens  Tue May 4 16:49:13 CEST 2004  EncoderPositionSensor.hpp

                        EncoderPositionSensor.hpp -  description
                           -------------------
    begin                : Tue May 04 2004
    copyright            : (C) 2004 Peter Soetens
    email                : peter.soetens@mech.kuleuven.ac.be

 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/

#ifndef ENCODER_POSITION_SENSOR_HPP
#define ENCODER_POSITION_SENSOR_HPP

#include <rtt/dev/EncoderInterface.hpp>
#include <rtt/dev/SensorInterface.hpp>
#include <rtt/dev/CalibrationInterface.hpp>

#include <limits>

namespace OCL
{


    class EncoderPositionSensor
        : public SensorInterface<double>
    {
        EncoderInterface* enc;
        double unit_to_inc;
        double min;
        double max;
        double posOffset;
        double cal_pos;
        double cal_dir;
    public:
        EncoderPositionSensor(EncoderInterface* _enc, double _unit_to_inc, double _minpos, double _maxpos)
            : enc(_enc), unit_to_inc(_unit_to_inc), min(_minpos), max(_maxpos), posOffset(0),
              cal_pos(0), cal_dir(-1)
        {}

        virtual int readSensor( double& p ) const
        {
            p = readSensor();
            return 0;
        }

        void limit(double _min, double _max)
        {
            min = _min;
            max = _max;
        }
        void calibrationPosition( double pos )
        {
            cal_pos = pos;
        }
        void calibrationDirection( double dir )
        {
            cal_dir = dir;
        }

        virtual void calibrate()
        {
            this->calibrate( cal_pos, cal_dir );
        }

        virtual void unCalibrate()
        {
            this->calibrated = false;
        }

        virtual bool isCalibrated() const
        {
            return this->calibrated;
        }

        virtual double readSensor() const
        {
            return double( enc->turnGet()*enc->resolution() +  enc->positionGet() ) / unit_to_inc + posOffset;
        }

        virtual double maxMeasurement() const
        {
            return calibrated ? max : std::numeric_limits<double>::max();
        }

        virtual double minMeasurement() const
        {
            // ::min() is the <smallest> number ( cfr 0 )
            return calibrated ? min : -std::numeric_limits<double>::max();
        }

        virtual double zeroMeasurement() const
        {
            return 0.0;
        }

        double calibrate( double calpos, double sign )
        {
            double currentPos;
            if ( sign < 0 )
                {
                    if ( unit_to_inc > 0)
                        currentPos = calpos - ( enc->resolution() - enc->positionGet() ) / unit_to_inc;
                    else
                        currentPos = calpos - enc->positionGet() / unit_to_inc;
                }
            else
                {
                    if ( unit_to_inc > 0)
                        currentPos =  calpos + enc->positionGet() / unit_to_inc;
                    else
                        currentPos = calpos + (enc->resolution() - enc->positionGet() ) / unit_to_inc;
                }
            double oldposition = readSensor();
            posOffset = currentPos -  (readSensor() - posOffset); //calculate the offset
            calibrated = true;
            return readSensor() - oldposition;
        }
    };

}

#endif

---