int HaptionDriver::initDevice(char* ip)
{
cout<<"HaptionDriver::initDevice() called"<<endl;
connection_device = 0;
/*m_indexingMode = INDEXING_ALL_FORCE_FEEDBACK_INHIBITION;*/
m_indexingMode = INDEXING_ALL;
m_speedFactor = 1.0;
haptic_time_step = 0.003f;
//haptic_time_step = 0.5f;
myData.m_virtContext = NULL;
cout<<"tentative de connection sur: "<<ip<<endl;
myData.m_virtContext = virtOpen (ip);
if (myData.m_virtContext == NULL)
{
cout<<"erreur connection"<<endl;
return 0;
}
else
cout<<"connection OK"<<endl;
virtSetIndexingMode(myData.m_virtContext, m_indexingMode);
cout<<"virtSetSpeedFactor return "<<virtSetSpeedFactor(myData.m_virtContext, m_speedFactor)<<endl;
float speddFactor[1];
virtGetSpeedFactor(myData.m_virtContext, speddFactor);
cout<<"virtGetSpeedFactor return "<<speddFactor[0]<<endl;
virtSetTimeStep(myData.m_virtContext,haptic_time_step);
cout<<"set base frame ok"<<endl;
m_typeCommand = COMMAND_TYPE_IMPEDANCE;
m_forceFactor = 1.0f;
virtSetCommandType(myData.m_virtContext, m_typeCommand);
virtSetForceFactor(myData.m_virtContext, m_forceFactor);
virtSetPowerOn(myData.m_virtContext, 1);
cout<<"init callback"<<endl;
virtSetPeriodicFunction(myData.m_virtContext, haptic_callback, &haptic_time_step, &myData);
cout<<"callback initialise"<<endl;
virtSaturateTorque(myData.m_virtContext, 15.0f,0.7f);
cout<<posBase.getValue()[0].getCenter()<<" "<<posBase.getValue()[0].getOrientation()<<endl;
float baseFrame[7] = { (float) posBase.getValue()[0].getCenter().x()/(float) scale.getValue(),
(float) posBase.getValue()[0].getCenter().y()/(float) scale.getValue(),
(float) posBase.getValue()[0].getCenter().z()/(float) scale.getValue(),
(float) posBase.getValue()[0].getOrientation()[0],
(float) posBase.getValue()[0].getOrientation()[1],
(float) posBase.getValue()[0].getOrientation()[2],
(float) posBase.getValue()[0].getOrientation()[3]
};
cout<<"virtSetBaseFrame return "<<virtSetBaseFrame(myData.m_virtContext, baseFrame)<<endl;
cout<<"virtGetErrorCode return "<<virtGetErrorCode(myData.m_virtContext)<<endl;
return 1;
}
/*!
* Register the periodic function. This function could be started using
* startLoop() and stopped using stopLoop().
* \param CallBackVirt :
* \param period :
* \param virtuose :
*/
void vpVirtuose::setPeriodicFunction(VirtPeriodicFunction CallBackVirt, float &period, vpVirtuose &virtuose)
{
init();
if (virtSetPeriodicFunction(m_virtContext, CallBackVirt, &period, &virtuose)) {
int err = virtGetErrorCode(m_virtContext);
throw(vpException(vpException::fatalError,
"Error calling virtSetPeriodicFunction: error code %d", err));
}
}