naogetup.cpp

agentspark 机器人模拟代码 适用robocup 机器人步态模拟仿真（机器人动作在NAOGETUP.cpp下修改）

/***************************************************************************
 *   Copyright (C) 2008 by Neil_Zhu   *
 *   [e-mail]   *
 *   Walk Test                                                             *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program 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 General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include "NaoGetUp.h"
#include "./WorldModel/self.h"
#include "./RobotModel/nao.h"

using namespace std;

void NaoGetUp::Init()
{
    Action::Init();
}

void NaoGetUp::SetParam()
{		cout << "NaoGetUp::SetParam" << endl;
    	mPhase              = GETUP1;
    	mPhaseCycle         = 0;
    	mPhaseStartCycle    = SELF.CurrentCycle;
	
}
void NaoGetUp::Execute()
{	gettingUp();
	switch (mPhase)
    	{
        	case GETUP1:
			getUp1();
		break;
		case GETUP2:
			getUp2();
		break;
		case GETUP3:
			getUp3();
		break;
		case GETUP4:
			getUp4();
		break;
		case GETUP5:
			getUp5();
		break;
		case GETUP6:
			getUp6();
		break;
		case GETUP7:
			getUp7();
		break;
	}
}

void NaoGetUp::gettingUp()
{	cout << "GettingUp: " << SELF.CurrentCycle<<"  "<< mPhaseStartCycle << endl;
	/*if( mPhase == GETUP1 && (SELF.CurrentCycle - mPhaseStartCycle) > 40)
	{	mPhaseStartCycle = SELF.CurrentCycle;
		mPhase = GETUP2;
	}
	if( mPhase == GETUP2 && (SELF.CurrentCycle - mPhaseStartCycle) > 30)
	{	mPhaseStartCycle = SELF.CurrentCycle;
		mPhase = GETUP3;
	}
	if( mPhase == GETUP3 && (SELF.CurrentCycle - mPhaseStartCycle) > 30)
	{	mPhaseStartCycle = SELF.CurrentCycle;
		mPhase = GETUP4;
	}
	if( mPhase == GETUP4 && (SELF.CurrentCycle - mPhaseStartCycle) > 35)
	{	mPhaseStartCycle = SELF.CurrentCycle;
		mPhase = GETUP5;
	}
	if( mPhase == GETUP5 && (SELF.CurrentCycle - mPhaseStartCycle) > 30)
	{	mPhaseStartCycle = SELF.CurrentCycle;
		mPhase = GETUP6;
	}
	if( mPhase == GETUP6 && (SELF.CurrentCycle - mPhaseStartCycle) > 30)
	{	mPhaseStartCycle = SELF.CurrentCycle;
		mPhase = GETUP7;
	}
	if( mPhase == GETUP7 && (SELF.CurrentCycle - mPhaseStartCycle) > 30)
	{	mPhaseStartCycle = SELF.CurrentCycle;
		mIsDone = true;
	}*/
}

void NaoGetUp::getUp1()
{	/**&head1;=-2,&head2;=-45,&larm1;=-120,&larm2;= 	20,&larm3;=-80,&larm4;=-80,&rarm1;=-120,&rarm2;=-10,&rarm3;= 80,&rarm4;= 80,&lleg1;= 0,&lleg2;= 0,&lleg3;= 35,&lleg4;=-84,&lleg5;= 70,&lleg6;= 0,&rleg1;= 0,&rleg2;= 0,&rleg3;= 35,&rleg4;=-84,&rleg5;= 70,&rleg6;= 0;*/
	/*CalculateVel(Nao::JID_LARM_1, -120.0, 40);  CalculateVel(Nao::JID_RARM_1, -120.0, 40);
    	CalculateVel(Nao::JID_LARM_3, 20.0, 40);    CalculateVel(Nao::JID_RARM_3, -10.0, 40);
   	CalculateVel(Nao::JID_LARM_2, -80.0, 40);  CalculateVel(Nao::JID_RARM_2, 80.0, 40);
    	CalculateVel(Nao::JID_LARM_4, -80.0, 40);  CalculateVel(Nao::JID_RARM_4, 80.0, 40);*/
	CalculateVel(Nao::JID_LLEG_1, 0.0, 40);  CalculateVel(Nao::JID_RLEG_1, -0.0, 40);
    	CalculateVel(Nao::JID_LLEG_3, 0.0, 40);    CalculateVel(Nao::JID_RLEG_3, -0.0, 40);
   	CalculateVel(Nao::JID_LLEG_2, 35.0, 40);  CalculateVel(Nao::JID_RLEG_2, 35.0, 40);
    	CalculateVel(Nao::JID_LLEG_4, -84.0, 40);  CalculateVel(Nao::JID_RLEG_4, -84.0, 40);
	CalculateVel(Nao::JID_LLEG_5, 70.0, 40);  CalculateVel(Nao::JID_RLEG_5, 70.0, 40);
    	CalculateVel(Nao::JID_LLEG_6, 0.0, 40);  CalculateVel(Nao::JID_RLEG_6, 0.0, 40);
}
void NaoGetUp::getUp2()
{	/**&head1;=-2,&head2;=-45,&larm1;=-56,&larm2;= 20,&larm3;=-80,&larm4;=-80,&rarm1;=-50,&rarm2;=-10,&rarm3;= 80,&rarm4;= 80,&lleg1;= -29,&lleg2;= 0,&lleg3;= 35,&lleg4;=-25,&lleg5;= 60,&lleg6;=-20,&rleg1;= -29,&rleg2;= 0,&rleg3;= 35,&rleg4;=-24,&rleg5;= 59,&rleg6;=22;*/
	CalculateVel(Nao::JID_LARM_1, -56.0, 40);  CalculateVel(Nao::JID_RARM_1, -50.0, 40);
    	CalculateVel(Nao::JID_LARM_2, 20.0, 40);    CalculateVel(Nao::JID_RARM_2, -10.0, 40);
   	CalculateVel(Nao::JID_LARM_3, -80.0, 40);  CalculateVel(Nao::JID_RARM_3, 80.0, 40);
    	CalculateVel(Nao::JID_LARM_4, -80.0, 40);  CalculateVel(Nao::JID_RARM_4, 80.0, 40);
	CalculateVel(Nao::JID_LLEG_1, -29.0, 40);  CalculateVel(Nao::JID_RLEG_1, -29.0, 40);
    	CalculateVel(Nao::JID_LLEG_2, 0.0, 40);    CalculateVel(Nao::JID_RLEG_2, -0.0, 40);
   	CalculateVel(Nao::JID_LLEG_3, 35.0, 40);  CalculateVel(Nao::JID_RLEG_3, 35.0, 40);
    	CalculateVel(Nao::JID_LLEG_4, -25.0, 40);  CalculateVel(Nao::JID_RLEG_4, -24.0, 40);
	CalculateVel(Nao::JID_LLEG_5, 60.0, 40);  CalculateVel(Nao::JID_RLEG_5, 59.0, 40);
    	CalculateVel(Nao::JID_LLEG_6, -20.0, 40);  CalculateVel(Nao::JID_RLEG_6, 22.0, 40);

}
void NaoGetUp::getUp3()
{	/**&head1;=-2,&head2;=-45,&larm1;=-12,&larm2;= 20,&larm3;=-71,&larm4;=-16,&rarm1;=-12,&rarm2;=-10,&rarm3;= 78,&rarm4;= 12,&lleg1;= -15,&lleg2;=-2,&lleg3;= 90,&lleg4;=-88,&lleg5;= 70,&lleg6;=-12,&rleg1;= -15,&rleg2;= 0,&rleg3;= 86,&rleg4;=-86,&rleg5;= 70,&rleg6;=12;*/
	CalculateVel(Nao::JID_LARM_1, -12.0, 40);  CalculateVel(Nao::JID_RARM_1, -12.0, 40);
    	CalculateVel(Nao::JID_LARM_2, 20.0, 40);    CalculateVel(Nao::JID_RARM_2, -10.0, 40);
   	CalculateVel(Nao::JID_LARM_3, -71.0, 40);  CalculateVel(Nao::JID_RARM_3, 78.0, 40);
    	CalculateVel(Nao::JID_LARM_4, -16.0, 40);  CalculateVel(Nao::JID_RARM_4, 12.0, 40);
	CalculateVel(Nao::JID_LLEG_1, -15.0, 20);  CalculateVel(Nao::JID_RLEG_1, -15.0, 40);
    	CalculateVel(Nao::JID_LLEG_2, -2.0, 40);    CalculateVel(Nao::JID_RLEG_2, -0.0, 40);
   	CalculateVel(Nao::JID_LLEG_3, 90.0, 40);  CalculateVel(Nao::JID_RLEG_3, 86.0, 40);
    	CalculateVel(Nao::JID_LLEG_4, -88.0, 40);  CalculateVel(Nao::JID_RLEG_4, -86.0, 40);
	CalculateVel(Nao::JID_LLEG_5, 70.0, 40);  CalculateVel(Nao::JID_RLEG_5, 70.0, 40);
    	CalculateVel(Nao::JID_LLEG_6, -12.0, 40);  CalculateVel(Nao::JID_RLEG_6, 12.0, 40);

}

void NaoGetUp::getUp4()
{	/**&wait;= 0.5;
	&head1;=-0,&head2;=-0,&larm1;=-14,&larm2;= 0,&larm3;=-71,&larm4;= 0,&rarm1;=-21,&rarm2;=-1,&rarm3;= 78,&rarm4;= 0,&lleg1;= -70,&lleg2;= 23,&lleg3;= 90,&lleg4;=-130,&lleg5;= 35,&lleg6;=-40,&rleg1;=-70,&rleg2;=-30,&rleg3;= 90,&rleg4;=-130,&rleg5;= 34,&rleg6;=45;*/
	CalculateVel(Nao::JID_LARM_1, -14.0, 40);  CalculateVel(Nao::JID_RARM_1, -21.0, 40);
    	CalculateVel(Nao::JID_LARM_2, 0.0, 40);    CalculateVel(Nao::JID_RARM_2, -1.0, 40);
   	CalculateVel(Nao::JID_LARM_3, -71.0, 40);  CalculateVel(Nao::JID_RARM_3, 78.0, 40);
    	CalculateVel(Nao::JID_LARM_4, 0.0, 40);  CalculateVel(Nao::JID_RARM_4, 0.0, 40);
	CalculateVel(Nao::JID_LLEG_1, -70.0, 40);  CalculateVel(Nao::JID_RLEG_1, -70.0, 40);
    	CalculateVel(Nao::JID_LLEG_2, 23.0, 40);    CalculateVel(Nao::JID_RLEG_2, -30.0, 40);
   	CalculateVel(Nao::JID_LLEG_3, 90.0, 40);  CalculateVel(Nao::JID_RLEG_3, 90.0, 40);
    	CalculateVel(Nao::JID_LLEG_4, -130.0, 40);  CalculateVel(Nao::JID_RLEG_4, -130.0, 40);
	CalculateVel(Nao::JID_LLEG_5, 35.0, 40);  CalculateVel(Nao::JID_RLEG_5, 34.0, 40);
    	CalculateVel(Nao::JID_LLEG_6, -40.0, 40);  CalculateVel(Nao::JID_RLEG_6, 45.0, 40);

}

void NaoGetUp::getUp5()
{	/**&wait;= 0.1;
	&head1;=0,&head2;=0,&larm1;=-37,&larm2;= 0,&larm3;=-71,&larm4;= 0,&rarm1;=-37,&rarm2;=-1,&rarm3;= 78,&rarm4;= 0,&lleg1;= -56,&lleg2;=-25,&lleg3;= 90,&lleg4;=-120,&lleg5;= 18,&lleg6;= 7,&rleg1;= -56,&rleg2;=-45,&rleg3;= 90,&rleg4;=-130,&rleg5;= 41,&rleg6;=45;*/
	CalculateVel(Nao::JID_LARM_1, -37.0, 40);  CalculateVel(Nao::JID_RARM_1, -37.0, 40);
    	CalculateVel(Nao::JID_LARM_2, 0.0, 40);    CalculateVel(Nao::JID_RARM_2, -1.0, 40);
   	CalculateVel(Nao::JID_LARM_3, -71.0, 40);  CalculateVel(Nao::JID_RARM_3, 78.0, 40);
    	CalculateVel(Nao::JID_LARM_4, 0.0, 40);  CalculateVel(Nao::JID_RARM_4, 0.0, 40);
	CalculateVel(Nao::JID_LLEG_1, -56.0, 40);  CalculateVel(Nao::JID_RLEG_1, -56.0, 40);
    	CalculateVel(Nao::JID_LLEG_2, -25.0, 40);    CalculateVel(Nao::JID_RLEG_2, -45.0, 40);
   	CalculateVel(Nao::JID_LLEG_3, 90.0, 40);  CalculateVel(Nao::JID_RLEG_3, 90.0, 40);
    	CalculateVel(Nao::JID_LLEG_4, -120.0, 40);  CalculateVel(Nao::JID_RLEG_4, -130.0, 40);
	CalculateVel(Nao::JID_LLEG_5, 18.0, 40);  CalculateVel(Nao::JID_RLEG_5, 41.0, 40);
    	CalculateVel(Nao::JID_LLEG_6, 7.0, 40);  CalculateVel(Nao::JID_RLEG_6, 45.0, 40);

}

void NaoGetUp::getUp6()
{	/**&head1;= 0,&head2;= 0,&larm1;= 0,&larm2;= 0,&larm3;= 0,&larm4;= 0,&rarm1;= 0,&rarm2;= 0,&rarm3;= 0,&rarm4;= 0,&lleg1;=-70,&lleg2;= 3,&lleg3;= 90,&lleg4;=-110,&lleg5;= 0,&lleg6;= 0,&rleg1;=-70,&rleg2;= 0,&rleg3;= 90,&rleg4;=-100,&rleg5;= 0,&rleg6;= 0;*/
	CalculateVel(Nao::JID_LARM_1, 0.0, 40);  CalculateVel(Nao::JID_RARM_1, 0.0, 40);
    	CalculateVel(Nao::JID_LARM_2, 0.0, 40);    CalculateVel(Nao::JID_RARM_2, 0.0, 40);
   	CalculateVel(Nao::JID_LARM_3, 0.0, 40);  CalculateVel(Nao::JID_RARM_3, 0.0, 40);
    	CalculateVel(Nao::JID_LARM_4, 0.0, 40);  CalculateVel(Nao::JID_RARM_4, 0.0, 40);
	CalculateVel(Nao::JID_LLEG_1, -70.0, 40);  CalculateVel(Nao::JID_RLEG_1, -70.0, 40);
    	CalculateVel(Nao::JID_LLEG_2, 3.0, 40);    CalculateVel(Nao::JID_RLEG_2, -0.0, 40);
   	CalculateVel(Nao::JID_LLEG_3, 90.0, 40);  CalculateVel(Nao::JID_RLEG_3, 90.0, 40);
    	CalculateVel(Nao::JID_LLEG_4, -110.0, 40);  CalculateVel(Nao::JID_RLEG_4, -100.0, 40);
	CalculateVel(Nao::JID_LLEG_5, 0.0, 40);  CalculateVel(Nao::JID_RLEG_5, 0.0, 40);
    	CalculateVel(Nao::JID_LLEG_6, 0.0, 40);  CalculateVel(Nao::JID_RLEG_6, 0.0, 40);

}

void NaoGetUp::getUp7()
{	/**&wait;= 0;
	&head1;=0,&head2;=0,&larm1;= 0,&larm2;= 0,&larm3;=0,&larm4;= 0,&rarm1;=0,&rarm2;=0,&rarm3;= 0,&rarm4;= 0,&lleg1;= 0,&lleg2;=0,&lleg3;= 0,&lleg4;= 0,&lleg5;= 0,&lleg6;= 0,&rleg1;= 0,&rleg2;=0,&rleg3;= 0,&rleg4;= 0,&rleg5;= 0,&rleg6;= 0;
	&wait;= 0.5;*/
	CalculateVel(Nao::JID_LARM_1, 0.0, 40);  CalculateVel(Nao::JID_RARM_1, 0.0, 40);
    	CalculateVel(Nao::JID_LARM_2, 0.0, 40);  CalculateVel(Nao::JID_RARM_2, 0.0, 40);
   	CalculateVel(Nao::JID_LARM_3, 0.0, 40);  CalculateVel(Nao::JID_RARM_3, 0.0, 40);
    	CalculateVel(Nao::JID_LARM_4, 0.0, 40);  CalculateVel(Nao::JID_RARM_4, 0.0, 40);
	CalculateVel(Nao::JID_LLEG_1, 0.0, 40);  CalculateVel(Nao::JID_RLEG_1, 0.0, 40);
    	CalculateVel(Nao::JID_LLEG_2, 0.0, 40);  CalculateVel(Nao::JID_RLEG_2, 0.0, 40);
   	CalculateVel(Nao::JID_LLEG_3, 0.0, 40);  CalculateVel(Nao::JID_RLEG_3, 0.0, 40);
    	CalculateVel(Nao::JID_LLEG_4, 0.0, 40);  CalculateVel(Nao::JID_RLEG_4, 0.0, 40);
	CalculateVel(Nao::JID_LLEG_5, 0.0, 40);  CalculateVel(Nao::JID_RLEG_5, 0.0, 40);
    	CalculateVel(Nao::JID_LLEG_6, 0.0, 40);  CalculateVel(Nao::JID_RLEG_6, 0.0, 40);

}