› › ›

sending datas over serial port

iggy_DEiggy_DE
edited March 2016 in Kinect

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.

i don´t now why the code isn´t complete in the box. sorry for that

`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); }`

Tagged:

Answers

Sign In or Register to comment.