Move a servo motor (Ax-12) with arduino, processing and kinect

lufrazao

Hey guys,

I'm having a problem with my code. I wanna move a Dynamixel servo according to the movement of a person, using kinect and processing. When I tried only with the Arduino moving randomly the servo it worked perfectly, but when I try with processing+kinect it doesnt move at all. I'm gonna appreciate any help :) Oh and I'm new at this... so correct me please if y'all find dumb mistakes :P

Arduino code:

#include <DynamixelSerial.h>
int angulo=0;

void setup() {
  Dynamixel.begin(1000000, 2); // Inicialize the servo at 1Mbps and Pin Control 2
  Serial.begin(9600);
}

void loop() {

  if (Serial.available() > 0) {
  // get incoming byte:
  angulo=Serial.read();
  }

  Dynamixel.ledStatus(1, ON);
  Dynamixel.move(1,angulo);

  delay(1000);


}

Processing code:

import SimpleOpenNI.*;
import processing.serial.*;
import cc.arduino.*;
Arduino arduino;
int pos=0;
Serial myPort;

SimpleOpenNI context;
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)
};
PVector com = new PVector(); 
PVector com2d = new PVector();

void setup() {
  size(640, 480);
  println(Serial.list());
  String portName = Serial.list()[1];
  myPort = new Serial(this, portName, 9600);
  //arduino = new Arduino(this, Arduino.list()[1], 9600); //your offset may vary
  //arduino.pinMode(2);
  context = new SimpleOpenNI(this);
  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();
  context.enableRGB();
  background(200, 0, 0);
  stroke(0, 0, 255);
  strokeWeight(3);
  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();
  for (int i=0; i<userList.length; i++) {
    if (context.isTrackingSkeleton(userList)) {
      stroke(userClr[ (userList - 1) % userClr.length ] );
      drawSkeleton(userList);
    }
  }
}



// draw the skeleton with the selected joints
void drawSkeleton(int userId) {


  // aqui é definido qual parte do corpo vai rastrear
  PVector jointPos = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, jointPos);
  PVector convertedHead = new PVector();
  context.convertRealWorldToProjective(jointPos, convertedHead);
  //desenhar uma elipse sobre a parte do corpo rastreada
  fill(255, 0, 0);
  ellipse(convertedHead.x, convertedHead.y, 20, 20);

  //draw YOUR Right Shoulder
  PVector jointPosLS = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, jointPosLS);
  PVector convertedLS = new PVector();
  context.convertRealWorldToProjective(jointPosLS, convertedLS);
  //int LS = convertedLS.x, convertedLS.y

  //draw YOUR Right Elbow
  PVector jointPosLE = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, jointPosLE);
  PVector convertedLE = new PVector();
  context.convertRealWorldToProjective(jointPosLE, convertedLE);
  fill(200, 200, 200);
  ellipse(convertedLE.x, convertedLE.y, 20, 20);


  //angulooo
  int anguloLSE =int(degrees(atan2(convertedLS.x - convertedLE.x, convertedLS.y - convertedLE.y)));
  println(anguloLSE); 
  myPort.write(anguloLSE);



  //se quiser desenhar o esqueleto inteiro, descomentar as linhas abaixo
  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(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);
}

void onVisibleUser(SimpleOpenNI curContext, int userId)
{
  //println("onVisibleUser - userId: " + userId);
}
void keyPressed()
{
  switch(key)
  {
  case ' ':
    context.setMirror(!context.mirror());
    break;
  }
}

kfrajer

Edit your post (gear icon in the top right corner of your post), select your code and hit ctrl+o to format your code. Make sure there is an empty line above and below your code.

Kf

lufrazao

Done :)

kfrajer

I will suggest to drop the frameRate of processing to maybe 1 or 2 to start, as you have a delay of 1000 in your arduino code.

Now, you said that your arduino works well by itself.

When I tried only with the Arduino moving randomly the servo it worked perfectly

This was done arduino+processing or you introduced the random numbers in the ino code? I suggest you run a simple sketch of processing +arduino.

Notice that Arduino will read a byte (check here), a value from 0 to 255. However, you are sending a value of 0 to 360. This is not the main problem in your code right now but you will need to tackle it sooner or later.

Kf

dimpapadop

Since the command myPort.write(anguloLSE);

sends an integer value to arduino, i'd suggest you to use in arduino firmware line 13 the comand:

angulo=Serial.ParseInt(); instead of angulo=Serial.read();

**this should do the job! **(the easiest solution to your problem so not to change anything else).

I suppose the Processing code is tested and sends the correct angle

you have to understand arduino serial monitor to use it with other ways like you did. ParseInt() is easy in your example because reads whatever is in the serial buffer as an integer (angle) and you don't have to deal with more complex solutions of manipulating serial data.

(excuse my English)

dimpapadop

ps you do not need the

    import cc.arduino.*;

    Arduino arduino;

library since you do not use the firmata protocol. You just send an angle via serial and arduino "caches" that value

dimpapadop

i also run your processing code. It did not run properly.

Try: (just rename the COM3 port

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

int pos=0;
Serial myPort;

SimpleOpenNI context;
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)
};
PVector com = new PVector(); 
PVector com2d = new PVector();

void setup() {
  size(640, 480);
  println(Serial.list());

  // rename COM3 to the arduino port
  myPort = new Serial(this,"COM3", 9600);

  context = new SimpleOpenNI(this);
  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();
  context.enableRGB();
  background(200, 0, 0);
  stroke(0, 0, 255);
  strokeWeight(3);
  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
  IntVector userList = new IntVector();
  context.getUsers(userList);

    if (userList.size() > 0) {
      int userId = userList.get(0);
      drawSkeleton(userId);
    }

}



// draw the skeleton with the selected joints
void drawSkeleton(int userId) {


  // aqui é definido qual parte do corpo vai rastrear
  PVector jointPos = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, jointPos);
  PVector convertedHead = new PVector();
  context.convertRealWorldToProjective(jointPos, convertedHead);
  //desenhar uma elipse sobre a parte do corpo rastreada
  fill(255, 0, 0);
  ellipse(convertedHead.x, convertedHead.y, 20, 20);

  //draw YOUR Right Shoulder
  PVector jointPosLS = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, jointPosLS);
  PVector convertedLS = new PVector();
  context.convertRealWorldToProjective(jointPosLS, convertedLS);
  //int LS = convertedLS.x, convertedLS.y

  //draw YOUR Right Elbow
  PVector jointPosLE = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, jointPosLE);
  PVector convertedLE = new PVector();
  context.convertRealWorldToProjective(jointPosLE, convertedLE);
  fill(200, 200, 200);
  ellipse(convertedLE.x, convertedLE.y, 20, 20);


  //angulooo
  int anguloLSE =int(degrees(atan2(convertedLS.x - convertedLE.x, convertedLS.y - convertedLE.y)));
  println(anguloLSE); 
  myPort.write(anguloLSE);



  //se quiser desenhar o esqueleto inteiro, descomentar as linhas abaixo
  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(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);
}

void onVisibleUser(SimpleOpenNI curContext, int userId)
{
  //println("onVisibleUser - userId: " + userId);
}
void keyPressed()
{
  switch(key)
  {
  case ' ':
    context.setMirror(!context.mirror());
    break;
  }
}