Serial Communication trouble

Hi,
I am working on an big spider robot. Right now the programm “Processing” does alle the processing :stuck_out_tongue: and sends angles to the arduino board where 24 Servos are attached.
Every kordinate ist konected to a letter and the arduino sketch uses switch case to write it to the specific Servo.
But the movement is far away from smooth. I think (hope) it has something to do with the bad way I send the information over.

has someone an idea how to speed this up or make it more kompact?
Is that an efective way?
Because I wasn’t able to use Firmata or something similar. (I don’t know how to adapt it, or don’t know if they would be better)

Thank you a lot!
Florian

This is the part of the Processing code where it sends the data out:

void SendLeg(int leg, float[] Deg){
  
  // Method to write to the arduino
  
    String[] Order = {"a","b","c","d","e","f","g","h","i","j","k","l","m","n","o","p","q","r","s","t","u","v","w","x"}; 
  
  port.write((int (degrees(Deg[0]))) + Order[3*leg]);
  port.write((int (degrees(Deg[1]))+90) + Order[(3*leg)+1]);
  port.write((int (degrees(Deg[2]))) + Order[(3*leg)+2]);   

}

and this is the Arduino code:

#include <Servo.h>

// initialize all servos.
Servo left1, left2, left3, left4, left5, left6, left7, left8, left9, left10, left11, left12;
Servo right1, right2, right3, right4, right5, right6, right7, right8, right9, right10, right11, right12;

// initalize servos in three arrays
Servo hip[8]   = {left1, left2, left3, left4, right1, right2, right3, right4};
Servo femur[8] = {left5, left6, left7, left8, right5, right6, right7, right8};
Servo tibia[8] = {left9, left10, left11, left12, right9, right10, right11, right12};




void setup() {

  Attach();
  initiate();

  Serial.begin(9600);
  Serial.println("Ready");

}

void loop() {

  static int v = 0;

  if ( Serial.available()) {
    char ch = Serial.read();

    switch(ch) {
    case '0'...'9':
      v = v * 10 + ch - '0';
      break;
//----------------------------------------------------------------------      
//----------------------------------------| leg 0 |---------------------         
    case 'a':
        
        hip[0].write(v);
      
      v = 0;
      break;
    
    case 'b':

        femur[0].write(180-v);
      
      v = 0;
      break;
    
    case 'c':

        tibia[0].write(v);

      v = 0;
      break;
//----------------------------------------------------------------------      
//----------------------------------------| leg 1 |---------------------           
    case 'd':

        hip[1].write(v);

      v = 0;
      break;
    
    case 'e':

        femur[1].write(180-v);

      v = 0;
      break;
    
    case 'f':

        tibia[1].write(v);

      v = 0;
      break;
//----------------------------------------------------------------------      
//----------------------------------------| leg 2 |---------------------           
    case 'g':

        hip[2].write(v);

      v = 0;
      break;
    
    case 'h':

        femur[2].write(180-v);

      v = 0;
      break;
    
    case 'i':

        tibia[2].write(v);

      v = 0;
      break;
//----------------------------------------------------------------------      
//----------------------------------------| leg 3 |---------------------           
    case 'j':

        hip[3].write(v);

      v = 0;
      break;
    
    case 'k':

        femur[3].write(180-v);

      v = 0;
      break;
    
    case 'l':

        tibia[3].write(v);

      v = 0;
      break;
//----------------------------------------------------------------------      
//----------------------------------------| leg 4 |---------------------           
    case 'm':

        hip[4].write(180-v);

      v = 0;
      break;
    
    case 'n':

        femur[4].write(v);
      
      v = 0;
      break;
    
    case 'o':

        tibia[4].write(180-v);
      
      v = 0;
      break;
//----------------------------------------------------------------------      
//----------------------------------------| leg 5 |---------------------           
    case 'p':

        hip[5].write(180-v);

      v = 0;
      break;
    
    case 'q':

        femur[5].write(v);
      
      v = 0;
      break;
    
    case 'r':

        tibia[5].write(180-v);

      v = 0;
      break;
//----------------------------------------------------------------------      
//----------------------------------------| leg 6 |---------------------          
    case 's':

        hip[6].write(180-v);

      v = 0;
      break;
    
    case 't':

        femur[6].write(v);

      v = 0;
      break;
    
    case 'u':

        tibia[6].write(180-v);

      v = 0;
      break;
//----------------------------------------------------------------------      
//----------------------------------------| leg 7 |---------------------           
    case 'v':

        hip[7].write(180-v);

      v = 0;
      break;
    
    case 'w':

        femur[7].write(v);
      
      v = 0;
      break;
    
    case 'x':

        tibia[7].write(180-v);

      v = 0;
      break;
      
//----------------------------------------------------------------------      

    
    }
  }

} 


// attach servos to pins: hip (0-7) | femur(8-13,22,23) | tibia(25-32)
void Attach(){

  for (int i = 0; i < 8; i++){
    hip[i].attach(i+2);
  }

  for (int i = 0; i < 2; i++){
    femur[i].attach(i+10);
    femur[i+2].attach(i+12);
    femur[i+4].attach(i+22);
    femur[i+6].attach(i+24);
  }

  for (int i = 0; i < 4; i++){
    tibia[i].attach(i+27);
    tibia[i+4].attach(i+32);
  }
}


void initiate(){
  
  for (int i = 0; i< 4;i++){
    hip[i].write(90);
    hip[i+4].write(180-90);
  }
  for (int i = 0; i < 4;i++){
    femur[i].write(180-170);
    femur[i+4].write(170);
  }
  for (int i = 0; i< 4;i++){
    tibia[i].write(30);
    tibia[i+4].write(180-30);
  }
}

Unless you really need to use 9600, using a higher baud rate may help and it certainly wouldn't hurt.

  port.write((int (degrees(Deg[0]))) + Order[3*leg]);

We can't see what the degrees() function is doing.

  Serial.begin(9600);

This is a limiting factor. Processing and Arduino can talk much faster then this. Try a higher speed, like 115200.

    case '0'...'9':

Did this even compile?

Instead of 10 case statements, handle the digits separately, and handle every thing else in the switch statement.

Of course, since the range of value that the servos can move to is (presumably) limited to 0 to 180 degrees, and those values conveniently fit in a byte, you could save the time required to convert them to strings, send the string, and convert the string back, by sending the angle as a byte.

There is no change with 115200baud…

Did this even compile?

Instead of 10 case statements, handle the digits separately, and handle every thing else in the switch statement.

Of course, since the range of value that the servos can move to is (presumably) limited to 0 to 180 degrees, and those values conveniently fit in a byte, you could save the time required to convert them to strings, send the string, and convert the string back, by sending the angle as a byte.

I don’t know exactly what you mean. Could you give me an example?

example how the digits are handled separately;
(partial code not tested)

if ( Serial.available()) 
{
   char ch = Serial.read();
   if (ch -'0' < 10) v = v *10 + ch - '0';
   else 
   {
     switch(ch)
     { 
        case 'a' : hip[0].write(v); break;
        case 'b' : femur[0].write(180-v); break;
        ...
        case 'z' : dotheZ; break;
     }
     v = 0;
   }
}

example how byte codes can be used (Arduino site only)

if ( Serial.available() >=2)
{
   char ch = Serial.read();
   byte v = Serial.read();
   else
   {
     switch(ch)
     {
        case 'a' : hip[0].write(v); break;
        case 'b' : femur[0].write(180-v); break;
        ...
        case 'z' : dotheZ; break;
     }
   }
}

Note that this assumes that all chars / bytes are send and received correctly. In theory this will allways be so, but in practice ...

But the movement is far away from smooth. I think (hope) it has something to do with the bad way I send the information over.

It could be that the servos are under powered. Disconnect all servos except for one leg and run the code. Does that one leg now operate more smoothly?

   if (ch -'0' < 10) v = v *10 + ch - '0';

Rob, you really should be checking both ends of the range. Think about what happens when a space is part of the serial data (intentional or not), or a carriage return or line feed. These are all less than ‘0’, so ch - ‘0’ could go negative. That would not be a good thing for the calculation of v.

-Robtillaart: I like your code It seems to work the same and is much nicer to look at. (but no change in the way it behaves)

-zoomkat: I am absolutly sure that this is no power problem.

-PaulS: Is that a big problem? if yes, is there an easy way to solve it?

I figured out that this is no Arduino problem..... I turned off the Serialsend function in my Processing sketch and everything works fine and smooth. As soon I enable the function is starts to get chunky.. I am afraid I need to move over to the Processing forum.

Rob, you really should be checking both ends of the range.

Paul, you got me :wink:

(just tried) Arduino does support isdigit() so the test could become:

if (isdigit(ch)) v*= 10 + ch -'0';

@gonras

-Robtillaart:
I like your code

That’s the power of proper identation. but beware, one can write complete bogus code that has a nice layout. but it will easier to detect - see my mistake above.

Is that a big problem?

Not for me. I don’t know if it’s a problem for you.

is there an easy way to solve it?

Very easy.

   if (ch >= '0' && ch <= '9')
    v = v *10 + ch - '0';

I figured out that this is no Arduino problem…
I turned off the Serialsend function in my Processing sketch and everything works fine and smooth.

Since everything to be done depends on serial input, this doesn’t make sense.

There have been several suggestions on how to improve the speed, but you haven’t implemented, or commented on, them.

This isn’t strictly a Processing issue. Moving the topic to the Processing forum probably won’t get you any better answers than you are getting here.

I changed everything I understood into the way you suggsted.(there is no change in the behavior):

#include <Servo.h>

// initialize all servos.
Servo left1, left2, left3, left4, left5, left6, left7, left8, left9, left10, left11, left12;
Servo right1, right2, right3, right4, right5, right6, right7, right8, right9, right10, right11, right12;

// initalize servos in three arrays
Servo hip[8]   = {left1, left2, left3, left4, right1, right2, right3, right4};
Servo femur[8] = {left5, left6, left7, left8, right5, right6, right7, right8};
Servo tibia[8] = {left9, left10, left11, left12, right9, right10, right11, right12};




void setup() {

  Attach();
  initiate();

  Serial.begin(115200);
  Serial.println("Ready");

}

void loop() {

  static int v = 0;

  if ( Serial.available() >= 2) {
    char ch = Serial.read();

if ( ch >= '0' && ch <= '9') {v = v *10 + ch - '0';}
   else
   {
     switch(ch)
     {
      case 'a': hip[0].write(v);       break;
      case 'b': femur[0].write(180-v); break;    
      case 'c': tibia[0].write(v);     break;
      case 'd': hip[1].write(v);       break;
      case 'e': femur[1].write(180-v); break;    
      case 'f': tibia[1].write(v);     break;
      case 'g': hip[2].write(v);       break;
      case 'h': femur[2].write(180-v); break;
      case 'i': tibia[2].write(v);     break;
      case 'j': hip[3].write(v);       break; 
      case 'k': femur[3].write(180-v); break;
      case 'l': tibia[3].write(v);     break;
      
//----------------|other side|-----------------------------------    

      case 'm': hip[4].write(180-v);   break;
      case 'n': femur[4].write(v);     break;    
      case 'o': tibia[4].write(180-v); break;
      case 'p': hip[5].write(180-v);   break;
      case 'q': femur[5].write(v);     break;
      case 'r': tibia[5].write(180-v); break;      
      case 's': hip[6].write(180-v);   break;
      case 't': femur[6].write(v);     break;
      case 'u': tibia[6].write(180-v); break;  
      case 'v': hip[7].write(180-v);   break;
      case 'w': femur[7].write(v);     break;
      case 'x': tibia[7].write(180-v); break;
     }
     v = 0;
   }
  }
}


// attach servos to pins: hip (0-7) | femur(8-13,22,23) | tibia(25-32)
void Attach(){

  for (int i = 0; i < 8; i++){
    hip[i].attach(i+2);
  }

  for (int i = 0; i < 2; i++){
    femur[i].attach(i+10);
    femur[i+2].attach(i+12);
    femur[i+4].attach(i+22);
    femur[i+6].attach(i+24);
  }

  for (int i = 0; i < 4; i++){
    tibia[i].attach(i+27);
    tibia[i+4].attach(i+32);
  }
}


void initiate(){
  
  for (int i = 0; i< 4;i++){
    hip[i].write(90);
    hip[i+4].write(180-90);
  }
  for (int i = 0; i < 4;i++){
    femur[i].write(180-170);
    femur[i+4].write(170);
  }
  for (int i = 0; i< 4;i++){
    tibia[i].write(30);
    tibia[i+4].write(180-30);
  }
}

It does look much nicer now :slight_smile:

I meant that “the mistake is on the Processing side” because of the fact that it seems to have something to do with the Serial sending section and not with the reciving. the Processing program is getting chunky as soon I enable this section:

void SendLeg(int leg, float[] Deg){
  
  // Method to write to the arduino
  
    String[] Order = {"a","b","c","d","e","f","g","h","i","j","k","l","m","n","o","p","q","r","s","t","u","v","w","x"}; 
  
  port.write((int (degrees(Deg[0]))) + Order[3*leg]);
  port.write((int (degrees(Deg[1]))+90) + Order[(3*leg)+1]);
  port.write((int (degrees(Deg[2]))) + Order[(3*leg)+2]);   

}

It doesn’t matter if the Arduino itself is conected. It is either way chunky…

That section does send the radiants(which are changed into degrees by the degress() function " 180 == degrees(3.14) ") over serial to the Arduino. the " Deg " array does store the three coordinates fpr each leg.
After that the letter is attached so that the Arduino knows what servo the angle belongs to.

If some of you have Processing you might want to see what I mean exactly.

I marked the function and you just need to comment or uncomment to see the diffrence:

SendLeg(leg, Deg);                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// slows down the programm immensly!!!!!!!!!!!!!!!!!!

The Processing code doesn’t fit into one post so I split it in half…
The Serial funktion is in the second part.

import processing.opengl.*;
import processing.serial.*;        

Serial port;


float hx;
float hy;
float hz;

float ex;
float ey;
float ez;




float Bxz;
float bxz;
float Bleg;
float B;                                    //B: length of imaginary line
float q1;                                   //q1: angle between X-axis and imaginary line
float q2;                                   //q2: interior angle between imaginary line and link l1
float Qh;                                   //Qh; angle of of the hip on the ground plain
float Q1rad;
float Q1deg;                                //Q1: angle between x-axis and "l1"
float Q2rad;                                   //Q2: angle between "l1" and "l2"
float Q2deg;
static long l1 = 130;                       //l1: length of the upper leg in mm
static long l2 = 180;                          //l2: length of the lower leg in mm
static long hL = 60;                        // length between inner hip and outer hip
float Xpos;                                  //Xpos: relative X-position out of Xhip and Xfoot where the leg should move to
float Ypos;                                  //Ypos: relative Y-position out of Yhip and Yfoot where the leg should move to      
float Xfoot;
float Yfoot;
float Zfoot;
float Xhip;                                  //Xpos: relative X-position out of Xhip and Xfoot where the leg should move to
float Yhip;  

int Stepp = 5;         // beide sind für die automatische begegung zuständig
int Koord = 0;

 
  
float steppLength = 50;
float steppHight = 50;

void setup(){
size(screen.width, screen.height);

  println(Serial.list()); // List COM-ports
  port = new Serial(this, Serial.list()[0], 115200); 
}

void draw(){

  
  
  background(255);


Zfoot = mouseX-(width/2); 

  if (Zfoot <= -150){Zfoot = -150;}
  if (Zfoot >=  150){Zfoot =  150;}

Xfoot =  120;



/*____________________________________________________________________________

  Koord = Koord + Stepp;                             // einfache schleife
  if (Koord == 20 || Koord == -90) {
     Stepp = -Stepp ; 
  }
Yfoot = Koord ; 

/____________________________________________________________________________________________
*/
Yfoot = (height/2)-mouseY; //(90*sqrt((150*150)-(Zfoot*Zfoot))/150);

  if (Yfoot <= -100){Yfoot = -100;}
  if (Yfoot >=  20){Yfoot =  20;}


  KoordinatenSysteme();
  

    
  DrawLeg(0,0,60,Xfoot,Yfoot,Zfoot);
  DrawLeg(1,0,60,Xfoot,Yfoot,Zfoot);
  DrawLeg(2,0,60,Xfoot,Yfoot,Zfoot);
  DrawLeg(3,0,60,Xfoot,Yfoot,Zfoot);
  DrawLeg(4,0,60,Xfoot,Yfoot,Zfoot);
  DrawLeg(5,0,60,Xfoot,Yfoot,Zfoot);
  DrawLeg(6,0,60,Xfoot,Yfoot,Zfoot);
  DrawLeg(7,0,60,Xfoot,Yfoot,Zfoot);






 

}




//---------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------|  Begin XZ koordinatesystem        |------------------------------------


void KoordinatenSysteme(){


   
   stroke(0,0,255,30); 
   strokeWeight(4);



int gapH = height/100;
int gapW = width/100;

 int[] Height = {height*1/4,height*2/4,height*3/4,height*4/4};
int[] Width  = {width*1/4,width*2/4,width*3/4,width*4/4};

//koordinaten system nullpunkten
PVector Zero11 = new PVector(Width[0]/2,Height[0]);
PVector Zero12 = new PVector(Width[1]+((Width[0]-Width[1])/2),Height[0]);
PVector Zero21 = new PVector(Width[2]+((Width[0]-Width[1])/2),Height[0]);
PVector Zero22 = new PVector(Width[3]+((Width[0]-Width[1])/2),Height[0]);
PVector Zero31 = new PVector(Width[0]/2,Height[1]);
PVector Zero32 = new PVector(Width[1]+((Width[0]-Width[1])/2),Height[1]);
PVector Zero41 = new PVector(Width[2]+((Width[0]-Width[1])/2),Height[1]);
PVector Zero42 = new PVector(Width[3]+((Width[0]-Width[1])/2),Height[1]);
PVector Zero51 = new PVector(Width[0]/2,Height[2]);
PVector Zero52 = new PVector(Width[1]+((Width[0]-Width[1])/2),Height[2]);
PVector Zero61 = new PVector(Width[2]+((Width[0]-Width[1])/2),Height[2]);
PVector Zero62 = new PVector(Width[3]+((Width[0]-Width[1])/2),Height[2]);
PVector Zero71 = new PVector(Width[0]/2,Height[3]);
PVector Zero72 = new PVector(Width[1]+((Width[0]-Width[1])/2),Height[3]);
PVector Zero81 = new PVector(Width[2]+((Width[0]-Width[1])/2),Height[3]);
PVector Zero82 = new PVector(Width[3]+((Width[0]-Width[1])/2),Height[3]);

//Quadranten einteilung
  
  line(0,Height[0],width,Height[0]);  //horizontal
  line(0,Height[1],width,Height[1]);  //horizontal
  line(0,Height[2],width,Height[2]);  //horizontal
 
  line(Width[0],0,Width[0],height);  //vertical
  line(Width[1],0,Width[1],height);  //vertical
  line(Width[2],0,Width[2],height);  //vertical

  


  //Koordinatensysteme

   stroke(0); 
   strokeWeight(2);
   
   //Horizontal
  
  line(0+gapW,Height[0],Width[0]-gapW,Height[0]);
  line(Width[0]+gapW,Height[0],Width[1]-gapW,Height[0]);
  line(Width[1]+gapW,Height[0],Width[2]-gapW,Height[0]);
  line(Width[2]+gapW,Height[0],Width[3]-gapW,Height[0]);
  
  line(0+gapW,Height[1],Width[0]-gapW,Height[1]);
  line(Width[0]+gapW,Height[1],Width[1]-gapW,Height[1]);
  line(Width[1]+gapW,Height[1],Width[2]-gapW,Height[1]);
  line(Width[2]+gapW,Height[1],Width[3]-gapW,Height[1]);
  
  line(0+gapW,Height[2],Width[0]-gapW,Height[2]);
  line(Width[0]+gapW,Height[2],Width[1]-gapW,Height[2]);
  line(Width[1]+gapW,Height[2],Width[2]-gapW,Height[2]);
  line(Width[2]+gapW,Height[2],Width[3]-gapW,Height[2]);
  
    line(0+gapW,Height[3],Width[0]-gapW,Height[3]);
  line(Width[0]+gapW,Height[3],Width[1]-gapW,Height[3]);
  line(Width[1]+gapW,Height[3],Width[2]-gapW,Height[3]);
  line(Width[2]+gapW,Height[3],Width[3]-gapW,Height[3]);
    
    
    //Vertikal
    
    line(Width[0]/2,0+gapH,Zero11.x,Zero11.y);
    line(Width[0]/2,Height[0]+gapH,Zero31.x,Zero31.y);
    line(Width[0]/2,Height[1]+gapH,Zero51.x,Zero51.y);
    line(Width[0]/2,Height[2]+gapH,Zero71.x,Zero71.y);
    
    line(Width[1]+((Width[0]-Width[1])/2),0+gapH,Zero12.x,Zero12.y);
    line(Width[1]+((Width[0]-Width[1])/2),Height[0]+gapH,Zero32.x,Zero32.y);
    line(Width[1]+((Width[0]-Width[1])/2),Height[1]+gapH,Zero52.x,Zero52.y);
    line(Width[1]+((Width[0]-Width[1])/2),Height[2]+gapH,Zero72.x,Zero72.y);
     
    line(Width[2]+((Width[0]-Width[1])/2),0+gapH,Zero21.x,Zero21.y);
    line(Width[2]+((Width[0]-Width[1])/2),Height[0]+gapH,Zero41.x,Zero41.y);
    line(Width[2]+((Width[0]-Width[1])/2),Height[1]+gapH,Zero61.x,Zero61.y);
    line(Width[2]+((Width[0]-Width[1])/2),Height[2]+gapH,Zero81.x,Zero81.y);
   
    line(Width[3]+((Width[0]-Width[1])/2),0+gapH,Zero22.x,Zero22.y);
    line(Width[3]+((Width[0]-Width[1])/2),Height[0]+gapH,Zero42.x,Zero42.y);
    line(Width[3]+((Width[0]-Width[1])/2),Height[1]+gapH,Zero62.x,Zero62.y);
    line(Width[3]+((Width[0]-Width[1])/2),Height[2]+gapH,Zero82.x,Zero82.y);






}




float[] IK(float Xhip, float Yhip, float Xfoot, float Yfoot, float Zfoot){
  


  
  
  Xpos = abs(Xhip-Xfoot);                                 //relative distance of Xhip and Xfoot
  Ypos = abs(Yhip-Yfoot);                                 //relative distance of Yhip and Yfoot
    


  Bleg    = sqrt((Xfoot*Xfoot)+(Zfoot*Zfoot));   //total leg lenght including hL
  Qh      = HALF_PI-asin(Zfoot/Bleg);     // calculation of angle

  

  Bxz    = Bleg-hL;   //leg length without HL
  B      = sqrt(Bxz*Bxz + Ypos*Ypos);  // lenght between X-,Y-hip and X-,Y-foot
  bxz    = cos(Q1rad)*l1;

  q1 = atan2(Ypos,Xpos);
  q2 = acos((l1*l1 - l2*l2 + B*B)/(2*l1*B));              //the law of cosines and converted from radiants into degrees        
  Q1rad = q2 - q1;                                     
  Q1deg = degrees(Q1rad); 
  Q2rad = (acos((l1*l1 + l2*l2 - B*B)/(2*l1*l2)));    //the law of cosines and converted from radiants into degrees   
  Q2deg = degrees(Q2rad);


  hx = sin(Qh)*hL;                                // XYZ position of the outer hip joint
  hy = Yhip;
  hz = cos(Qh)*hL; 
 
  ex = (cos(Q1rad)*l1) + hx;                        //XYZ positions of the knee joint  /////////////falsch falsch falsch  keine ahnung was da falsch ist....
  ey = (sin(Q1rad)*l1) + Yhip;
  ez = sin(Qh)*(bxz+hL);     // falsch
 
   float[] DegAndKoor = {Qh,Q1rad,Q2rad,hx,hy,hz,ex,ey,ez,Xfoot,Yfoot,Zfoot,Xpos,Ypos,Xhip,Yhip};  //stores the Values in array

   return DegAndKoor;                                                                        // puts out the array
  
//---------------------------------------------------------------------------------------------------------------------------------
}

Second part:

void DrawLeg(int leg, float Xhip, float Yhip, float Xfoot, float Yfoot, float Zfoot){





  
//koordinaten system nullpunkten
int[] Height   = {height*1/4,height*2/4,height*3/4,height*4/4};
int[] Width    = {width*1/4,width*2/4,width*3/4,width*4/4};


PVector Zero11 = new PVector(Width[0]/2,Height[0]);
PVector Zero12 = new PVector(Width[1]+((Width[0]-Width[1])/2),Height[0]);
PVector Zero21 = new PVector(Width[2]+((Width[0]-Width[1])/2),Height[0]);
PVector Zero22 = new PVector(Width[3]+((Width[0]-Width[1])/2),Height[0]);
PVector Zero31 = new PVector(Width[0]/2,Height[1]);
PVector Zero32 = new PVector(Width[1]+((Width[0]-Width[1])/2),Height[1]);
PVector Zero41 = new PVector(Width[2]+((Width[0]-Width[1])/2),Height[1]);
PVector Zero42 = new PVector(Width[3]+((Width[0]-Width[1])/2),Height[1]);
PVector Zero51 = new PVector(Width[0]/2,Height[2]);
PVector Zero52 = new PVector(Width[1]+((Width[0]-Width[1])/2),Height[2]);
PVector Zero61 = new PVector(Width[2]+((Width[0]-Width[1])/2),Height[2]);
PVector Zero62 = new PVector(Width[3]+((Width[0]-Width[1])/2),Height[2]);
PVector Zero71 = new PVector(Width[0]/2,Height[3]);
PVector Zero72 = new PVector(Width[1]+((Width[0]-Width[1])/2),Height[3]);
PVector Zero81 = new PVector(Width[2]+((Width[0]-Width[1])/2),Height[3]);
PVector Zero82 = new PVector(Width[3]+((Width[0]-Width[1])/2),Height[3]);
  

float[] ZeroX ={Zero11.x,Zero12.x,Zero21.x,Zero22.x,Zero31.x,Zero32.x,Zero41.x,Zero42.x,Zero51.x,Zero52.x,Zero61.x,Zero62.x,Zero71.x,Zero72.x,Zero81.x,Zero82.x,};
float[] ZeroY ={Zero11.y,Zero12.y,Zero21.y,Zero22.y,Zero31.y,Zero32.y,Zero41.y,Zero42.y,Zero51.y,Zero52.y,Zero61.y,Zero62.y,Zero71.y,Zero72.y,Zero81.y,Zero82.y,};

  
  float[] DegAndKoor = IK(Xhip, Yhip, Xfoot, Yfoot, Zfoot);     //copies the IK array into the new array

  float[] Deg   = {DegAndKoor[0],DegAndKoor[1],DegAndKoor[2]};   //copies array into smaller sections for better overview
  PVector h     = new PVector(DegAndKoor[3],DegAndKoor[4],DegAndKoor[5]);
  PVector e     = new PVector(DegAndKoor[6],DegAndKoor[7],DegAndKoor[8]);
  PVector foot  = new PVector(DegAndKoor[9],DegAndKoor[10],DegAndKoor[11]);
  PVector pos   = new PVector(DegAndKoor[12],DegAndKoor[13]);
  PVector hip   = new PVector(DegAndKoor[14],DegAndKoor[15]);
  


SendLeg(leg, Deg);                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// slows down the programm immensly!!!!!!!!!!!!!!!!!!


//---------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------|          ZX Axis              |----------------------------------------
//--------------------------------------------------------|          drawing leg          |----------------------------------------

strokeWeight(1);

fill(240,0,0,200);
ellipse(ZeroX[leg],ZeroY[leg]-hip.x,8,8);            //Hip joint
fill(100,0,100,200);
line(ZeroX[leg],ZeroY[leg],ZeroX[leg]+h.z,ZeroY[leg]-h.x);     //from the hip joint to the outer hip
fill(0,100,100,200);
ellipse(ZeroX[leg]+h.z,ZeroY[leg]-h.x,15,15);        //outer hip joint       
stroke(2);
line(ZeroX[leg]+h.z,ZeroY[leg]-h.x,ZeroX[leg]+foot.z,ZeroY[leg]-foot.x);   // from the outer hip joint to the foot


fill(240,0,0,200);
ellipse(ZeroX[leg]+foot.z,ZeroY[leg]-foot.x,8,8);




//--------------------------------------------------------|             end drawing leg             |------------------------------
//---------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------|          drawing Reference dots         |------------------------------

fill(100,0,200,100);

ellipse(ZeroX[leg],ZeroY[leg]-foot.x,10,10);  // Xfoot-axis
text("    Xfoot:  " + foot.x, ZeroX[leg], ZeroY[leg]-foot.x+5);

ellipse(ZeroX[leg]+foot.z,ZeroY[leg],10,10);
text(" Zfoot:  " + foot.z, ZeroX[leg]+foot.z, ZeroY[leg]+20);

//--------------------------------------------------------|        end drawing Reference dots        |-----------------------------
//---------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------|          ZY Axis                         |-----------------------------


strokeWeight(1);

fill(240,0,0,200);
ellipse(ZeroX[leg+8]+foot.z,ZeroY[leg+8]-foot.y,8,8);

//---------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------|          drawing Reference dots         |------------------------------

fill(100,0,200,100);

ellipse(ZeroX[leg+8],ZeroY[leg+8]-foot.y,10,10);  // Xfoot-axis
text("    Yfoot:  " + foot.y, ZeroX[leg+8], ZeroY[leg+8]-foot.y+5);

ellipse(ZeroX[leg+8]+foot.z,ZeroY[leg+8],10,10);
text(" Zfoot:  " + foot.z, ZeroX[leg+8]+foot.z, ZeroY[leg+8]+20);



//--------------------------------------------------------|        end drawing Reference dots        |-----------------------------
//---------------------------------------------------------------------------------------------------------------------------------

// naming each sector by leg name

fill(0);
text("Leg : " +(leg+1) ,ZeroX[leg]-(ZeroX[0]*6/7), ZeroY[leg]-(ZeroY[0]*6/7));
text("Leg : " +(leg+1) ,ZeroX[leg+8]-(ZeroX[0]*6/7), ZeroY[leg+8]-(ZeroY[0]*6/7));
text("Qh : " + degrees(Qh) ,ZeroX[leg]-(ZeroX[0]*6/7), ZeroY[leg]-(ZeroY[0]*5/7));
text("Q1 : " + int((degrees(Q1rad)+90)) ,ZeroX[leg]-(ZeroX[0]*6/7), ZeroY[leg]-(ZeroY[0]*4/7));
text("Q2 : " + int(degrees(Q2rad)) ,ZeroX[leg]-(ZeroX[0]*6/7), ZeroY[leg]-(ZeroY[0]*3/7));


}






void SendLeg(int leg, float[] Deg){
  
  // Method to write to the arduino
  
    String[] Order = {"a","b","c","d","e","f","g","h","i","j","k","l","m","n","o","p","q","r","s","t","u","v","w","x"}; 

  
  port.write((int (degrees(Deg[0]))) + Order[3*leg]);
  port.write((int (degrees(Deg[1]))+90) + Order[(3*leg)+1]);
  port.write((int (degrees(Deg[2]))) + Order[(3*leg)+2]);   

}

In the SendLeg function, the values in the Deg array are converted from radians to degrees. Then, the value in degrees is converted to a string. Then, the appropriate character from the Order array is appended. Finally, the data is sent to the serial port.

This happens three times.

In order to determine which part is causing the slow performance, you need to break each step up, and comment out parts of the code.

float degVal = degrees(Deg[0]); int degInt = (int)degVal; String degStg = nf(degInt, 3); port.write(degStg); port.write(Order[3*leg]);

If this shows that the most time is taken in the port.write statements, there is not much you can do about that. Serial data transmission takes time.

There are a number of things that you can do, though.

The degInt value is in the range 0 to 180, is it not? If so, store that as a byte, rather than an int, and send it as a byte, rather than a string. The strings are up to 3 bytes long. The value is a single byte.

Another thing you can do is to not send data that hasn't changed. Instead of sending all three values in each call to SendLeg, pass in two array. As a value is sent, store it in the second array, too. Before sending a value, make sure that it is different from the value sent last time.

These two steps can reduce the amount of data sent in each call to DrawLeg, and can greatly reduce the data sent on each pass through draw().

Thank you very much,
I will try my best to get it running

Florian

Okay I finally figured out what the real problem is and I really don’t like my find…
The really only thing that slows down my entire program is the “port.write()” function it self. (not that what’s inside it)
Even when I just send a single number like this:

  port.write(1);   //(int (degrees(Deg[0]))) + Order[3*leg]);
  port.write(1);   //(int (degrees(Deg[1]))+90) + Order[(3*leg)+1]);
  port.write(1);   //(int (degrees(Deg[2]))) + Order[(3*leg)+2]);

I deeply hope someone will come and say some thing like:
"hey dude! you can totaly do this without the “port.write()” function!!

HERE<<< is the perfect solution!!!11! "

someone ??
really big please!

Florian

Ps. there is no ‘here’ link :slight_smile:

I just tried to skip the port.write() every 2 loops , but that didn't make it better. Is the write function so big and complex ?

No, the write() function is not that big and complex. What it is doing is sending data to the serial port. That is not a fast operation.

So if I would want to send information that fast I can not use The Serial?! Is Wifi Faster? or Bluetouth? I would really like to use Xbee but that is serial... Do you maybe have a life saving idea for me? :(