jameshead.cpp

来自「一个语言识别引擎」· C++ 代码 · 共 1,200 行 · 第 1/4 页

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-

/*
 * Copyright (C) 2006 Lorenzo Natale, Giorgio Metta, Francesco Nori
 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
 *
 */


#include <stdio.h>
//#include <ace/os.h>
#include <math.h>
#include <yarp/JamesHead.h>
#include <yarp/os/RateThread.h>
#include <yarp/os/Semaphore.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/sig/Vector.h>
#include <yarp/dev/GenericSensorInterfaces.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/os/Time.h>

#include "../../../ControlBoardInterfacesImpl.inl"

using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;

const double    P_GAIN=0.008;
const int       NC_RATE=20;
const double    IN_POSITION_THRESHOLD = 0.1;
const double    REL_WEIGHT = 0.9;					//WARNING: has to be set between 0 and 1!!

const int HEAD_JOINTS=7;

void computeTendonsLength(double &d1, double &d2, double &d3, double Roll, double Pitch);
void computeModifiedPitchRoll(double Yaw, double Roll, double Pitch, double &Roll_hat, double &Pitch_hat);
void computeOriginalPitchRoll(double Yaw, double &Roll, double &Pitch, double Roll_hat, double Pitch_hat);

class HeadControl;
inline HeadControl* my_cast(void *p)
{
    return static_cast<HeadControl *>(p);
}

class HeadControl: public RateThread
{
private:
    PolyDriver *ddInertia;
    PolyDriver *ddHead;
    
    IGenericSensor *isensor;
    IVelocityControl *ivel;
    IPidControl *ipid;
    IEncoders *iencs;
    IAmplifierControl *iamps;
    IControlCalibration *icalib;
    IPositionControl *ipos;

    double *velocityCmds;
    double *positionCmds;
    double *positions;
public:
    HeadControl(int r, PolyDriver *inertia, PolyDriver *head): RateThread(r),
                                                               ddInertia(0),
                                                               ddHead(0),
                                                               inertiaValue(3),
                                                               encoders(0),
                                                               nAxes(0)
    {
        pGain=P_GAIN;

        if (inertia!=0)
            ddInertia=inertia;
        if (head!=0)
            ddHead=head;

        vCmds=new double [4];

        positions=new double [HEAD_JOINTS];
        positionCmds=new double [HEAD_JOINTS];
        velocityCmds=new double [HEAD_JOINTS];

        for(int k=0; k<HEAD_JOINTS; k++)
            {
                positions[k]=0.0;
                positionCmds[k]=0.0;
                velocityCmds[k]=0.0;
            }
    }
    
    ~HeadControl()
    {
        delete [] vCmds;
        delete [] positions;
        delete [] velocityCmds;
        delete [] positionCmds;
    }

    bool getRefAcceleration(int j, double *acc)
    {
		mutex.wait();
        for(int k=0;k<5;k++)
            ipos->getRefAcceleration(j, acc);

		mutex.post();
        return true;
    }

    bool getRefAccelerations(double *accs)
    {
		mutex.wait();

        for(int k=0;k<5;k++)
            ipos->getRefAcceleration(k, accs+k);

		mutex.post();
        return true; 
    }

    bool getRefSpeed(int j, double *sp)
    {
		mutex.wait();
        for(int k=0;k<5;k++)
            ipos->getRefSpeed(j, sp);

		mutex.post();
        return true;
    }

    bool getRefSpeeds(double *sps)
    {
		mutex.wait();
        for(int k=0;k<5;k++)
            ipos->getRefSpeed(k, sps+k);

		mutex.post();
        return true; 
    }

    bool setRefSpeeds(const double *sp)
    {
		mutex.wait();
        for(int k=0;k<5;k++)
            ipos->setRefSpeed(k, sp[k]);

		mutex.post();
        return true;
    }

    bool setRefSpeed(int j, double sp)
    {
		mutex.wait();
        if (j<5)
            ipos->setRefSpeed(j, sp);
		mutex.post();
        return true;
    }

    bool setRefAcceleration(int j, double acc)
    {
		mutex.wait();
        if (j<5)
            ipos->setRefAcceleration(j, acc);
    
		mutex.post();
        return true;
    }

    bool setRefAccelerations(const double *acc)
    {
		mutex.wait();
        for(int k=0;k<5;k++)
            ipos->setRefAcceleration(k, acc[k]);

		mutex.post();
        return true;
    }

    bool positionMove(int j, double pos)
    {
        mutex.wait();
        positionCmds[j]=pos;

        if (j<5)
            ipos->positionMove(j, pos);

		mutex.post();

        return true;
    }

    bool positionMove(const double *pos)
    {
        mutex.wait();
        for(int k=0;k<HEAD_JOINTS;k++)
            {
                positionCmds[k]=pos[k];
                if (k<5)
                    ipos->positionMove(k, pos[k]);
            }
        mutex.post();
       
        return true;
    }

    bool velocityMove(int j, double vel)
    {
        mutex.wait();
        velocityCmds[j]=vel;
 
        if (j<5)
            ivel->velocityMove(j, vel);

        mutex.post();

        return true;
    }

    bool velocityMove(const double *vel)
    {
        mutex.wait();
        for(int k=0;k<HEAD_JOINTS;k++)
            {
                velocityCmds[k]=vel[k];
                if (k<5)
                    ivel->velocityMove(k, vel[k]);
            }
        mutex.post();

        return true;
    }

    bool relativeMove(int j, double d)
    {
		mutex.wait();

        if (j<5)
            ipos->relativeMove(j,d);
        
		mutex.post();
        return true;
    }

    bool relativeMove(const double *dq)
    { 
		mutex.wait();
        for(int k=0;k<HEAD_JOINTS;k++)
            {
                if (k<5)
                    ipos->relativeMove(k,dq[k]);
            }
        
		mutex.post();
        return true;
    }

    bool checkMotionDone(int j, bool *fl)
    {
		mutex.wait();
        if (j<5)
            ipos->checkMotionDone(j, fl);
		mutex.post();
        return true;
    }

    bool checkMotionDone(bool *fl)
    {
		mutex.wait();
        for(int k=0;k<HEAD_JOINTS;k++)
            {
                if (k<5)
                    ipos->checkMotionDone(k,&fl[k]);
                else
                    fl[k]=true;
            }
		mutex.post();

        return true;
    }

    bool enablePid(int j)
    {
		bool ret=false;
		mutex.wait();
        if (j<5)
            ret=ipid->enablePid(j);
        
		mutex.post();
		return ret;
    }

    bool disableAmp(int j)
    {
		bool ret=false;
		mutex.wait();
        if (j<5)
            ret=iamps->disableAmp(j);
        
		mutex.post();
		return ret;