EsdMessageSniffer.h

Go to the documentation of this file.

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
/*
 * Copyright (C) 2006 Giorgio Metta
 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
 *
 */
 
//
// $Id: EsdMessageSniffer.h,v 1.1 2007/07/26 08:24:49 babybot Exp $
//
//
 
#ifndef __YARPEsdMessageSnifferh__
#define __YARPEsdMessageSnifferh__
 
#include <mutex>
#include <condition_variable>
 
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/ControlBoardInterfacesImpl.h>
#include <yarp/os/Thread.h>
 
namespace yarp{
    namespace dev{
        class EsdMessageSniffer;
        struct EsdMessageSnifferParameters;
    }
}
 
struct yarp::dev::EsdMessageSnifferParameters
{
    EsdMessageSnifferParameters(int nj);
 
    ~EsdMessageSnifferParameters();
 
    long int _txQueueSize;
    long int _rxQueueSize;
    long int _txTimeout;
    long int _rxTimeout;
 
    int _networkN;                              
    int _njoints;                               
    unsigned char _my_address;                  
    int _polling_interval;                      
    int _timeout;                               
    int (*_p) (const char *fmt, ...);           
    unsigned char *_destinations;               
    int *_axisMap;                              
    double *_angleToEncoder;                    
    double *_zeros;                             
};
 
class yarp::dev::EsdMessageSniffer : 
    public DeviceDriver,
            public os::Thread, 
            public IPidControlRaw, 
            public IAmplifierControlRaw,
            public IEncodersRaw, 
            public ImplementPidControl<EsdMessageSniffer, IPidControl>,
            public ImplementAmplifierControl<EsdMessageSniffer, IAmplifierControl>,
            public ImplementEncoders<EsdMessageSniffer, IEncoders>
{
private:
    EsdMessageSniffer(const EsdMessageSniffer&);
    void operator=(const EsdMessageSniffer&);
 
protected:
    inline bool NOT_YET_IMPLEMENTED(const char *txt) {
        fprintf(stderr, "%s not yet implemented for EsdMotionControl\n", txt);
        return false;
    }
 
public:
    EsdMessageSniffer();
 
    virtual ~EsdMessageSniffer();
 
    virtual bool open(EsdMessageSnifferParameters& params);
 
    virtual bool open(yarp::os::Searchable& config);
 
    virtual bool close(void);
 
    virtual bool getAxes(int *axis);
 
    virtual bool resetEncoderRaw(int j) { return NOT_YET_IMPLEMENTED("resetEncoderRaw"); }
    virtual bool resetEncodersRaw() { return NOT_YET_IMPLEMENTED("resetEncodersRaw"); }
    virtual bool setEncoderRaw(int j, double val) { return NOT_YET_IMPLEMENTED("setEncoderRaw"); }
    virtual bool setEncodersRaw(const double *vals) { return NOT_YET_IMPLEMENTED("setEncodersRaw"); }
    virtual bool getEncoderSpeedRaw(int j, double *sp) { return NOT_YET_IMPLEMENTED("getEncoderSpeedRaw"); }
    virtual bool getEncoderSpeedsRaw(double *spds) { return NOT_YET_IMPLEMENTED("getEncoderSpeedsRaw"); }
    virtual bool getEncoderAccelerationRaw(int j, double *spds) { return NOT_YET_IMPLEMENTED("getEncoderAccelerationRaw"); }
    virtual bool getEncoderAccelerationsRaw(double *accs) { return NOT_YET_IMPLEMENTED("getEncoderAccelerationsRaw"); }
 
    virtual bool getEncoderRaw(int j, double *v) { return getBCastPosition(j, v); }
 
    virtual bool getEncodersRaw(double *encs) { return getBCastPositions(encs); }
 
    virtual bool getErrorRaw(int j, double *err) { return getBCastPositionError(j, err); }
 
    virtual bool getErrorsRaw(double *errs) { return getBCastPositionErrors(errs); }
 
    virtual bool getOutputRaw(int j, double *out) { return getBCastPIDOutput(j, out); }
 
    virtual bool getOutputsRaw(double *outs) { return getBCastPIDOutputs(outs); }
 
    virtual bool setPidRaw(int j, const Pid &pid) { return NOT_YET_IMPLEMENTED("setPidRaw"); }
    virtual bool setPidsRaw(const Pid *pids) { return NOT_YET_IMPLEMENTED("setPidsRaw"); }
    virtual bool setReferenceRaw(int j, double ref) { return NOT_YET_IMPLEMENTED("setReferenceRaw"); }
    virtual bool setReferencesRaw(const double *refs) { return NOT_YET_IMPLEMENTED("setReferencesRaw"); }
    virtual bool setErrorLimitRaw(int j, double limit) { return NOT_YET_IMPLEMENTED("setErrorLimitRaw"); }
    virtual bool setErrorLimitsRaw(const double *limits) { return NOT_YET_IMPLEMENTED("setErrorLimitsRaw"); }
    virtual bool getPidRaw(int j, Pid *pid) { return NOT_YET_IMPLEMENTED("getPidRaw"); }
    virtual bool getPidsRaw(Pid *pids) { return NOT_YET_IMPLEMENTED("getPidsRaw"); }
    virtual bool getReferenceRaw(int j, double *ref) { return NOT_YET_IMPLEMENTED("getReferenceRaw"); }
    virtual bool getReferencesRaw(double *refs) { return NOT_YET_IMPLEMENTED("getReferencesRaw"); }
    virtual bool getErrorLimitRaw(int j, double *limit) { return NOT_YET_IMPLEMENTED("getErrorLimitRaw"); }
    virtual bool getErrorLimitsRaw(double *limits) { return NOT_YET_IMPLEMENTED("getErrorLimitsRaw"); }
    virtual bool resetPidRaw(int j) { return NOT_YET_IMPLEMENTED("resetPidRaw"); }
    virtual bool disablePidRaw(int j) { return NOT_YET_IMPLEMENTED("disablePidRaw"); }
    virtual bool enablePidRaw(int j) { return NOT_YET_IMPLEMENTED("enablePidRaw"); }
    virtual bool setOffsetRaw(int j, double v) { return NOT_YET_IMPLEMENTED("setOffsetRaw"); }
 
    virtual bool getCurrentsRaw(double *vals) { return getBCastCurrents(vals); }
 
    virtual bool getCurrentRaw(int j, double *val) { return getBCastCurrent(j, val); }
 
    /* Get the status of the amplifiers, coded in a 32 bits integer for
     * each amplifier (at the moment contains only the fault, it will be 
     * expanded in the future).
     * @param st pointer to storage
     * @return true in good luck, false otherwise.
     */
    virtual bool getAmpStatusRaw(int *st) { return getBCastFaults(st); }
 
    virtual bool enableAmpRaw(int j) { return NOT_YET_IMPLEMENTED("enableAmpRaw"); }
    virtual bool disableAmpRaw(int j) { return NOT_YET_IMPLEMENTED("disableAmpRaw"); }
    virtual bool setMaxCurrentRaw(int j, double v) { return NOT_YET_IMPLEMENTED("setMaxCurrentRaw"); }
 
protected:
 
    bool setBCastMessages (int i, int value);
    bool getBCastPositions (double *p);
    bool getBCastPosition (int i, double *value);
    bool getBCastPIDOutput (int i, double *value);
    bool getBCastPIDOutputs (double *p);
    bool getBCastCurrent (int i, double *value);
    bool getBCastCurrents (double *p);
    bool getBCastFaults (int *p);
    bool getBCastPositionError (int i, double *value);
    bool getBCastPositionErrors (double *cmd);
 
    bool setDebugPrintFunction (void *cmd);
 
protected:
    void *system_resources;
    std::mutex _mutex;
    std::mutex mtx_done;
    std::condition_variable cv_done;
 
    bool _writerequested;
    bool _noreply;
 
    inline bool ENABLED (int axis);
 
    int (*_p) (const char *fmt, ...);
 
    virtual void run(void);
 
    bool _writeWord16 (int msg, int axis, short s);
    bool _writeWord16Ex (int msg, int axis, short s1, short s2);
    bool _readWord16 (int msg, int axis, short& value);
    bool _readWord16Array (int msg, double *out);
    bool _readDWord (int msg, int axis, int& value);
    bool _readDWordArray (int msg, double *out);
    bool _writeDWord (int msg, int axis, int value);
    bool _writeNone (int msg, int axis);
 
    enum { MAX_SHORT = 32767, MIN_SHORT = -32768, MAX_INT = 0x7fffffff, MIN_INT = 0x80000000 };
    enum { ESD_CAN_SKIP_ADDR = 0x80 };
 
    inline short S_16(double x) const 
    {
        if (x <= double(-(MAX_SHORT))-1)
            return MIN_SHORT;
        else
            if (x >= double(MAX_SHORT))
                return MAX_SHORT;
            else
                return short(x + .5);
    }
 
    inline int S_32(double x) const
    {
        if (x <= double(-(MAX_INT))-1.0)
            return MIN_INT;
        else
            if (x >= double(MAX_INT))
                return MAX_INT;
            else
                return int(x + .5);
    }
};
 
 
#endif