//Sample from Samples/Devices/RangeFinder #include "Modacpp.h" #include "conio.h" #define MODASERVER "localhost" #define WORLD "/" #define DEVICENAME "zone0/rangefinderdevice" using namespace ModaCPP; //********************************************************************************* void DisplayScan(Moda::Commons::RangeFinderScan *pScan) { int i; //display text _cprintf("T=%lu ",pScan->time); for(i=0;i<pScan->ValuesCount;i++) { _cprintf("%f, ",pScan->Values[i]); } _cprintf("\r\n"); //display directly on screen int dx=100,dy=100; HDC hdcecr=GetDC(NULL); HBRUSH hBrush=CreateSolidBrush(RGB(255,255,255)); HPEN hPen=CreatePen(PS_SOLID,1,RGB(0,0,0)); HBRUSH hSaveBrush; HPEN hSavePen; //save data hSaveBrush=(HBRUSH) SelectObject(hdcecr,hBrush); hSavePen=(HPEN) SelectObject(hdcecr,hPen); //paint data Rectangle(hdcecr,0,0,dx,dy); float measuredepth=float(dy-10)/pScan->RangeMax; float measurewidth=float(dx)/float(pScan->ValuesCount); MoveToEx(hdcecr,0,(int) (0+measuredepth*pScan->Values[0]),NULL); for(i=1;i<pScan->ValuesCount;i++) { LineTo(hdcecr,(int) (0+float(i)*measurewidth),(int) (0+measuredepth*pScan->Values[i])); } //restore data SelectObject(hdcecr,hSaveBrush); SelectObject(hdcecr,hSavePen); DeleteObject(hBrush); DeleteObject(hPen); ReleaseDC(NULL,hdcecr); } //********************************************************************************* void EnumWorkingModes(DeviceRangeFinder *pRangeFinder) { xkode::lib::Array<Moda::Commons::RangeFinderWorkingMode> _Modes; pRangeFinder->GetWorkingModes(_Modes); _cprintf("Enum Working modes:\r\n"); for(int i=0;i<_Modes.Count();i++) { Moda::Commons::RangeFinderWorkingMode &m=_Modes[i]; _cprintf("->%s\r\n",m.ToString().GetData()); } _cprintf("Enum end.\r\n"); } //********************************************************************************* int main(int argc, char* argv[]) { ModaCPP::Connection *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 ModaCPP::RobotPHX *pWorld=connection->QueryRobotPHX(WORLD); if(pWorld) { _cprintf("world found\r\n"); DeviceRangeFinder *pRangeFinder=pWorld->QueryDeviceRangeFinder(DEVICENAME); if(pRangeFinder) { _cprintf("device found\r\n"); //enum working modes EnumWorkingModes(pRangeFinder); //scan int time=pWorld->GetTime(); int workingmode=0; while(!_kbhit()) { xkode::lib::Array<MU8> _Scan; M32 DeviceResponse; M32 SystemResponse=pRangeFinder->GetScanWaitComplete(_Scan,&DeviceResponse); if(_Scan.Count()!=0) { //Data is valid if array is not empty Moda::Commons::RangeFinderScan *pScan=(Moda::Commons::RangeFinderScan *)_Scan.GetData(); DisplayScan(pScan); } else { _cprintf("error while getting scan data (system:%d device:%d)\r\n",SystemResponse,DeviceResponse); } //change working mode every 4s if(pWorld->GetTime()-time>4000) { if(workingmode==0) { workingmode=1; pRangeFinder->ChooseWorkingMode(workingmode); } else { workingmode=0; pRangeFinder->ChooseWorkingMode(workingmode); } time=pWorld->GetTime(); } } delete pRangeFinder; } else { _cprintf("device %s not found\r\n",DEVICENAME); } delete pWorld; } else { _cprintf("world not found\r\n"); } } else { _cprintf("Unable to connect to moda server\r\n"); } //Disconnect & delete connection->Disconnect(); delete connection; _getch(); return 0; }