› › ›

Kinect processing serial to arduino problem firmmata

dmngu9dmngu9
edited November 2017 in Arduino

Hi , im doing a project using kinect and arduino and i use firmmata. My problem is the arduino suddenly stop working . i think because of synching issue.

This is my code

 import SimpleOpenNI.*;
> import processing.serial.*;
> import cc.arduino.*;
> 
> SimpleOpenNI  context;
> Arduino arduino;
> int pin =8;
> int pin_motor =9;
> int pos=90;
> 
> float PI = 3.14159265;
> int id=100;
> PVector torso = new PVector();
> float angle;
> 
> //check bit if gesture is ddetected
> int check =0;
> 
> void setup()
> {
>   // instantiate a new context
>   context = new SimpleOpenNI(this);
>  
>   // enable depthMap generation 
>   context.enableDepth();
>  
>   // enable skeleton generation for all joints
>   context.enableUser();
>  
>   background(200,0,0);
>   stroke(0,0,255);
>   strokeWeight(3);
>   smooth();
>  
>   // create a window the size of the depth information
>   size(context.depthWidth(), context.depthHeight());
> 
>   arduino = new Arduino (this,Arduino.list()[0],57600);
>   arduino.pinMode(pin, Arduino.SERVO);
>   arduino.pinMode(pin_motor, Arduino.SERVO);
>   arduino.servoWrite(pin,90);
>   Setpoint=0;
>   SetControllerDirection(DIRECT);
>   SetMode(AUTOMATIC);
>   SetSampleTime(30);
>   SetOutputLimits(-90, 90);
>   SetTunings(1.2, 0, 0.2);
>   for (int i =90; i<93; i++)
>   {
>     arduino.servoWrite(pin,i);
>     delay (200);
>   }
>   
> }
>  
> void draw()
> {
>   // update the camera
>   context.update();
>   // draw depth image
>   image(context.depthImage(),0,0); 
>   IntVector userList = new IntVector();
>   context.getUsers(userList);
>   // check if the skeleton is being tracked
> 
>     for (int i=0; i<userList.size(); i++)
>     {
>       int userId = userList.get(i);
>       
>       if (context.isTrackingSkeleton(userId))
>       {
>         drawSkeleton(userId);
>         float handDistance = getJointDistance(userId, SimpleOpenNI.SKEL_LEFT_HAND,SimpleOpenNI.SKEL_RIGHT_HAND);  
>         PVector position = new PVector();
>         if (id != 100)
>         {
>           context.getJointPositionSkeleton(id, SimpleOpenNI.SKEL_TORSO, torso);
>           Input = atan2(torso.x,torso.z) * 180/PI;
>           Compute(); 
>           arduino.servoWrite(pin,(int)(pos+Output));
>           context.getCoM(id,position);
>           context.convertRealWorldToProjective(position,position);
>           fill(0,255,0);
>           textSize(80);
>           text(id,position.x,position.y);
>           arduino.servoWrite(pin_motor,92);
>         }
>         else
>         {
>           context.getCoM(userId,position);
>           context.convertRealWorldToProjective(position,position);
>           fill(255,0,0);
>           textSize(40);
>           text(userId,position.x,position.y);
>           arduino.servoWrite(pin_motor, 90);
>         }
>       }
>     }
>     
> }
>  
> // draw the skeleton with the selected joints
> void drawSkeleton(int userId)
> {  
>   // draw limbs  
>   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);  
> }
>  
> // Event-based Methods
>  
> // when a person ('user') enters the field of view
> void onNewUser(SimpleOpenNI curContext,int userId)
> {
>   println("onNewUser - userId: " + userId);
>   println("\tstart tracking skeleton");
>   
>   context.startTrackingSkeleton(userId);
> }
> 
> void onLostUser(SimpleOpenNI curContext,int userId)
> {
>   println("onLostUser - userId: " + userId);
>   if (userId == id)
>   {  
>     id =100;
>     check=0;
>   }
> }
> 
> void onVisibleUser(SimpleOpenNI curContext,int userId)
> {
>   //println("onVisibleUser - userId: " + userId);
> }
> 
> 
>  
> // prints out the distance between any two joints 
> float getJointDistance(int userId,int joint1Id,int joint2Id)
> {
>   float d;    // to store final distance value
>  
>   // two PVectors to hold the position of two joints
>   PVector joint1 = new PVector();
>   PVector joint2 = new PVector();
>  
>   // get 3D position of both joints
>   context.getJointPositionSkeleton(userId,joint1Id,joint1);
>   context.getJointPositionSkeleton(userId,joint2Id,joint2);
>  
>   d = distance3D(joint1,joint2);    // calcualte the distance between the two joints
>   if ((d<300) && (check==0))
>   {
>      id =userId;
>      check=1;
>   }
>   return d;
> }
>  
> // calculate the distance between any two points in 3D space and return it as a float
> float distance3D(PVector point1, PVector point2)
> {
>    float diff_x, diff_y, diff_z;    // to store differences along x, y and z axes
>    float distance;                  // to store final distance value
>  
>    // calculate the difference between the two points
>    diff_x = point1.x - point2.x;
>    diff_y = point1.y - point2.y;
>    diff_z = point1.z - point2.z; 
>  
>    // calculate the Euclidean distance between the two points
>    distance = sqrt(pow(diff_x,2)+pow(diff_y,2)+pow(diff_z,2));
>  
>    return distance;  // return the distance as a float
> }
> 
> //PID
> 
> /*working variables*/
> float lastTime;
> double Input, Output, Setpoint;
> double ITerm, lastInput;
> double kp, ki, kd;
> long SampleTime = 1000;
> double outMin, outMax;
> boolean inAuto = false;
> 
> int MANUAL =0;
> int AUTOMATIC  = 1;
> int DIRECT =0;
> int REVERSE= 1;
> 
> int controllerDirection = DIRECT;
> 
> void Compute()
> {
>    /*How long since we last calculated*/
>    if(!inAuto) return;
>    long now = millis();
>    int timeChange = int(now - lastTime);
>  
>    if(timeChange>=SampleTime)
>    { 
>      /*Compute all the working error variables*/
>      double error = Setpoint - Input;
>      ITerm += (ki * error);
>      if(ITerm> outMax) ITerm= outMax;
>      else if(ITerm< outMin) ITerm= outMin;
>      double dInput = (Input - lastInput);
>   
>      /*Compute PID Output*/
>      Output = kp * error + ITerm - kd * dInput;
>      if(Output > outMax) Output = outMax;
>      else if(Output < outMin) Output = outMin;
>      /*Remember some variables for next time*/
>      lastInput = Input;
>      lastTime = now;
>    }
> }
>   
> void SetTunings(double Kp, double Ki, double Kd)
> {
>    if (Kp<0 || Ki<0|| Kd<0) return;
>    double SampleTimeInSec = ((double)SampleTime)/1000;
>    kp = Kp;
>    ki = Ki * SampleTimeInSec;
>    kd = Kd / SampleTimeInSec;
>    
>    if(controllerDirection ==REVERSE)
>    {
>       kp = (0 - kp);
>       ki = (0 - ki);
>       kd = (0 - kd);
>    }
> }
> 
> void SetSampleTime(int NewSampleTime)
> {
>    if (NewSampleTime > 0)
>    {
>       double ratio  = (double)NewSampleTime
>                       / (double)SampleTime;
>       ki *= ratio;
>       kd /= ratio;
>       SampleTime = (long)NewSampleTime;
>    }
> }
> 
> void SetOutputLimits(double Min, double Max)
> {
>    if(Min > Max) return;
>    outMin = Min;
>    outMax = Max;
>     
>    if(Output > outMax) Output = outMax;
>    else if(Output < outMin) Output = outMin;
>  
>    if(ITerm> outMax) ITerm= outMax;
>    else if(ITerm< outMin) ITerm= outMin;
> }
> 
> void SetMode(int Mode)
> {
>    boolean newAuto = (Mode == AUTOMATIC);
>     if(newAuto && !inAuto)
>     {  /*we just went from manual to auto*/
>         Initialize();
>     }
>     inAuto = newAuto;
> }
> 
> void Initialize()
> {
>    lastInput = Input;
>    ITerm = Output;
>    if(ITerm> outMax) ITerm= outMax;
>    else if(ITerm< outMin) ITerm= outMin;
> }
> 
> void SetControllerDirection(int Direction)
> {
>    controllerDirection = Direction;
> }
Tagged:

Answers

Sign In or Register to comment.