////////////////////////////////////////////////////// // Sample from Samples/Devices/AbsoluteCompass/C++ /////////////////////////////////////////////////////// #include "Modacpp.h" #include "MyScene.h" #include "conio.h" #define MODASERVER "localhost" using namespace ModaCPP; using namespace xkode::lib; //------------------------------------------------------------------------------- //global vars ModaCPP::Connection *connection; DeviceMotor *pLeft; DeviceMotor *pRight; DeviceAbsolutCompass *pCompass; DeviceLightSource *pLightN; DeviceLightSource *pLightS; DeviceLightSource *pLightW; DeviceLightSource *pLightE; //------------------------------------------------------------------------------- float CalculateError(float currentangle,float togodeg) { currentangle=MathFunctions::Mod_MPI_PI(currentangle); float error=currentangle-MathFunctions::ToRad(togodeg); return( xkode::lib::Math::fmod_MPI_PI_FLOAT(error)); } //------------------------------------------------------------------------------- void RegulateAngle(float togodeg,DeviceLightSource *pLight) { float angle,error; pLight->SetIntensity(0.7f); while(true) { angle=pCompass->GetAngleRAD(); error=CalculateError(angle,togodeg); _cprintf("togo: %f currentangle: %f, error:%f\r\n",togodeg,MathFunctions::ToDeg(angle),MathFunctions::ToDeg(error)); pLeft->SetVelocityRPS(2.0f*error); pRight->SetVelocityRPS(-2.0f*error); connection->Sleep(20); if( fabs(error)<=xkode::lib::Math::ToRad(5.0f)) { pLight->SetIntensity(1.0f); pLeft->SetVelocityRPS(0); pRight->SetVelocityRPS(0); connection->Sleep(2000); break; } } pLight->SetIntensity(0.0f); } //------------------------------------------------------------------------------- int main(int argc, char* argv[]) { connection=new ModaCPP::Connection(true); //Try connect to MODA server if(connection->Connect(MODASERVER)) { _cprintf("Connection ok to moda server\r\n"); //Find the robot MyScene *pScene=connection->QueryRobotPHX2<MyScene>("/"); if(pScene && pScene->IsValid() ) { _cprintf("scene ok\r\n"); pLeft=pScene->GetMotorByIndex(MyScene_MOTOR01_INDEX); pRight=pScene->GetMotorByIndex(MyScene_MOTOR00_INDEX); pCompass=pScene->GetAbsolutCompassByIndex(MyScene_ABSOLUTCOMPASS00_INDEX); pLightN=pScene->GetLightSourceByPath("lightN/rlight0/l"); pLightS=pScene->GetLightSourceByPath("lightS/rlight0/l"); pLightW=pScene->GetLightSourceByPath("lightW/rlight0/l"); pLightE=pScene->GetLightSourceByPath("lightE/rlight0/l"); //sitch off lights pLightN->SetIntensity(0); pLightS->SetIntensity(0); pLightW->SetIntensity(0); pLightE->SetIntensity(0); //force devices on state pCompass->On(); pRight->On(); pLeft->On(); connection->Sleep(100); while(!_kbhit()) { //rotate in north direction RegulateAngle(0,pLightN); //rotate in north west RegulateAngle(90,pLightW); //rotate in north south RegulateAngle(180,pLightS); //rotate in north east RegulateAngle(270,pLightE); } delete pScene; } else { _cprintf("robot not found\r\n"); } } else { _cprintf("Unable to connect to moda server\r\n"); } //Disconnect & delete connection->Disconnect(); delete connection; _getch(); return 0; }