anyKode Marilou
|
Absolut compass sensors.
class DeviceAbsolutCompass : public Device;
ModaCpp.h
The DeviceAbsolutCompass class cannot be created directly by user : it must be obtained by QueryDeviceAbsolutCompass of the class RobotPHX.
The sensor gives the angle between sensor's axis and the absolute north pole direction.
The various parameters given to the functions can be interpreted and modified if necessary by the device in order to respect the constraints imposed in modeling.
////////////////////////////////////////////////////// // 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; }
Documentation v4.7 (18/01/2015), Copyright (c) 2015 anyKode. All rights reserved.
|
What do you think about this topic? Send feedback!
|