processing sketch for Kinected Arduino

Arduino sketch for Kinected Arduino

/*
   *** Kinect to Arduino ***
   
   created 01 Sept 2012
 by Andrew D. Farquharson
 
 this gets serial coms from Processing and Kinect and displays hand X,Y,Z on 20x4 LCD
 
 - This program is edited from: http://forums.trossenrobotics.com/tutorials/how-to-diy-128/complete-control-of-an-arduino-via-serial-3300/
 */


unsigned long serialdata;
int inbyte;
int digitalState;
int Y;
int X;
int Z;
#include <LiquidCrystal.h>

LiquidCrystal lcd(12, 11, 5, 4, 3, 2);

void setup()
{
  Serial.begin(9600);
    lcd.begin(20, 4);
    lcd.setCursor(0,0);
    lcd.write("Left");
        lcd.setCursor(15,0);
    lcd.write("Right");
            lcd.setCursor(9,1);
    lcd.write("|");
            lcd.setCursor(9,2);
    lcd.write("|");
            lcd.setCursor(9,3);
    lcd.write("|");
}

void loop()
{
  getSerial();
  switch(serialdata)
  {
  case 1:
    {
      //analog digital write
      getSerial();
      switch (serialdata)
      {
      case 1:
        {
          getSerial();
          X = serialdata;
             lcd.setCursor(0,1);
          lcd.print("X=");
          lcd.print(X);
          break;
        }
      case 2:
        {
          getSerial();
          Y = serialdata;
lcd.setCursor(0,2);
          lcd.print("Y=");
          lcd.print(Y);
          break;
          
        }
        case 3:
        {
          getSerial();
         Z  = serialdata;
lcd.setCursor(0,3);
          lcd.print("Z=");
          lcd.print(Z);
          break;
          
        }
                case 4:
        {
          getSerial();
        int  XH = serialdata;
lcd.setCursor(10,1);
          lcd.print("X=");
          lcd.print(XH);
          break;
          
        }
                case 5:
        {
          getSerial();
      int    YH = serialdata;
lcd.setCursor(10,2);
          lcd.print("Y=");
          lcd.print(YH);
          break;
          
        }
                case 6:
        {
          getSerial();
      int    HZ = serialdata;
lcd.setCursor(10,3);
          lcd.print("HZ=");
          lcd.print(HZ);
          break;
          
        }
                        case 7:
        {
          getSerial();
      int    angle = serialdata;
lcd.setCursor(5,0);
          lcd.print("angle=");
          lcd.print(angle);
          break;
          
        }
        
     }
     break; 
   }
  }
}

long getSerial()
{
  serialdata = 0;
  while (inbyte != '/')
  {
    inbyte = Serial.read(); 
    if (inbyte > 0 && inbyte != '/')
    {
     
      serialdata = serialdata * 10 + inbyte - '0';
    }
  }
  inbyte = 0;
  return serialdata;
}

processing sketch for Kinected Arduino

/* --------------------------------------------------------------------------
 * SimpleOpenNI User 
 * --------------------------------------------------------------------------
 * Processing Wrapper for the OpenNI/Kinect library
 * http://code.google.com/p/simple-openni
 * --------------------------------------------------------------------------
 * Main code modified from:
 * prog:  Max Rheiner / Interaction Design / zhdk / http://iad.zhdk.ch/
 *
 * Arduino Coms code modified from:
 * http://forums.trossenrobotics.com/tutorials/how-to-diy-128/complete-control-of-an-arduino-via-serial-3300/
 *
 * Angle Calc Code modified from:
 * http://forum.processing.org/topic/calculating-angles
 * ----------------------------------------------------------------------------
 */

import SimpleOpenNI.*;

SimpleOpenNI  context;

 import processing.serial.*;

Serial myPort;
//import java.awt.AWTException;
//import java.awt.Robot;
 
//Robot robot;
  //*************Angle Calc/draw bit*********************
PVector to        = new PVector();
PVector from      = new PVector();

final float   pointSize = 15;
final float   margin    = 20;
final color   fromColor = #931333;
final color   toColor   = #34AAE8;
final color   lineColor = #54A243;
  //*************Angle Calc/draw bit*********************
int handX;
int handY;
int handZ;
int handXl;
int handYl;
int handZl;
int MyAngle;





void setup()
{
          String portName = Serial.list()[5];
  myPort = new Serial(this, portName, 9600);
  context = new SimpleOpenNI(this);

  // enable depthMap generation
  context.enableDepth();

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

  background(200, 0, 0);

  stroke(0, 0, 255);
  strokeWeight(3);
  smooth();
  PFont myFont = createFont("FFScala", 32);
  textFont(myFont);

  size(context.depthWidth(), context.depthHeight());
  
  //*************Angle Calc/draw bit*********************
  textFont( createFont( "Arial", 14 ) );
  smooth();
  //*************Angle Calc/draw bit*********************
}

void draw()
{

  // update the cam
  context.update();

  // draw depthImageMap
  image(context.depthImage(), 0, 0);
  
    textSize(14);
  text("Right Hand, X:"+ handX +" Y:"+ handY +" Z:"+ handZ +"", 50, 50);
  text("Left  Hand, X:"+ handXl +" Y:"+ handYl +" Z:"+ handZl +"", 50, 70);
   // text("Hand Angle: "+ MyAngle +"º ", 50, 90);
    ///************mouse control****************
   //  try {
    // robot = new Robot();
    // robot.mouseMove(handX*2,handY*2);
//  } 
//    catch (Exception e) {
 //   println("Can't Initialize the Robot");
 // }
    ///************mouse control****************

  // draw the skeleton if it's available
  if (context.isTrackingSkeleton(1)){
    drawSkeleton(1);

  }
      //*************Angle Calc/draw bit*********************
// from.set( handX, handY, 0 );
// to.set(handXl, handYl, 0);
// float angle = seg_angle( to, from );
//drawAngle( angle, height*0.4 );
//drawFrom();
// drawTo();  
 

  //*************Angle Calc/draw bit*********************
}

// draw the skeleton with the selected joints
void drawSkeleton(int userId)
{
  // to get the 3d joint data
 

  PVector leftHand = new PVector();
  PVector rightHand = new PVector();
  PVector leftHip = new PVector();
  PVector rightHip = new PVector();
  PVector torso = new PVector();
  PVector head = new PVector();

  //PVector leftElbow = new PVector();

  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND, leftHand);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND, rightHand);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HIP, rightHip);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HIP, leftHip);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_TORSO, torso);
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, head);


  float groinDist = PVector.dist(leftHand, leftHip) + PVector.dist(rightHand, rightHip);
  float heartDist = PVector.dist(leftHand, torso) + PVector.dist(rightHand, torso);
  float headDist = PVector.dist(leftHand, head) + PVector.dist(rightHand, head);

//***** this bit finds X,Y,Z for any point wanted and converts to reall world... *********

PVector leftHands = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND,leftHand);
  context.convertRealWorldToProjective(leftHand, leftHand);
  // println(" Y="+ leftHand.y + "   X="+ leftHand.x + "  Z="+ leftHand.z +" ");
   
   PVector rightHands = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND,rightHands);
  context.convertRealWorldToProjective(rightHands, rightHands);
 //  println(" Y="+ rightHands.y + "   X="+ rightHands.x + "  Z="+ rightHands.z +" ");
   
// ******* end ******
//************************This Bit sends to arduino*********************************************
//*********Send left hand*****************
          handX = (int)leftHand.x;
            myPort.write("/1/4/"+ handX +"/"); 
          handY = (int)leftHand.y;
            myPort.write("/1/5/"+ handY +"/");      
          handZ = (int)leftHand.z;
            myPort.write("/1/6/"+ handZ +"/");   
  //***** send right hand**********************
         handXl = (int)rightHands.x;
           myPort.write("/1/1/"+ handXl +"/");     
         handYl = (int)rightHands.y;
           myPort.write("/1/2/"+ handYl +"/");    
         handZl = (int)rightHands.z;
           myPort.write("/1/3/"+ handZl +"/");   
  //*******send angle***************************
         myPort.write("/1/7/"+ MyAngle +"/");  
  //***************************************************


  context.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);

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

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

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

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

void onNewUser(int userId)
{
  println("onNewUser - userId: " + userId);
  println("  start pose detection");

  context.startPoseDetection("Psi", userId);
}

void onLostUser(int userId)
{
  println("onLostUser - userId: " + userId);
}

void onStartCalibration(int userId)
{
  println("onStartCalibration - userId: " + userId);
}

void onEndCalibration(int userId, boolean successfull)
{
  println("onEndCalibration - userId: " + userId + ", successfull: " + successfull);

  if (successfull)
  {
    println("  User calibrated !!!");
    context.startTrackingSkeleton(userId);
  }
  else
  {
    println("  Failed to calibrate user !!!");
    println("  Start pose detection");
    context.startPoseDetection("Psi", userId);
  }
}

void onStartPose(String pose, int userId)
{
  println("onStartPose - userId: " + userId + ", pose: " + pose);
  println(" stop pose detection");

  context.stopPoseDetection(userId);
  context.requestCalibrationSkeleton(userId, true);
}

void onEndPose(String pose, int userId)
{
  println("onEndPose - userId: " + userId + ", pose: " + pose);
}
  //*************Angle Calc/draw bit*********************
// ------------------------------------------------------------------------------------------- //
void drawFrom() { noStroke();  fill( fromColor );  ellipse( from.x, from.y, 15, 15 ); }
void drawTo()   { noStroke();  fill( toColor   );  ellipse( to.x,     to.y, 15, 15 ); }

// ------------------------------------------------------------------------------------------- //
void drawAngle( float angle, float seg_length )
{
  noFill();
  stroke( lineColor );
  strokeWeight(1.5);
  
  // Draw the line segments
  
  line( seg_x( angle, margin     ), seg_y( angle, margin     )
      , seg_x( angle, seg_length ), seg_y( angle, seg_length ) );
      
  line( seg_x( -PI, margin     ), seg_y( -PI, margin     )
      , seg_x( -PI, seg_length ), seg_y( -PI, seg_length ) );

  // Draw the arc
  strokeWeight(3);
  arc( from.x, from.y, 3*margin, 3*margin, -PI -.2, angle + .2 );
  
  // Draw the text for the angle  
  float text_angle = ( -PI + angle ) * 0.5f;
  
  
  noStroke();
  fill(0);
  text( degrees(PI + angle), seg_x( text_angle, 2*margin ), seg_y( text_angle, 2*margin ) );
  //****************get angle for arduino********************
MyAngle = (int)(degrees(PI + angle));
  //*************Angle Calc/draw bit*********************
}

// ------------------------------------------------------------------------------------------- //
float seg_x    ( float   angle, float   distance ) { return from.x + cos( angle ) * distance; }
float seg_y    ( float   angle, float   distance ) { return from.y + sin( angle ) * distance; }
float seg_angle( PVector to,    PVector from     ) { return seg_angle( to.x, to.y, from.x, from.y ); }
float seg_angle( float   xTo,   float   yTo
               , float   xFrom, float   yFrom )
{
  float dx = xTo - xFrom;
  float dy = yTo - yFrom;

  return atan2(dy,dx);
}
  //*************Angle Calc/draw bit*********************