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;
}
void HaptionDriver::closeDevice()
{
if (myData.m_virtContext)
{
virtSetPowerOn(myData.m_virtContext, 0);
//virtDetachVO(myData.m_virtContext);
virtClose(myData.m_virtContext);
myData.m_virtContext = 0;
}
}
/*!
* Set power On
* \param power : When set to 1 allows to turn on the motors. When 0 turns them off.
*/
void vpVirtuose::setPowerOn (const int &power)
{
init();
if (virtSetPowerOn(m_virtContext, power)) {
int err = virtGetErrorCode(m_virtContext);
throw(vpException(vpException::fatalError,
"Error calling virtSetPowerOn: error code %d", err));
}
}
