What data type is sent from V-Rep to Arduino by serial port???

Typically: "How do I... ", "How can I... " questions
Post Reply
xescbdn
Posts: 11
Joined: 14 Jun 2018, 17:54

What data type is sent from V-Rep to Arduino by serial port???

Post by xescbdn » 28 Jun 2018, 19:27

Hello,

I have designed a Platform in Vrep. This platform is moved by 6 joints (simulatin 6 servos). Simulation runs well in Vrep.

Next step was to build a real platform an send joint positions (angles) of each ones from VRep to real platform managed by Arduino Uno using sim.serialSend. On arduino side, I made the code for data receiving and real servos running. Both codes does not report any error message separately; but once arduino is plugged to serial port and I start Vrep simulation "Child Script Abort" message is showed on Vrep and nothings happen on servos....

Have you got any clue or suggestion to fix this issue?

Here are the codes:

V-Rep

Code: Select all

function sysCall_init()
  
    --Retrieve handles:

    SP=sim.getObjectHandle('SP')
    Origen=sim.getObjectHandle('Origen')
    Sphere=sim.getObjectHandle('Sphere')
    ui=simGetUIHandle("SP_UI")
    Servo1=sim.getObjectHandle('Servo_A')
    Servo2=sim.getObjectHandle('Servo_B')
    Servo3=sim.getObjectHandle('Servo_C')
    Servo4=sim.getObjectHandle('Servo_D')
    Servo5=sim.getObjectHandle('Servo_E')
    Servo6=sim.getObjectHandle('Servo_F')

    simSetUIButtonLabel(ui,0,sim.getObjectName(SP)..' user interface   (units: mm / degree)') -- Set the UI title
    ik1=sim.getIkGroupHandle('IK_SP') --undamped
    ik2=sim.getIkGroupHandle('IK_SP_damped') --damped
    ikFailedReportHandle=-1

    --Prepare initial values
    desiredConf={0,0,0,0,0,0} -- when in IK mode
    currentConf={0,0,0,0,0,0} -- when in IK mode
    angle=0
    angle1=0
    angle2=0
    angle3=0
    angle4=0
    angle5=0
    angle6=0


    ikMinPos={-0.04,-0.04,-0.024} --min position xyz 
    ikRange={0.08,0.08,0.066} --xyz actuation interval

    --Initial position and orientation of the Sphere (RELATIVE to the defined Origin)

    InitialPos=sim.getObjectPosition(Sphere,Origen)
    InitialOri=sim.getObjectOrientation(Sphere,Origen)
    
    movementMode=0 -- 0=None, 1=IK through dialog

    --defining the serial port number

    --port=sim.getScriptSimulationParameter(sim.handle_self,'serialPortNumberWin')
    port=sim.getScriptSimulationParameter(sim.handle_self,'serialPortNumberMac')
    --portNumber=[[COM3]]  --Windows port definition
    portNumber=[[/dev/cu.usbmodemFD121]] --MacBook port definition
    baudrate=9600
    serial=sim.serialOpen(portNumber,baudrate)

    --some tools
    rad2deg=180/math.pi
    deg2rad=math.pi/180
    MPI=math.pi


end
function sysCall_cleanup() 
 
end 
function sysCall_actuation()

    buttonID=simGetUIEventButton(ui)

    if ((buttonID>=2001)and(buttonID<=2003)) then -- position only is handled here
        desiredConf[buttonID-2000]=ikMinPos[buttonID-2000]+ikRange[buttonID-2000]*simGetUISlider(ui,buttonID)/1000
        movementMode=1
    end
    if ((buttonID>=2004)and(buttonID<=2006)) then -- orientation only is handled here
        desiredConf[buttonID-2000]=(18*math.pi/180)*(-1+2*simGetUISlider(ui,buttonID)/1000)
        movementMode=1
    end

    if movementMode==1 then       -- We are in IK mode

        maxLinVariationAllowed=0.04
        maxAngVariationAllowed=(18*math.pi/180)
        deltaX={0,0,0,0,0,0}
        
        -- position:
        for i=1,3,1 do
            deltaX[i]=desiredConf[i]-currentConf[i]
            if (math.abs(deltaX[i])>maxLinVariationAllowed) then
                deltaX[i]=maxLinVariationAllowed*deltaX[i]/math.abs(deltaX[i]) -- we limit the variation to the maximum allowed
            end
        end
        -- orientation:
        for i=1,3,1 do
            deltaX[3+i]=desiredConf[3+i]-currentConf[3+i]
            
            -- Normalize delta to be between -18º and +18º:
            
            rnge=(2*18*math.pi/180)
            
            deltaX[3+i]=math.fmod(deltaX[3+i],rnge)
            if (deltaX[3+i]<-rnge*0.5) then
                deltaX[3+i]=deltaX[3+i]+rnge
            else
                if (deltaX[3+i]>rnge*0.5) then
                    deltaX[3+i]=deltaX[3+i]-rnge
                end
            end
            if (math.abs(deltaX[3+i])>maxAngVariationAllowed) then
                deltaX[3+i]=maxAngVariationAllowed*deltaX[3+i]/math.abs(deltaX[3+i]) -- we limit the variation to the maximum allowed
            end
        end
    
        for i=1,6,1 do
            currentConf[i]=currentConf[i]+deltaX[i]
        end
          
        pos={0,0,0}
        ori={0,0,0}
        for i=1,3,1 do
            pos[i]=InitialPos[i]+currentConf[i]
            ori[i]=InitialOri[i]+currentConf[3+i]
        end
        -- We set the desired position and orientation
       sim.setObjectPosition(Sphere,Origen,pos)
       sim.setObjectOrientation(Sphere,Origen,ori)
        
    
    end

    if (movementMode==1) then
        if (sim.handleIkGroup(ik1)==sim.ikresult_fail) then
            -- the position/orientation could not be reached.
            sim.handleIkGroup(ik2) -- Apply a damped resolution method
            if (ikFailedReportHandle==-1) then -- We display a IK failure report message
                ikFailedReportHandle=sim.displayDialog("IK failure report","IK solver failed.",sim.dlgstyle_message,false,"",nil,{1,0.7,0,0,0,0})
            end
        else
            if (ikFailedReportHandle>=0) then
                sim.endDialog(ikFailedReportHandle) -- We close any report message about IK failure
                ikFailedReportHandle=-1
            end
        end
        
    end

    -- Now update the user interface:
    -- First the text boxes:
    -- Linear:
    for i=1,3,1 do
        str=string.format("%.3f",currentConf[i]*1000)
        if (str=='-0.000') then
            str='0.000' -- avoid having the - sign appearing and disappearing when 0
        end
        simSetUIButtonLabel(ui,2010+i,str)
    end
    -- Angular:
    for i=1,3,1 do
        str=string.format("%.1f",currentConf[3+i]*180/math.pi)
        if (str=='-0.0') then
            str='0.0' -- avoid having the - sign appearing and disappearing when 0
        end
        simSetUIButtonLabel(ui,2013+i,str)
    end
    
    -- Now the sliders
    -- Linear:
    for i=1,3,1 do
        
        simSetUISlider(ui,2000+i,1000*(currentConf[i]-ikMinPos[i])/ikRange[i])
        
    -- Angular:
   
        simSetUISlider(ui,2004,1000*(currentConf[4]+18*math.pi/180)/(36*math.pi/180))
        simSetUISlider(ui,2005,1000*(currentConf[5]+18*math.pi/180)/(36*math.pi/180))
        simSetUISlider(ui,2006,1000*(currentConf[6]+18*math.pi/180)/(36*math.pi/180))
    end

    -- Read joint angle information

        angle1=string.format("%.0f,",sim.getJointPosition(Servo1)*180/math.pi) --angle in degrees
        angle2=string.format("%.0f,",sim.getJointPosition(Servo2)*180/math.pi) --angle in degrees
        angle3=string.format("%.0f,",sim.getJointPosition(Servo3)*180/math.pi) --angle in degrees
        angle4=string.format("%.0f,",sim.getJointPosition(Servo4)*180/math.pi) --angle in degrees
        angle5=string.format("%.0f,",sim.getJointPosition(Servo5)*180/math.pi) --angle in degrees
        angle6=string.format("%.0f",sim.getJointPosition(Servo6)*180/math.pi) --angle in degrees
        
 
        
   -- Auxiliary Window to show joint angles

        window=sim.auxiliaryConsoleOpen("Angle",10,1)
        angle=angle1..angle2..angle3..angle4..angle5..angle6.."*"
        sim.auxiliaryConsolePrint(window,angle)

    -- Send servo information to Arduino
    
    
        while (sim.getSimulationState()~=sim.simulation_advancing_abouttostop) do
    
            sim.serialSend(serial,angle)

        end

    sim.serialClose(serial)
end 
   
Arduino Code:

Code: Select all

#include <Servo.h>

// * is used as the data string delimiter
// , is used to delimit individual data

// Declaracion de variables

  String readString; //main captured String 
  
  Servo a; //servo A
  Servo b; //servo B
  Servo c; //servo C
  Servo d; //servo D
  Servo e; //servo E
  Servo f; //servo F
  
  String Angulo1;
  String Angulo2;
  String Angulo3;
  String Angulo4;
  String Angulo5;
  String Angulo6;  

  int ind1; // , locations
  int ind2;
  int ind3;
  int ind4;
  int ind5;
  int ind6;

  int Angle1;
  int Angle2;
  int Angle3;
  int Angle4;
  int Angle5;
  int Angle6;
   
  
void setup() {
  
  Serial.begin(9600); // Abro el puerto serie

  // Asignacion de cada servo a un pin de salida
  a.attach(3);
  b.attach(4);
  c.attach(5);
  d.attach(6);
  e.attach(7);
  f.attach(8);
}

void loop() {

  // Read serial input:
  
  if (Serial.available())  {
    char Vrep = Serial.read();  //gets one byte from serial buffer
    if (Vrep == '*') {
      
      //do stuff
      
      Serial.println();
      Serial.print("captured String is : "); 
      Serial.println(readString); //prints string to serial port out
      
      ind1 = readString.indexOf(',');                   //finds location of first ,
      Angulo1 = readString.substring(0, ind1);          //captures first data String
      ind2 = readString.indexOf(',', ind1+1 );          //finds location of second ,
      Angulo2 = readString.substring(ind1+1, ind2+1);   //captures second data String
      ind3 = readString.indexOf(',', ind2+1 );          //finds location of third 
      Angulo3 = readString.substring(ind2+1, ind3+1);   //captures third data String
      ind4 = readString.indexOf(',', ind3+1 );          //finds location of fourth 
      Angulo4 = readString.substring(ind3+1,ind4+1);    //captures fourth data String,
      ind4 = readString.indexOf(',', ind4+1 );          //finds location of fifth 
      Angulo4 = readString.substring(ind4+1,ind5+1);    //captures fifth data String
      ind4 = readString.indexOf(',', ind5+1 );          //finds location of sixth 
      Angulo4 = readString.substring(ind5+1);    //captures remain part of data after last " "

      
      Serial.print("Angulo1 = ");
      Serial.println(Angulo1); 
      Serial.print("Angulo2 = ");
      Serial.println(Angulo2);
      Serial.print("Angulo3 = ");
      Serial.println(Angulo3);
      Serial.print("Angulo4 = ");
      Serial.println(Angulo4);
      Serial.print("Angulo5 = ");
      Serial.println(Angulo5);
      Serial.print("Angulo6 = ");
      Serial.println(Angulo6);
      Serial.println();
      Serial.println();

      Angle1=Angulo1.toInt();
      Angle2=Angulo2.toInt();
      Angle3=Angulo3.toInt();
      Angle4=Angulo4.toInt();
      Angle5=Angulo5.toInt();
      Angle6=Angulo6.toInt();
      

      a.write(Angle1); //send angle data to servo
      b.write(Angle2);
      c.write(Angle3);
      d.write(Angle4);
      e.write(Angle5);
      f.write(Angle6);
      
      readString=""; //clears variable for new input
      Angulo1="";
      Angulo2="";
      Angulo3="";
      Angulo4="";
      Angulo5="";
      Angulo6="";

    }  
    else {     
      readString += Vrep; //makes the string readString
    }
  }
}  

I am in a hurry (sorry for that) so I would appreciate your quick feedback...

Thanks in advance.

Irving
Posts: 11
Joined: 24 Jun 2018, 19:55

Re: What data type is sent from V-Rep to Arduino by serial port???

Post by Irving » 29 Jun 2018, 15:54

You are closing the serial port in the wrong place.

Try taking sim.serialClose() out of sysCall_actuation() and put it in sysCall_cleanup() instead.

xescbdn
Posts: 11
Joined: 14 Jun 2018, 17:54

Re: What data type is sent from V-Rep to Arduino by serial port???

Post by xescbdn » 29 Jun 2018, 16:26

Hello;
Irving wrote:
29 Jun 2018, 15:54
You are closing the serial port in the wrong place.

Try taking sim.serialClose() out of sysCall_actuation() and put it in sysCall_cleanup() instead.
I try taking sim.serialClose() out of sysCall_actuation() and put it in sysCall_cleanup() instead but result is the same
"Child Script Abort Execution"

... What I have realised is that red message appears in scene area: "main script not called"... can this be related with communication issues?

xescbdn
Posts: 11
Joined: 14 Jun 2018, 17:54

Re: What data type is sent from V-Rep to Arduino by serial port???

Post by xescbdn » 29 Jun 2018, 18:22

I think I have already find a clue...issue is that Vrep is sending value (in my case revolute joint position=angle) in a continuous way...

Is there any way to sim.serialSend a value only once it changes...?

Irving
Posts: 11
Joined: 24 Jun 2018, 19:55

Re: What data type is sent from V-Rep to Arduino by serial port???

Post by Irving » 03 Jul 2018, 11:31

In your Arduino code you read a line of serial data then send it back out on the same serial port. Where does that end up? Where do you see that output?

I wonder, if that goes back to VREP, whether that's causing the problem. What happens if you comment out the three Serial.printxx lines in your Arduino code?


Further to your previous question, I guess VREP sends the serial data once per minor step, i.e. every 5mS on a 50mS dt. You can send once per major step I suppose by tracking the simulation time.

The other issue I see is, even if you reduce the rate by tracking simulation time, your baud rate is much too low; you need 115,200 or higher to get that data across in a reasonable time.

xescbdn
Posts: 11
Joined: 14 Jun 2018, 17:54

Re: What data type is sent from V-Rep to Arduino by serial port???

Post by xescbdn » 19 Jul 2018, 08:28

Irving wrote:
03 Jul 2018, 11:31
In your Arduino code you read a line of serial data then send it back out on the same serial port. Where does that end up? Where do you see that output? --> No, arduino is only reading. No data is sent back to V-Rep

Further to your previous question, I guess VREP sends the serial data once per minor step, i.e. every 5mS on a 50mS dt. You can send once per major step I suppose by tracking the simulation time.

The other issue I see is, even if you reduce the rate by tracking simulation time, your baud rate is much too low; you need 115,200 or higher to get that data across in a reasonable time
. --> to fix the issue I made some kind of loop on the code in order to send needed data once every time this data changes. When data does not changes it sends "STOP" and Arduino ignore "STOP" messsage


Irving
Posts: 11
Joined: 24 Jun 2018, 19:55

Re: What data type is sent from V-Rep to Arduino by serial port???

Post by Irving » 19 Jul 2018, 11:04

xescbdn wrote:
19 Jul 2018, 08:28
Irving wrote:
03 Jul 2018, 11:31
In your Arduino code you read a line of serial data then send it back out on the same serial port. Where does that end up? Where do you see that output? --> No, arduino is only reading. No data is sent back to V-Rep
I beg to differ... the three lines of Arduino code

Code: Select all

      
      //do stuff
      
      Serial.println();
      Serial.print("captured String is : "); 
      Serial.println(readString); //prints string to serial port out
appear to send data back on the same serial link as V-Rep is sending data to the Arduino. Even if you don't use the data V-Rep may be taking time to process it (and the Arduino certainly is).
xescbdn wrote:
19 Jul 2018, 08:28
Irving wrote:
03 Jul 2018, 11:31
Further to your previous question, I guess VREP sends the serial data once per minor step, i.e. every 5mS on a 50mS dt. You can send once per major step I suppose by tracking the simulation time.

The other issue I see is, even if you reduce the rate by tracking simulation time, your baud rate is much too low; you need 115,200 or higher to get that data across in a reasonable time
. --> to fix the issue I made some kind of loop on the code in order to send needed data once every time this data changes. When data does not changes it sends "STOP" and Arduino ignore "STOP" messsage
OK, that may work. Your code sends 25 characters of data - at 9600baud each character takes approx 1mS - so the string takes at least 25mS (or over 50mS if you're re-transmitting it as per above). Are you sure your data only changes no more than once every other macro simulation step? It wouldn't hurt to increase the data rate to at least 38400 to be safe.

Post Reply