#include "Modacpp.h"
#include "conio.h"

#define MODASERVER "localhost"
#define MYROBOTNAME    "/"

using namespace ModaCPP;

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 *robot=connection->QueryRobotPHX(MYROBOTNAME);
    if(robot)
        {
        _cprintf("robot found\r\n");
        DeviceForceSensor *pForceSensor=robot->QueryDeviceForceSensor("slider_force/a1/sensor");
        if(pForceSensor)
            {
            pForceSensor->On();
            while(!_kbhit())
                {
                //display the force sensor value
                float force=pForceSensor->GetForce();
                _cprintf("Current force: %f\r\n",force);
                connection->Sleep(50);
                }
            //clean
            delete pForceSensor;
            }
        else
            {
            _cprintf("unable to find force sensor\r\n");
            }

        }
    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;
}