sending bytes over serial port problems

iggy_DE

hello i´m new here and i´m sorry for my englsich. i hope you understand my problem. i capture 3 angles with a kinect and i will send them to the sofware OpenCM which controlls the servos. my problem is that the different angles are not sending well. with one angle i can controll the right servo, but when i try it with 2 or more angles it doesn´t work, for that i have to uncomment the other two angles and of course the servos in the OpenCM code too. i hope that anyone could give me a advise how to fix the problem. the problem is in the region PROBLEM there are the 3 angles which have to be send thank you and i hope someone could help.

import SimpleOpenNI.*;//Einbinden von Libarys
SimpleOpenNI kinect;
import processing.serial.*;
Serial port;     //Wichtig für die Serielle Kommunikation

PVector SchulterEllenbogenHand =new PVector();

float magnitude;
float magnitude1;
float magnitude2;
float magnitude3;
float magnitude4;
float magnitude5;
float magnitude6;

void setup() 
{
  kinect = new SimpleOpenNI(this);//Speicherplatz für diesen Namen
  kinect.enableDepth();           //Tiefenmessung
  kinect.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);//ganzes Skelet Tracking
  kinect.setMirror(true);//Setzt das Bild in ein reales Bild
  println(Serial.list());
  String portName = Serial.list()[0];
  size(640, 480);
  stroke(255, 255, 0);//Farbe Linie
  strokeWeight(5);//Breit Linie
  textSize(20);//Text groeße
  port = new Serial(this, portName, 9600);
}

void draw() 
{
  kinect.update();//einlesen der neuen Kamera Bilder
  image(kinect.depthImage(), 0, 0);
  IntVector userList = new IntVector();

  kinect.getUsers(userList);
  if (userList.size() > 0) 
  {
    int userId = userList.get(0);
    if ( kinect.isTrackingSkeleton(userId)) 
    {
      //!!!creat new vectors!!!
      PVector leftShoulder = new PVector();//Vektoren left Shoulder
      PVector rightShoulder = new PVector();//Vektor right Sholder
      PVector leftElbow = new PVector();//Vector left Elbow
      PVector leftHand = new PVector();//Vector left Hnad
      PVector leftHip = new PVector();//Vector left Hip
      
      //!!!creat the variable for the angles!!!
      float Winkel_Ellenbogen;
      float Winkel_Ellenbogen1;
      float Winkel_Achsel_0;
      float Winkel_Achsel_1;
      float Winkel_Z_Achse_Schulter_0;
      float Winkel_Z_Achse_Schulter_1;
      
      //!!!get the position from the different vectors!!!
      kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_SHOULDER,leftShoulder);//Position of left Shoulder
      kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_ELBOW,leftElbow);//Position of left Elbow
      kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_SHOULDER,rightShoulder);//Position of right Sholuder
      kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_HIP,leftHip);//Position of left Hip
      kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_HAND,leftHand);//Position of left Hand

      //!!!drawing a limb between two Vectors!!!
      kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);//Linie zwischen gelenken Zeichnen
      kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);
      kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_HAND);
      kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HIP);
      kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_SHOULDER);

      //!!!!!!!!Winkel Ellenbogen!!!!!!!

      //!!!calculate difference by subtracting one vector from another!!!
      PVector differenceVector = PVector.sub(leftShoulder, leftHand);
      PVector differenceVector1 = PVector.sub(leftShoulder, leftElbow);
      PVector differenceVector2 = PVector.sub(leftElbow, leftHand); 
      PVector differenceVector3 = PVector.sub(leftShoulder, leftHip);
      PVector differenceVector4 = PVector.sub(leftElbow, leftHip);
      PVector differenceVector5 = PVector.sub(rightShoulder, leftShoulder);
      PVector differenceVector6 = PVector.sub(leftElbow, rightShoulder);
      //!!!calculate the distance and direction of the difference vector!!!
      magnitude = differenceVector.mag();
      magnitude1 = differenceVector1.mag(); 
      magnitude2 = differenceVector2.mag();
      magnitude3 = differenceVector3.mag();
      magnitude4 = differenceVector4.mag();
      magnitude5 = differenceVector5.mag();
      magnitude6 = differenceVector6.mag(); 
      //!!!Normalize the vector to length 1 (make it a unit vector)!!!
      differenceVector.normalize();        
      differenceVector1.normalize();      
      differenceVector2.normalize();
      differenceVector3.normalize();
      differenceVector4.normalize();
      differenceVector5.normalize();
      differenceVector6.normalize();

      Winkel_Ellenbogen= acos(((magnitude*magnitude)-(magnitude2*magnitude2)-(magnitude1*magnitude1))/(-2*magnitude2*magnitude1));
      Winkel_Ellenbogen1= degrees (Winkel_Ellenbogen);      
      

      Winkel_Achsel_0= acos(((magnitude4*magnitude4)-(magnitude3*magnitude3)-(magnitude1*magnitude1))/(-2*magnitude3*magnitude1));
      Winkel_Achsel_1= degrees (Winkel_Achsel_0);


      Winkel_Z_Achse_Schulter_0= acos(((magnitude6*magnitude6)-(magnitude1*magnitude1)-(magnitude5*magnitude5))/(-2*magnitude1*magnitude5));
      Winkel_Z_Achse_Schulter_1= degrees (Winkel_Z_Achse_Schulter_0);

                 //PROBLEM

      byte out[] = new byte[3]; //This is our output signal that goes serial to the Open Cm board 
      out[0] = byte(Winkel_Achsel_1); //Array 0 is the first axis for the shoulder 
      out[1] = byte(Winkel_Ellenbogen1); //Array 1 is the second axis for the shoulder
      out[2] = byte(Winkel_Z_Achse_Schulter_1); //Array 2 is the complete ellenbow
      port.write(out);

      // display
      pushMatrix(); 
      text("Winkel_Ellenbogen: " + Winkel_Ellenbogen1, 10, 110);
      text("Winkel_Achsel: " + Winkel_Achsel_1, 10, 130);
      text("Winkel_Z_Schulter: " + Winkel_Z_Achse_Schulter_1, 10, 150);


      popMatrix();
    }
  }
}
// user-tracking callbacks!
void onNewUser(int userId) 
{
  println("start pose detection");
  kinect.startPoseDetection("Psi", userId);
}

void onEndCalibration(int userId, boolean successful) 
{
  if (successful) 
  {
    println(" User calibrated !!!");
    kinect.startTrackingSkeleton(userId);
  }
  else 
  {
    println(" Failed to calibrate user !!!");
    kinect.startPoseDetection("Psi", userId);
  }
}

void onStartPose(String pose, int userId) 
{
  println("Started pose for user");
  kinect.stopPoseDetection(userId);
  kinect.requestCalibrationSkeleton(userId, true);
}

bottled_ant

when i worked with serial i noticed that what is printed using print or println is also sent over the serial port, and that is going to cause geberish to be sent which will cause very weird problems and bugs i was using processing v2, i don't hnow if the problem is fixed now.

other things that might cause problems is sending and reviving data over the serial i suggest you only send or recive in processing

do this: processing --> serial or serial --> processing

not this: serial <-> processing