jameshead.cpp

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

    }

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

		return ret;
    }

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

    bool halt(int j)
    { return true;}

    bool halt()
    {return true;}

    bool getEncoders(double *v)
    {
        mutex.wait();
        memcpy(v, positions, sizeof(double)*HEAD_JOINTS);
        mutex.post();

        return true;
    }

    bool getEncoder(int j, double *v)
    {
        mutex.wait();
        *v=positions[j];
        mutex.post();

        return true;
    }

    bool start()
    {
        ACE_OS::printf("NeckControl::starting...");

        bool ok;
        if (ddInertia==0)
            return false;
        if (ddHead==0)
            return false;

        ACE_OS::printf("successful\n");

        ok=ddInertia->view(isensor);
        if (!ok)
            {
                ACE_OS::printf("NeckControl::Error getting IGenericSensor interface, returning false\n");
                return false;
            }

        // calibration
        isensor->calibrate(0,0);
 
        ok=ddHead->view(ivel);
        ok=ok && ddHead->view(iencs);
        ok=ok && ddHead->view(iamps);
        ok=ok && ddHead->view(ipid);
        ok=ok && ddHead->view(icalib);
        ok=ok && ddHead->view(ipos);
        
        if (!ok)
            {
                ACE_OS::printf("NeckControl::Error getting head interfaces, returning false\n");
                return false;
            }

		mutex.wait();
        iencs->getAxes(&nAxes);
		mutex.post();
        encoders=new double [nAxes];
        return RateThread::start();
    }

	void enableControl()
	{
		ipid->enablePid(4);
		ipid->enablePid(5);
        ipid->enablePid(6);
        ipid->enablePid(7);

		iamps->enableAmp(4);
        iamps->enableAmp(5);
        iamps->enableAmp(6);
        iamps->enableAmp(7);
	}

	void disableControl()
	{
		ipid->disablePid(4);
        ipid->disablePid(5);
        ipid->disablePid(6);
        ipid->disablePid(7);

		iamps->disableAmp(4);
        iamps->disableAmp(5);
        iamps->disableAmp(6);
        iamps->disableAmp(7);
	}

    void doInit()
    {
		mutex.wait();
        ivel->setRefAcceleration(5, 1000);
        ivel->setRefAcceleration(6, 1000);
        ivel->setRefAcceleration(7, 1000);

		enableControl();
		
		ACE_OS::printf("NeckControl::Starting calibration sequence");
		yawCalibrate();
		rollPitchCalibrate();

		mutex.post();
    }

	void yawCalibrate()
	{
		double pos;
		bool done;

		ACE_OS::printf("Calibrating yaw\n");

		icalib->calibrate(4, -200);

		while(!icalib->done(4))
			ACE_OS::printf(".");

		ACE_OS::printf("\nMoving to the center:");

		iencs->getEncoder(4, &pos);
		ipos->setRefSpeed(4,20);
		ipos->positionMove(4, 90);

		done = false;
		while(!done)
            {
                if (ipos->checkMotionDone(4, &done))
                    ACE_OS::printf(".");
                else
                    ACE_OS::printf("CheckMotionDone returned false\n");
            }
		
		ACE_OS::printf("\nYaw calibration terminated");
 
	}

	void rollPitchCalibrate()
	{
		ACE_OS::printf("Calibrating roll and pitch\n");

		bool inPosition = false;

        double roll_d=0;
        double pitch_d=0;

		ACE_OS::printf("Trying to reach zero configuration: ");
		while(!inPosition)
            {
                ACE_OS::printf(".");

                isensor->read(inertiaValue);

                roll=inertiaValue[0];
                pitch=inertiaValue[1];

                vCmds[0]=-600*(roll-roll_d);
                vCmds[1]=500*(pitch-pitch_d)+400*(roll-roll_d);
                vCmds[2]=-500*(pitch-pitch_d)+400*(roll-roll_d);
 
                vCmds[0]*=pGain;
                vCmds[1]*=pGain;
                vCmds[2]*=pGain;

                if (fabs(roll - roll_d) < IN_POSITION_THRESHOLD)
                    {
                        if (fabs(pitch - pitch_d) < IN_POSITION_THRESHOLD)
                            {
                                ACE_OS::printf("\nZero Configuration Reached\n");

                                inPosition = true;

                                vCmds[0]=0;
                                vCmds[1]=0;
                                vCmds[2]=0;
                            }
                    }

                //ivel->setRefAcceleration(5, 1000);
                //ivel->setRefAcceleration(6, 1000);
                //ivel->setRefAcceleration(7, 1000);

                ivel->velocityMove(5, vCmds[0]);
                ivel->velocityMove(6, vCmds[1]);
                ivel->velocityMove(7, vCmds[2]);
            }

		double pos;
		bool done;

		ACE_OS::printf("Pulling the cables:");
		icalib->calibrate(5, 400);
		icalib->calibrate(6, 400);
		icalib->calibrate(7, 400);

        int k=0;
		for (k = 5; k <8; k++)
            {
                while(!icalib->done(k))
                    ACE_OS::printf(".");
            }

		ACE_OS::printf("\nReleasing cable:");
		for (k = 5; k <8; k++)
            {
                iencs->getEncoder(k, &pos);
                ipos->setRefSpeed(k,5);
                ipos->positionMove(k, pos-5);
            }

		for (k = 5; k <8; k++)
            {
                done = false;
                while(!done)
                    {
                        if (ipos->checkMotionDone(k, &done))
                            ACE_OS::printf(".");
                        else
                            ACE_OS::printf("CheckMotionDone returned false\n");
                    }
            }
		
		ACE_OS::printf("\n");

	    disableControl();

		iencs->resetEncoder(5);
		iencs->resetEncoder(6);
		iencs->resetEncoder(7);

		enableControl();
	}

    void doLoop()
    {
        double t1=Time::now();
        static int count=0;
        double d1;              //length[cm] of the cable attached to joint 7
        double d2;              //length[cm] of the cable attached to joint 6
        double d3;              //length[cm] of the cable attached to joint 5

        double roll_d;
        double pitch_d;

        mutex.wait();

        isensor->read(inertiaValue);
        iencs->getEncoders(encoders);

        roll=inertiaValue[0];
        pitch=inertiaValue[1];
        yaw=encoders[4];

        positions[0]=encoders[0];
        positions[1]=encoders[1];
        positions[2]=encoders[2];
        positions[3]=encoders[3];
        positions[4]=encoders[4];
        positions[5]=roll;
        positions[6]=pitch;

		roll_d=positionCmds[5];
        pitch_d=positionCmds[6];

 		//accounts for the fact that the base of the neck
		//is not affected by the yaw (pan of the head)
		//while the sensor is affected.
		double pitch_hat;
		double roll_hat;

		double pitch_d_hat;
		double roll_d_hat;