[SOLVED] Connecting sofa to an external data COM port

[Madani]

Hello Everyone

I am designing a virtual reality environment using sofa.I have built my customized robot and want to feed the motor encoders data through arduino microprocessor to SOFA via COM port.I think there should be a way to do this. I guess I should modify the C++ source codes which are related to position update of SOFA environment but I don’t know how! I have also looked through example codes for on-the-shelf robotic systems but it seems they are plug and play kind of devices. I would appreciate if you can help me with this. By the way I am a mechanical engineer so please don’t use jargon or think you are talking to a computer engineer!explain it in simple language

Thanks,

[Alex Bilger]

Hi,

that’s a very technical question. I hope you will get answers.
You will probably have to do this:
– create a plugin in sofa
– link the plugin with a library which allows you to get data from the Arduino
– write a controller to get the position from the Arduino and update a mechanical object position

If you succeed, please post some instructions if somebody else want to do the same thing.

Thanks,

[Madani]

I have managed to make my own plugin (actually it is not my own it is the plugin example of SOFA that I modified) and can control the position of the mechanical object by assigning increments to each coordination axis of my scene. The issue is that I want to connect it to my own encoders.I have the code which can read the serial port in VS c++ 11 and it works as a separate solution but when I plug it into the MyBehaviourModel in plugin example it seems that SOFA does not let me to allocate a handle to my COM port and give the error of INVALID_HANDLE_VALUE- ERROR_FILE_NOT_FOUND.do you have any idea where the problem might lie? I mean is there any security policy designed in SOFA for not letting me access the COM port?

[Madani]

I managed to step even further I just gave it a try and opened RunSofa in administration mode and opned my file in administrative mode and the problem changed.Now I get the error that MyBehaviorModel creation failed. I have no clue where the problem is originated from.any help would be appreciated.

[Froy]

Hello,

I guess you are speaking about debug mode ?
Anyway, did you check that your plugin is well loaded ? Otherwise, the fact that it cannot create your MyBehaviorModel object depends of your code but here are some hints :
– you did not load your plugin (either with the GUI or with the in the scene;
– there is a mistake with templates (if applicable)
– mistake on the spelling of your component in the scene file
– canCreate() static function in the MyBehaviorModel code is returning false

And about your COM error, I don’t really have any clue but the “ERROR_FILE_NOT_FOUND” may indicate that there is just a problem of path somewhere.

[Madani]

Hello Froy
thank you so much for your time and attention!
You are right I forgot to include pluginexample.dll in the RunSofa.exe file that was why it gave me the error I fixed it and now plugin loads.however the problem still seems to be the handle to COM port. the irritating thing is that the same code works out of SOFA but it does not inside MyBehaviorModel.cpp!!

[Madani]

The last step has come finally!

I managed to connect it to the COM port in visual studio.I have plugged my code into the MyBehaviorModel::updatePosition. The problem is that sofa just reads the port values once when simulation starts and does not iterate the procedure on every loop. The annoying thing is that if I assign an incremental value to (for example dz as dz-=0.1) it works fine but it just does not work with COM port code.Any idea about the problem?

[Froy]

Hello,

[it has been 3 months so this problem might have solved ?]

updatePosition() as you guessed, is called at every timestep.
So I would say that the problem may come from the COM port code part.
Do you mind to post the portion of the code relative to the COM port within updatePosition() ?

[Madani]

sorry for the long delay but I was working on another part of SOFA and now I am back to serial communication part.

Here is the Updates position() function:

m_hFile=OpenComPort(3,CBR_57600);
m_hFile2=OpenComPort2(10,CBR_57600);

if(m_hFile!=NULL)
{//read 100 lines from serial port
    for (int i = 0; i < 300; i++) {
        char ch = 0;
        DWORD read = 0;
        while(!read) {
            ReadFile(m_hFile, &ch, 1, &read, NULL);
            if (read) {

                if (ch==1 | ch==0 ){
                    hibyte=ch;
                }

                else{
                    lowbyte=ch;
                }

                pos=(255*hibyte)+lowbyte;
                ydummy=(double)(pos);
                y=(-ydummy/800)-1;
                if(y>-0.90){
                    y=-0.95;
                }

            }
        }
    }
}
CloseHandle(m_hFile);
//==================================================================

if(m_hFile2!=NULL)
{//read 100 lines from serial port
    for (int j = 0; j < 300; j++) {
        char ch2 = 0;
        DWORD read2 = 0;
        while(!read2) {
            ReadFile(m_hFile2, &ch2, 1, &read2, NULL);
            if (read2) {

                if (ch2==1 | ch2==2){
                    hibyte2=ch2;
                }

                else{
                    lowbyte2=ch2;
                }

                pos2=((255*hibyte2)+lowbyte2)-450;

                xdummy=(double)(pos2);
                x=((xdummy)/200);
            }
        }
    }
}
CloseHandle(m_hFile2);

//*********************************************************************
dy=0.00;
using core::behavior::MechanicalState;
mState1 = dynamic_cast<MechanicalState<sofa::defaulttype::Rigid3dTypes> *>(this->getContext()->getMechanicalState());
helper::WriteAccessor<sofa::core::objectmodel:: Data<sofa::defaulttype::Rigid3dTypes::VecCoord> > xp = *mState1->write(core::VecCoordId::position());
xp[0].getCenter()=sofa::defaulttype::Vec<3,Real>((Real)((x)),(Real)dy,(Real)((y)));

As you can see I have to open and close the serial port each time I run a graphical loop which is the worst optimized option and is causing delay in my simulation.

Thank you beforehand,

Mehdi

[Hugo]

Dear Madani,

I am not sure to understand what is then your question here?
If this is about how to improve the computational efficiency, you could deal with your port in a separate thread.

Cheers,

Hugo

[Madani]

The problem is that when I place OpenComPort() in MyBehaviorModel() the program compiles and it opens the port but it crashes. To say it more clearly, my question is that “Is there any part of PluginExample where I can place OpenComPort() that initiates it once and there won’t be any need to open and close port on every simulation loop?

[Madani]

again me! the usual annoying question person 🙂

I managed to solve that problem now I just open the COM port just once by putting it in the MyBehaviorModel(). Now the problem is that simulation runs normally(fast) but after like half a second it becomes slow. Here is my PlugIn code:

#include "MyBehaviorModel.h"
#include <sofa/core/ObjectFactory.h>
#include <sofa/core/behavior/MechanicalState.h>
#include <sofa/core/VecId.h>
#include <sofa/defaulttype/VecTypes.h>
#include <SofaUserInteraction/MechanicalStateController.h>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/core/behavior/MechanicalState.h>
#include <sofa/core/objectmodel/MouseEvent.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/defaulttype/RigidTypes.h>
#include <sofa/defaulttype/Quat.h>
#include <sofa/simulation/common/Node.h>
#include <sofa/simulation/common/MechanicalVisitor.h>
#include <sofa/simulation/common/UpdateMappingVisitor.h>
#include <sofa/defaulttype/RigidTypes.h>
#include <sofa/defaulttype/VecTypes.h>
#include <stdio.h>
#include <tchar.h>
#include <string>
#include<cstdlib>
#include<iostream>
#include <time.h>
#include <tinyxml.h>
#include <math.h>
#include "NIDAQmx.h"

#if defined(__linux__) || defined(__APPLE__)
#define __CFUNC
#define __CFUNC_C
#define __CFUNCPTRVAR
#define CVICDECL
#define CVICALLBACK     CVICDECL
#else
#define __CFUNC         __stdcall
#define __CFUNC_C       __cdecl
#define __CFUNCPTRVAR   __cdecl
#define CVICDECL        __cdecl
#define CVICALLBACK     CVICDECL
#endif

HANDLE m_hFile;
HANDLE m_hFile2;
// Current Values

unsigned char hibyte;
unsigned char lowbyte;
unsigned char hibyte2;
unsigned char lowbyte2;
long int pos;
long int pos2;
double x;
double y;
double ydummy;
double xdummy;
double prx=0;
double curx;
double avx=0;
long int count;
double sumprx;
double theta;

#define DAQmxErrChk(functionCall) { if( DAQmxFailed(error=(functionCall)) ) {  goto Error;} }

HANDLE OpenComPort(int nPortNo,int BaudRate,int ByteSize,int Parity,int StopBits)
{
	LPCSTR strPort="\\\\.\\COM3";

BOOL fSuccess;
COMMTIMEOUTS timeout;
DCB dcb;
//Create the handle for read the COM port
m_hFile = CreateFileA( strPort, GENERIC_READ,
(DWORD)NULL, // exclusive access
NULL, // no security
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
NULL // hTemplate
);
if (INVALID_HANDLE_VALUE == m_hFile)
{//Check whether the handle is valid or not.
printf("Error: Arduino Port Connection Not Established!");
return NULL;
}
SetupComm(m_hFile,(DWORD)2048, (DWORD)2048);// setup the com parameters now that we have a handle
// Set up the DCB with our settings
fSuccess = GetCommState(m_hFile,&dcb);// Get it first so we fill all members
if (!fSuccess)
{// Handle the error.
printf("Error: Failed to Get the System Communication Settings.");
return NULL;
}
dcb.BaudRate = BaudRate; // baud rate
dcb.ByteSize = ByteSize; // data size
dcb.Parity = Parity; // No Parity Bit
dcb.StopBits = StopBits; // stop bits
fSuccess = SetCommState(m_hFile, &dcb); // assign it
if ( fSuccess == 0 )
{//Now check the configuration of the communication device is valid or not after assign the com parameters
printf("Error: In Control Setting for a Serial Communications Device.");
return ("success!!!!");
}
// Set up the timeouts to use, they are quite short, since we will loop anyway.
// Do not make them zero, else we will have a CPU load problem. Too large a value,
// and we have to wait for comms to time out when shutting down.
GetCommTimeouts(m_hFile, &timeout); // fill timeout structure
timeout.ReadIntervalTimeout = 50; // 50ms between incomming chars.
timeout.ReadTotalTimeoutConstant = 50;
timeout.ReadTotalTimeoutMultiplier = 10;
timeout.WriteTotalTimeoutConstant = 50;
timeout.WriteTotalTimeoutMultiplier = 10; // 60ms per char sent
SetCommTimeouts(m_hFile, &timeout);
return m_hFile;
}

//===========================================================================

HANDLE OpenComPort2(int nPortNo,int BaudRate,int ByteSize,int Parity,int StopBits)
{
	LPCSTR strPort2="\\\\.\\COM10";

BOOL fSuccess;
COMMTIMEOUTS timeout;
DCB dcb;
//sprintf(strPort, "COM%d", nPortNo) ;
//Create the handle for read the COM port
m_hFile2 = CreateFileA( strPort2, GENERIC_READ,
(DWORD)NULL, // exclusive access
NULL, // no security
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
NULL // hTemplate
);
if (INVALID_HANDLE_VALUE == m_hFile2)
{//Check whether the handle is valid or not.
printf("Error: Prolific Port Connection Not Established!");
return NULL;
}
SetupComm(m_hFile2,(DWORD)2048, (DWORD)2048);// setup the com parameters now that we have a handle
// Set up the DCB with our settings
fSuccess = GetCommState(m_hFile2,&dcb);// Get it first so we fill all members
if (!fSuccess)
{// Handle the error.
printf("Error: Failed to Get the System Communication Settings.");
return NULL;
}
dcb.BaudRate = BaudRate; // baud rate
dcb.ByteSize = ByteSize; // data size
dcb.Parity = Parity; // No Parity Bit
dcb.StopBits = StopBits; // stop bits
fSuccess = SetCommState(m_hFile2, &dcb); // assign it
if ( fSuccess == 0 )
{//Now check the configuration of the communication device is valid or not after assign the com parameters
printf("Error: In Control Setting for a Serial Communications Device.");
return ("success!!!!");
}
// Set up the timeouts to use, they are quite short, since we will loop anyway.
// Do not make them zero, else we will have a CPU load problem. Too large a value,
// and we have to wait for comms to time out when shutting down.
GetCommTimeouts(m_hFile2, &timeout); // fill timeout structure
timeout.ReadIntervalTimeout = 50; // 50ms between incomming chars.
timeout.ReadTotalTimeoutConstant = 0;
timeout.ReadTotalTimeoutMultiplier = 0;
timeout.WriteTotalTimeoutConstant = 0;
timeout.WriteTotalTimeoutMultiplier = 60; // 60ms per char sent
SetCommTimeouts(m_hFile2, &timeout);
return m_hFile2;
}

//=========================================================================

 // Task parameters
    int32       error = 0;
    TaskHandle  taskHandle = 0;
    char        errBuff[2048]={'\0'};
    int32       i;

    // Channel parameters
    char        chan[] = "Dev1/ai2:3";
    float64     min = -10.0;
    float64     max = 10.0;

    // Timing parameters
    char        source[] = "OnboardClock";
    uInt64      samplesPerChan = 2;
    float64     sampleRate = 1000.0;

    // Data read parameters
    #define     bufferSize (uInt32)1000
    float64     data[bufferSize];
    int32       pointsToRead = -1;
    int32       pointsRead;
    float64     timeout = 10.0;

	double forcez;
	double tx;

  
//*********************************************************************

namespace sofa

{

namespace component
{

namespace behaviormodel
{

MyBehaviorModel::MyBehaviorModel():
    customUnsignedData(initData(&customUnsignedData, (unsigned)1,"Custom Unsigned Data","Example of unsigned data with custom widget")),
    regularUnsignedData(initData(&regularUnsignedData, (unsigned)1,"Unsigned Data","Example of unsigned data with standard widget"))
{HANDLE OpenComPort(int nPortNo=3,int BaudRate = CBR_57600,int ByteSize = 8,int Parity = NOPARITY,int StopBits
= ONESTOPBIT);
HANDLE OpenComPort2(int nPortNo=10,int BaudRate = CBR_57600,int ByteSize = 8,int Parity = NOPARITY,int StopBits
= ONESTOPBIT);
    customUnsignedData.setWidget("widget_myData");
	m_hFile=OpenComPort(3,CBR_57600,8,NOPARITY,ONESTOPBIT);
	m_hFile2=OpenComPort2(10,CBR_57600,8,NOPARITY,ONESTOPBIT);
}
	

MyBehaviorModel::~MyBehaviorModel()
{
}

void MyBehaviorModel::init()
{
	

}	

void MyBehaviorModel::reinit()
{
	
 
}

void MyBehaviorModel::updatePosition(SReal dt)
{ 
  
	

	HINSTANCE hinstLib = LoadLibraryA(_T("C:\\nicaiu.dll")); 

	
	
if(m_hFile!=NULL)
{//read 100 lines from serial port
	for (int i = 0; i < 300; i++) {
		 char ch = 0;
		DWORD read = 0;
		while(!read) {
		ReadFile(m_hFile, &ch, 1, &read, NULL);
		if (read) {

		
if (ch==1 | ch==0 ){
	hibyte=ch;
}

else{
	lowbyte=ch;
}

	pos=(255*hibyte)+lowbyte;
	ydummy=(double)(pos);
	y=(-ydummy/600)-1.05;
	if(y>-0.90){
		y=-1.05;
	}
	

		}
		}
		}
}
//CloseHandle(m_hFile);
//==================================================================

	if(m_hFile2!=NULL)
{//read 100 lines from serial port
	for (int j = 0; j < 300; j++) {
		 char ch2 = 0;
		DWORD read2 = 0;
		while(!read2) {
		ReadFile(m_hFile2, &ch2, 1, &read2, NULL);
		if (read2) {

		
if (ch2==1 | ch2==2){
	hibyte2=ch2;
}

else{
	lowbyte2=ch2;
}

	pos2=((255*hibyte2)+lowbyte2)-450;
	
	
	xdummy=(double)(pos2);
	x=((xdummy)/200);
	count++;
	avx=(x+sumprx)/count;
	if(abs(x)<abs((5*avx)) | abs(x)<0.8){	
		curx=x;
	}

	if(abs(x)>abs((5*avx)) ){
		curx=prx;
	}
	
	
		}
	}
	}
		}

//***************************************************************************

//CloseHandle(m_hFile2);
 /*DAQmxErrChk (DAQmxCreateTask ("", &taskHandle));
    DAQmxErrChk (DAQmxCreateAIVoltageChan (taskHandle, chan, "", DAQmx_Val_Cfg_Default, min, max, DAQmx_Val_Volts, NULL));
    DAQmxErrChk (DAQmxCfgSampClkTiming (taskHandle, source, sampleRate, DAQmx_Val_Rising, DAQmx_Val_FiniteSamps, samplesPerChan));

    DAQmxErrChk (DAQmxStartTask (taskHandle));

    DAQmxErrChk (DAQmxReadAnalogF64 (taskHandle, pointsToRead, timeout, 0, data, bufferSize, &pointsRead, NULL));

    printf ("Acquired %ld samples\n", pointsRead);
    
	// Just print out the first 10 points
   // for (i = 0; i < 16; ++i)
       
	
Error:
    if( DAQmxFailed(error) )
        DAQmxGetExtendedErrorInfo(errBuff,2048);
    if(taskHandle != 0) {
        DAQmxStopTask (taskHandle);
        DAQmxClearTask (taskHandle);
    }
    if( DAQmxFailed(error) )
		printf ("DAQmxBase Error %ld: %s\n", error, errBuff);
	*/
    
//*********************************************************************	
dy=0.00;
/*forcez=((double)data[1]-0.121);
forcez=forcez-1.05;
tx=((double)data[2])-0.325;
if(tx<0){
tx=tx*10;
//forcez=-forcez;
}
if(tx>0){
tx=tx*16;
//forcez=-forcez;
}
theta=((atan(tx/forcez))-1.54)/4;
printf ("data[%ld] = %f\n", 1, forcez);
printf ("data[%ld] = %f\n", 1, tx);*/
using core::behavior::MechanicalState;
mState1 = dynamic_cast<MechanicalState<sofa::defaulttype::Rigid3dTypes> *>(this->getContext()->getMechanicalState());
helper::WriteAccessor<sofa::core::objectmodel:: Data<sofa::defaulttype::Rigid3dTypes::VecCoord> > xp = *mState1->write(core::VecCoordId::position());
xp[0].getCenter()=sofa::defaulttype::Vec<3,Real>((Real)((x)),(Real)dy,(Real)((y)));

sumprx=sumprx+x;
prx=x;
}

SOFA_DECL_CLASS(MyBehaviorModel)

int MyBehaviorModelClass = core::RegisterObject("Dummy component with a custom widget.").add< MyBehaviorModel >();

} // namespace behaviormodel

} // namespace component

} // namespace sofa

Sorry for making you read this long code but just wanted to make sure you will see all the detail. by the way the commented parts are for the mode that I am using force input as my haptic device uses both position and force input.

Thank you again,

Mehdi

[dilthoms]

One possibility is that your simulation is not running fast enough. Therefore the data from the COM port gets buffered up and positions do not get updated as fast as you want.

[Hugo]

Hi Medhi and thank you @dilthoms for your comment.

No worries Medhi, questions are always welcome.
Maybe you could try to run you code in debug mode? The debug information might help. But I must say it is very hard to help on this remotely.

Cheers,

Hugo

[Madani]

Thank you @dilthoms and @Hugo for your answers

I solved the issue of crashing not the problem is that Sofa runs fast in the beginning of simulation (for about 1s) and then runs somehow in slowmotion. I think as well that the problem is from buffer. The thing is that interestingly when I change the refresh interval DT in SOFA GUI there is no change in my loop duration and no matter how I assign it it always takes 200 ms to finish one loop. I also tried modifying timeout intervals of my serial port but it did not give me any result as well. However when I run the simulation with NI force sensor everything runs smoothly and in real time manner.

thanks again for your time!

Mehdi,

[dilthoms]

From my understanding the time interval DT only specifies the resolution at which simulation is done. The time taken for your loop is not directly affected by DT. Someone from SOFA team can confirm this.

To reduce delay what you could try is slowing down the rate at which data is sent from arduino side so that it does not get buffered up on the receiving side.

[Hugo]

Hi Mehdi and Thomas,

As explained by Thomas, the time step dt defined in your root node monitors the time discretization used in the resolution of your system. The time required to solved the system at each time step just depends on the complexity of your problem (matrix size, conditioning ..). So I confirm your statement.

Good idea Thomas,
and good luck Medhi.

Cheers,

Hugo

[Hugo]

Hi Mehdi,

Do you need further assistance on this topic?
Otherwise, we will consider this thread as solved.

Cheers,

Hugo

[Madani]

Hi Hugo,

Thank you for your follow up I really appreciate how much effort SOFA support team puts in solving users’ issues.
Yes it is solved. Apparently the problem rose from my own code. I had a for loop which elongated processing time of SOFA, by decreasing the number of iterations and also adjusting the serial port (dcb) parameters the problem was not there anymore.

Regards,

Mehdi

[Hugo]

Thank you very much for your feedback Medhi.
As usual, if you face any future issue, do not hesitate to come back to the forum!

Cheers,

Hugo

[Author]

Posts