Demo entry 4722484

servo kinect

   

Submitted by anonymous on May 10, 2016 at 19:23
Language: Java. Code size: 10.2 kB.

import processing.serial.*;
import SimpleOpenNI.*;

//initalaize serial library
Serial Port;

SimpleOpenNI  context;
// Left Arm Vectors
PVector lHand = new PVector();
PVector lElbow = new PVector();
PVector lShoulder = new PVector();
// Left Leg Vectors
PVector lFoot = new PVector();
PVector lKnee = new PVector();
PVector lHip = new PVector();
// Right Arm Vectors
PVector rHand = new PVector();
PVector rElbow = new PVector();
PVector rShoulder = new PVector();
// Right Leg Vectors
PVector rFoot = new PVector();
PVector rKnee = new PVector();
PVector rHip = new PVector();
//floats for angles
float[] angles = new float[9];

//float data that is inputed 
float data[] = new float[9];

color[]       userClr = new color[] { 
  color(255, 0, 0), 
  color(0, 255, 0), 
  color(0, 0, 255), 
  color(255, 255, 0), 
  color(255, 0, 255), 
  color(0, 255, 255)
};
char writer[] = {
  '1', '2', '3', '4', '5', '6', '7', '8'
};
PVector com = new PVector();                                   
PVector com2d = new PVector();                                   

void setup()
{
  size(640, 480);

  context = new SimpleOpenNI(this);

  // If the camera is connected then continue else stop and message
  if (context.isInit() == false)
  {
    println("Can't init SimpleOpenNI, maybe the camera is not connected!");

    exit();
    return;
  }

  // enable depthMap generation 
  context.enableDepth();

  //enable skeleton generation for all joints
  context.enableUser();


  //Initazlize port

  String portNum = Serial.list()[0];
  Port = new Serial(this, portNum, 9600);
  /*if(Port.available() > 0){
   println("connection good");
   } else {
   println("there is no connection to output");
   exit();
   return;
   }*/


  //setting a visual mirror
  context.setMirror(true);

  //setting the background
  background(200, 0, 0);

  //stroke 
  stroke(0, 0, 255);

  //stroke Boldness
  strokeWeight(3);

  //smoothing Edges
  smooth();
}

void draw()
{
  // update the cam
  context.update();

  // draw depthImageMap
  //image(context.depthImage(),0,0);
  image(context.userImage(), 0, 0);

  // draw the skeleton if it's available
  int[] userList = context.getUsers();
  //if no user recognized then loop until found, also track 2 users
  for (int i=0; i<userList.length; i++)
  {
    //if user is being tracked
    if (context.isTrackingSkeleton(userList[i]))
    {
      stroke(userClr[ (userList[i] - 1) % userClr.length ] );
      drawSkeleton(userList[i]);
    }      

    // draw the center of mass
    if (context.getCoM(userList[i], com))
    {
      context.convertRealWorldToProjective(com, com2d);
      stroke(100, 255, 0);
      strokeWeight(1);
      beginShape(LINES);

      vertex(com2d.x, com2d.y - 5);
      vertex(com2d.x, com2d.y + 5);

      vertex(com2d.x - 5, com2d.y);
      vertex(com2d.x + 5, com2d.y);

      endShape();

      fill(0, 255, 100);
      text(Integer.toString(userList[i]), com2d.x, com2d.y);
    }
  }
}


// draw the skeleton with the selected joints
void drawSkeleton(int userId)
{
  updateAngles(userId);
  // to get the 3d joint data
  /*
  PVector jointPos = new PVector();
   context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_NECK,jointPos);
   println(jointPos);
   */
  //Head to neck
  context.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);
  //Neck to left shoulder
  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
  //Left shoulder to Left elbow
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);

  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);
  //Right Top
  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);
  //
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
  //body left lower
  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);
  //body right lower
  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);
  
  SendSerialData();
  println(angles);
}

// -----------------------------------------------------------------
// SimpleOpenNI events

void onNewUser(SimpleOpenNI curContext, int userId)
{
  println("onNewUser - userId: " + userId);
  println("\tstart tracking skeleton");

  curContext.startTrackingSkeleton(userId);
}

void onLostUser(SimpleOpenNI curContext, int userId)
{
  println("onLostUser - userId: " + userId);
  //Serial.print();
}

void onVisibleUser(SimpleOpenNI curContext, int userId)
{
  println("onVisibleUser - userId: " + userId);
  //Serial.print();
}


//** User created function to update joint information**//
void updateAngles(int userId) {

  angles[0] = atan2(PVector.sub(rShoulder, lShoulder).z, 
  PVector.sub(rShoulder, lShoulder).x);
  println(rShoulder.z +" rshoulder " + lShoulder.z + " lshoulder ");
  println(rShoulder.x +" rshoulder " + lShoulder.x + " lshoulder ");

  // Left Arm
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND, lHand);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, lElbow);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, lShoulder);
  // Left Leg
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_FOOT, lFoot);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_KNEE, lKnee);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HIP, lHip);
  // Right Arm
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND, rHand);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, rElbow);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, rShoulder);  
  // Right Leg
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_FOOT, rFoot);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, rKnee);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HIP, rHip);

  // Left-Side Angles
  angles[1] = angle(lShoulder, lElbow, lHand);
  angles[2] = angle(rShoulder, lShoulder, lElbow);
  angles[3] = angle(lHip, lKnee, lFoot);
  angles[4] = angle(new PVector(lHip.x, 0), lHip, lKnee);
  // Right-Side Angles
  angles[5] = angle(rHand, rElbow, rShoulder);
  angles[6] = angle(rElbow, rShoulder, lShoulder );
  angles[7] = angle(rFoot, rKnee, rHip);
  angles[8] = angle(rKnee, rHip, new PVector(rHip.x, 0));
  
  
  
}

// sending info threw serial communications
void SendSerialData()
{

  Port.write('S'); 
  for (int i=1; i<8; i++) {
    int serialAngle = (int)map(angles[i], -PI/2, PI/2, 0, 255);//(int)map(angles[i],-PI/2, PI/2 , 0, 180);
    Port.write(serialAngle);
    println( "angle " + i + " " +  serialAngle);
  }
}

// creates the diffrence between 2 angles to create a triangle then find the angle and rout put it
float angle(PVector a, PVector b, PVector c) 
{
  float angle01 = atan2(a.y - b.y, a.x - b.x);
  float angle02 = atan2(b.y - c.y, b.x - c.x);
  float ang = angle02 - angle01;
  return ang;
  //PVector limb = PVector.sub(b, a);

  //return degrees(PVector.angleBetween(limb, c));
}


int FindAngleElb(PVector hand, PVector elbow, PVector shoulder) {
  int segmentLength = 400;
  PVector difference = PVector.sub(hand, shoulder);
  float distance = difference.mag();

  // sides of the main triangle
  float a = segmentLength;
  float b = segmentLength;
  float c = min(distance, segmentLength + segmentLength);

  // angles of the main triangle 
  // via Law of Cosines
  float B = acos((a*a + c*c - b*b)/(2*a*c));
  float C = acos((a*a + b*b - c*c)/(2*a*b)); // C is also the elbow angle

  // angle of the shoulder joint 
  float D = atan2(difference.y, difference.x);
  float E = D + B + C - PI; // Pi is 180 degrees in rad
  float F = D + B;

  // use SOHCAHTOA to find rise and run from angles
  elbow.x = (cos(E) * segmentLength) + shoulder.x;
  elbow.y = (sin(E) * segmentLength) + shoulder.y;
  hand.x = (cos(F) * segmentLength) + elbow.x;
  hand.y = (sin(F) * segmentLength) + elbow.y;
  // adjust angles based on orientation of hardware 5
  float shoulderAngle = constrain(degrees(PI/2 - E), 0, 180);
  float elbowAngle = degrees(PI - C);
  fill(255, 0, 0);
  textSize(20);
  text("shoulder: " + int(shoulderAngle) +
    "\nelbow: " + int(elbowAngle), 20, 20);
  return floor(elbowAngle);
}
int FindAngleSho(PVector hand, PVector elbow, PVector shoulder) {
  int segmentLength =400;
  PVector difference = PVector.sub(hand, shoulder);
  float distance = difference.mag();

  // sides of the main triangle
  float a = segmentLength;
  float b = segmentLength;
  float c = min(distance, segmentLength + segmentLength);

  // angles of the main triangle 
  // via Law of Cosines
  float B = acos((a*a + c*c - b*b)/(2*a*c));
  float C = acos((a*a + b*b - c*c)/(2*a*b)); // C is also the elbow angle

  // angle of the shoulder joint 
  float D = atan2(difference.y, difference.x);
  float E = D + B + C - PI; // Pi is 180 degrees in rad
  float F = D + B;

  // use SOHCAHTOA to find rise and run from angles
  elbow.x = (cos(E) * segmentLength) + shoulder.x;
  elbow.y = (sin(E) * segmentLength) + shoulder.y;
  hand.x = (cos(F) * segmentLength) + elbow.x;
  hand.y = (sin(F) * segmentLength) + elbow.y;

  // adjust angles based on orientation of hardware 
  float shoulderAngle = constrain(degrees(PI/2 - E), 0, 180);
  float elbowAngle = degrees(PI - C);
  return floor(elbowAngle);
}

void keyPressed()
{
  exit();
  switch(key)
  {
  case ' ':
    context.setMirror(!context.mirror());
    break;
  }
}  

This snippet took 0.02 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).