hfutengine.zip_HFUTEngine_robotcup

/*************************************************************************** * Copyright (C) 2007 by hfut * * * * HFUT_Engine3D * * Created by Alex.Bao, ShiGuoQiang * * Other Members: YuanWeiFa, GuoWeiJie, YuanXing, XingRong * * Maintained by AI.Lab Robocup Group,CS,HFUT * * He Fei University of Technology, An Hui, China * * * ***************************************************************************/ #include "acthandle.h" #include <sstream> #include <string.h> #include "connection.h" #include "worldmodel.h" #include "timer.h" #include "skill.h" extern Skill skill; extern Connection connection; extern WorldModel wm; Timer timer; ActHandle::ActHandle() { #ifdef NAO_MODEL HJControl[JID_HEAD_1].sName = "he1"; HJControl[JID_HEAD_2].sName = "he2"; HJControl[JID_LLEG_1].sName = "lle1"; HJControl[JID_LLEG_2].sName = "lle2"; HJControl[JID_LLEG_3].sName = "lle3"; HJControl[JID_LLEG_4].sName = "lle4"; HJControl[JID_LLEG_5].sName = "lle5"; HJControl[JID_LLEG_6].sName = "lle6"; HJControl[JID_RLEG_1].sName = "rle1"; HJControl[JID_RLEG_2].sName = "rle2"; HJControl[JID_RLEG_3].sName = "rle3"; HJControl[JID_RLEG_4].sName = "rle4"; HJControl[JID_RLEG_5].sName = "rle5"; HJControl[JID_RLEG_6].sName = "rle6"; HJControl[JID_LARM_1].sName = "lae1"; HJControl[JID_LARM_2].sName = "lae2"; HJControl[JID_LARM_3].sName = "lae3"; HJControl[JID_LARM_4].sName = "lae4"; HJControl[JID_RARM_1].sName = "rae1"; HJControl[JID_RARM_2].sName = "rae2"; HJControl[JID_RARM_3].sName = "rae3"; HJControl[JID_RARM_4].sName = "rae4"; #else HJControl[0].sName = "lle1"; HJControl[1].sName = "rle1"; HJControl[2].sName = "lle4"; HJControl[3].sName = "rle4"; HJControl[4].sName = "lae3"; HJControl[5].sName = "rae3"; HJControl[6].sName = "lae4"; HJControl[7].sName = "rae4"; UJControl[0].sName = "lle2_3"; UJControl[1].sName = "rle2_3"; UJControl[2].sName = "lle5_6"; UJControl[3].sName = "rle5_6"; UJControl[4].sName = "lae1_2"; UJControl[5].sName = "rae1_2"; #endif } ActHandle::~ActHandle() { } void ActHandle::Beam(Vector vPos) { stringstream ss(""); ss<<"(beam "<<vPos.Vx<<" "<<vPos.Vy<<" "<<vPos.Vz<<")"; connection.SendMsg(ss.str()); } bool ActHandle::ActHandle::Beam(Vector vPos, int BeamNum, bool Clear) { static int iBeamedNum = 0; if (Clear) { BeamNum = 0; return true; } cout<<"BEAMBEAMBEAMBEAMBEAMBEAMBEAMBEAMBEAMBEAMBEAMBEAM"<<endl; if (iBeamedNum < BeamNum) { Beam(vPos); iBeamedNum++; if (iBeamedNum < BeamNum) return false; } iBeamedNum = 0; wm.UpdateFallState(1); ClearAll(); skill.ClearAll(); return true; } bool ActHandle::BeamAfterDelay(Vector vPos, int DelayTime, int BeamNum, bool Clear) { static double dTime = 0; if (Clear) { dTime = 0; return true; } if (fabs(dTime) < 0.001) { dTime = wm.GetServerTime() + 0.01 * DelayTime; } if (wm.GetServerTime() < dTime) return false; if (!Beam(vPos, BeamNum)) return false; dTime = 0; return true; } bool ActHandle::AdjustHJ(int iID, double dNewAng, double dSpeed, double dAccurate) { double dCurAng = wm.m_HJ[iID].fAngle; if (fabs(dCurAng - dNewAng) > dAccurate) { HJControl[iID].bCtrled = true; HJControl[iID].fAngle = dSpeed*(dNewAng - dCurAng); return false; } return true; } #ifndef NAO_MODEL bool ActHandle::AdjustUJ_1(int iID, double dNewAng, double dSpeed, double dAccurate) { double dCurAng = wm.m_UJ[iID].fAngle1; if (fabs(dCurAng - dNewAng) > dAccurate) { UJControl[iID].bCtrled = true; UJControl[iID].fAngle1 = dSpeed*(dNewAng - dCurAng); return false; } return true; } bool ActHandle::AdjustUJ_2(int iID, double dNewAng, double dSpeed, double dAccurate) { double dCurAng = wm.m_UJ[iID].fAngle2; if (fabs(dCurAng - dNewAng) > dAccurate) { UJControl[iID].bCtrled = true; UJControl[iID].fAngle2 = dSpeed*(dNewAng - dCurAng); return false; } return true; } #endif void ActHandle::ClearControl() { int iUJNum = 0, iHJNum = 0; #ifdef NAO_MODEL iHJNum = 22; #else iHJNum = 8; iUJNum = 6; #endif int i = 0; for (; i < iHJNum; i++) { HJControl[i].fAngle = 0.0; HJControl[i].bCtrled = false; } #ifndef NAO_MODEL for (i = 0; i < iUJNum; i++) { UJControl[i].fAngle1 = 0.0; UJControl[i].fAngle2 = 0.0; UJControl[i].bCtrled = false; } #endif } string ActHandle::MakeCommand() { int iUJNum = 0, iHJNum = 0; #ifdef NAO_MODEL iHJNum = 22; #else iHJNum = 8; iUJNum = 6; #endif stringstream ss(""); int i = 0; for(; i < iHJNum; i++) { if (!HJControl[i].bCtrled) { if (HJControl[i].bChanged) { ss<<"("<<HJControl[i].sName<<" 0)"; HJControl[i].bChanged = false; } } else { HJControl[i].bChanged = true; ss<<"("<<HJControl[i].sName<<" "<<HJControl[i].fAngle<<")"; } } #ifndef NAO_MODEL for(i = 0; i < iUJNum; i++) { if (!UJControl[i].bCtrled) { if (UJControl[i].bChanged) { ss<<"("<<UJControl[i].sName<<" 0 0)"; UJControl[i].bChanged = false; } } else { UJControl[i].bChanged = true; ss<<"("<<UJControl[i].sName<<" "<<UJControl[i].fAngle1<<" "<<UJControl[i].fAngle1<<")"; } } #endif if (wm.ShouldSay) { //cout<<" 我在说话 "<<endl; stringstream say(""); say << "(say "<<wm.GenerateSayInfomation()<<")"; ss<<say.str(); } return ss.str(); } /* bool ActHandle:: Squat() { if(!timer.IsTimerRun()) timer.SetTimer(); if(timer.GetTime()>0&& timer.GetTime()<100) { AdjustHJ(JID_HEAD_1,0,0.07); AdjustHJ(JID_HEAD_2,0,0.07);// �0�5�1�7�1�7�1�7�0�5�0�8�1�7�1�7�1�7�1�7�1�7 AdjustHJ( JID_LARM_1,-80,0.04); AdjustHJ( JID_RARM_1,-80,0.04); AdjustHJ( JID_LARM_2,0,0.1); AdjustHJ( JID_RARM_2,0,0.1); AdjustHJ( JID_LARM_3,-0,0.1); AdjustHJ( JID_RARM_3,0,0.1); AdjustHJ( JID_LARM_4,-0,0.1); AdjustHJ( JID_RARM_4,0,0.1);// �1�7�0�2�1�7�1�7�0�5�0�8�1�7�1�7�1�7�1�7�1�7 AdjustHJ(JID_LLEG_1,0,0.1); AdjustHJ(JID_RLEG_1,0,0.1); AdjustHJ(JID_LLEG_2,0,0.1); AdjustHJ(JID_RLEG_2,0,0.1); AdjustHJ(JID_LLEG_3,35,0.04); AdjustHJ(JID_RLEG_3,35,0.04); AdjustHJ(JID_LLEG_4,-70,0.04); AdjustHJ(JID_RLEG_4,-70,0.04); AdjustHJ(JID_LLEG_5,50,0.04); AdjustHJ(JID_RLEG_5,50,0.04); AdjustHJ(JID_LLEG_6,0,0.1); AdjustHJ(JID_RLEG_6,0,0.1);//�1�7�0�7�1�7�1�7�0�5�0�8�1�7�1�7�1�7�1�7�1�7 } if (timer.GetTime() < 100) return false; timer.ClearTimer(); return true; } */ bool ActHandle :: FallDownForTest(double speed,bool forward) { if(!timer.IsTimerRun()) timer.SetTimer(-50); if(timer.GetTime()>0&& timer.GetTime()<100) { AdjustHJ(JID_HEAD_1,0,0.07); AdjustHJ(JID_HEAD_2,0,0.07); AdjustHJ( JID_LARM_1,-80,0.37); AdjustHJ( JID_RARM_1,-80,0.37); AdjustHJ( JID_LARM_2, 0,0.37); AdjustHJ( JID_RARM_2,0,0.37); AdjustHJ( JID_LARM_3,-0,0.37); AdjustHJ( JID_RARM_3,