Kinect - Arduino - Stepper 28 BYJ

Hello, I am trying to send angles of my arm from Processing through serial to Arduino and control two stepper motors. I can´t find any answer online or similar project. I need this for my bachelor thesis so I would really appreciate any help cause I´ve tried so many different things and I am slightly going nuts. Of course, I don´t have a lot of expirience in programing but I was told this isn´t a hard thing to do.
I am trying to solve this problem with sending a string from Processing and reading each part for every stepper. This is what I´ve done so far, but nothing I´ve tried works.

Processing code:

import SimpleOpenNI.*;
SimpleOpenNI kinect;

// import the processing serial library
import processing.serial.*;
// and declare an object for our serial port
Serial port;

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

kinect = new SimpleOpenNI(this);
kinect.enableDepth();
kinect.enableUser();
kinect.setMirror(true);

println(Serial.list());
String portName = Serial.list()[1];

// initialize our serial object with this port
// and the baud rate of 9600
port = new Serial(this, portName, 9600);
}

void draw() {
kinect.update();
PImage depth = kinect.depthImage();
image(depth, 0, 0);

IntVector userList = new IntVector();
kinect.getUsers(userList);

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

if ( kinect.isTrackingSkeleton(userId)) {

// POZICIJE ZGLOBOVA
PVector rightHand = new PVector();
kinect.getJointPositionSkeleton(userId,
SimpleOpenNI.SKEL_RIGHT_HAND, rightHand);

PVector rightElbow = new PVector();
kinect.getJointPositionSkeleton(userId,
SimpleOpenNI.SKEL_RIGHT_ELBOW, rightElbow);

PVector rightShoulder = new PVector();
kinect.getJointPositionSkeleton(userId,
SimpleOpenNI.SKEL_RIGHT_SHOULDER, rightShoulder);

PVector rightHip = new PVector();
kinect.getJointPositionSkeleton(userId,
SimpleOpenNI.SKEL_RIGHT_HIP, rightHip);

//PREBACIVANJE U PROSTORNE KOORDINATE
PVector convertedRightHand = new PVector();
kinect.convertRealWorldToProjective(rightHand, convertedRightHand);

PVector convertedRightElbow = new PVector();
kinect.convertRealWorldToProjective(rightElbow, convertedRightElbow);

PVector convertedRightShoulder = new PVector();
kinect.convertRealWorldToProjective(rightShoulder,
convertedRightShoulder);

PVector convertedRightHip = new PVector();
kinect.convertRealWorldToProjective(rightHip,
convertedRightHip);

// PREBACIVANJE U 2D PROSTOR
PVector rightHand2D = new PVector(rightHand.x, rightHand.y);
PVector rightElbow2D = new PVector(rightElbow.x, rightElbow.y);
PVector rightShoulder2D = new PVector(rightShoulder.x, rightShoulder.y);
PVector rightHip2D = new PVector(rightHip.x, rightHip.y);

// calculate the axes against which we want to measure our angles
PVector torsoOrientation = PVector.sub(rightShoulder2D, rightHip2D);
PVector upperArmOrientation = PVector.sub(rightElbow2D, rightShoulder2D);

// calculate the angles of each of our arms
float shoulderAngle =
angleOf(rightElbow2D, rightShoulder2D, torsoOrientation);
float elbowAngle =
angleOf(rightHand2D, rightElbow2D, upperArmOrientation);

// show the angles on the screen for debugging
fill(255,0,0);
scale(3);
text("shoulder: " + int(shoulderAngle) + “\n” +
" elbow: " + int(elbowAngle), 20, 20);

int roundShoulder = round(shoulderAngle);
int roundElbow = round(elbowAngle);

String shoulder = str(roundShoulder);
String elbow = str(roundElbow);

char a; a = ‘A’;
char b; b = ‘B’;
char c; c = ‘C’;
String out = a + shoulder + b + elbow + c;
println(out);
port.write(out);

}
}
}

float angleOf(PVector one, PVector two, PVector axis) {
PVector limb = PVector.sub(two, one);
return degrees(PVector.angleBetween(limb, axis));
}

void onNewUser(SimpleOpenNI curKinect, int userId)
{
curKinect.startTrackingSkeleton(userId);
}

void onLostUser(SimpleOpenNI curKinect, int userId)
{
println("onLostUser - userId " + userId);
}

void onVisibleUser(SimpleOpenNI curKinect, int userId)
{
println("onVisibleUser - userId: " + userId);
}

Arduino code:

#include <EEPROM.h>
#include <Stepper.h>
#define STEPS_PER_MOTOR_REVOLUTION 32
#define STEPS_PER_OUTPUT_REVOLUTION 32 * 64
Stepper shoulder(STEPS_PER_MOTOR_REVOLUTION, 8, 10, 9, 11);
Stepper elbow(STEPS_PER_MOTOR_REVOLUTION, 4, 6, 5, 7);

String inputString = “”;
boolean stringComplete = false;

void setup(){
shoulder.setSpeed(700); //brzina
elbow.setSpeed(700); //motora

Serial.begin(9600);
inputString.reserve(200);
}

void loop() {
serialEvent();
if (stringComplete) {

String ugao1 = inputString.substring(inputString.indexOf(‘A’) + 1,
inputString.indexOf(‘B’));
String ugao2 = inputString.substring(inputString.indexOf(‘B’) + 1,
inputString.indexOf(‘C’));

int kutRame = ugao1.toInt();
int kutLakat = ugao2.toInt();

shoulder.step(kutRame*5.68888888888888);

if ((kutRame > EEPROM.get(0,kutRame))){
shoulder.step(1);
}
else if ((kutRame < EEPROM.get(0,kutRame))){
shoulder.step(- 1);
}
else if ((kutRame = EEPROM.get(0,kutRame))){
shoulder.step(0);
}
else if ((kutLakat > EEPROM.get(1,kutLakat))){
elbow.step(1);
}
else if ((kutLakat < EEPROM.get(1,kutLakat))){
elbow.step(-1);
}
else if ((kutLakat = EEPROM.get(1,kutLakat))){
elbow.step(0);
}*/

EEPROM.update(0,kutRame);
EEPROM.update(1,kutLakat);
// clear the string:
inputString = “”;
stringComplete = false;

}
}
void serialEvent() {
if (Serial.available()) {
// get the new byte:
char inChar = Serial.read();
// add it to the inputString:
inputString += inChar;
// if the incoming character is a newline, set a flag
// so the main loop can do something about it:
if (inChar == ‘\n’) {
stringComplete = true;
}
}
}

Hi,
Can you please post a copy of your sketch, using code tags?
They are made with the </> icon in the reply Menu.
See section 7 http://forum.arduino.cc/index.php/topic,148850.0.html

Have you tried a simple program to communicate between arduino and PC?

Processing has some examples that you should try first to establish your link.
When you have got that working, then build up from there with your other features.

Tom… :slight_smile:

import SimpleOpenNI.*;
SimpleOpenNI  kinect;

// import the processing serial library
import processing.serial.*;
// and declare an object for our serial port
Serial port;

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

  kinect = new SimpleOpenNI(this);
  kinect.enableDepth();
  kinect.enableUser();
  kinect.setMirror(true);
  
  println(Serial.list());
  String portName = Serial.list()[1];
  
  // initialize our serial object with this port
  // and the baud rate of 9600
  port = new Serial(this, portName, 9600);
}

void draw() {
  kinect.update();
  PImage depth = kinect.depthImage();
  image(depth, 0, 0);
  
  IntVector userList = new IntVector();
  kinect.getUsers(userList);
  
  if (userList.size() > 0) {
    int userId = userList.get(0);
    
    if ( kinect.isTrackingSkeleton(userId)) {
      
      // POZICIJE ZGLOBOVA
      PVector rightHand = new PVector();
      kinect.getJointPositionSkeleton(userId,
        SimpleOpenNI.SKEL_RIGHT_HAND, rightHand);
      
      PVector rightElbow = new PVector();
      kinect.getJointPositionSkeleton(userId,
            SimpleOpenNI.SKEL_RIGHT_ELBOW, rightElbow);
      
      PVector rightShoulder = new PVector();
      kinect.getJointPositionSkeleton(userId,
            SimpleOpenNI.SKEL_RIGHT_SHOULDER, rightShoulder);
            
      PVector rightHip = new PVector();
      kinect.getJointPositionSkeleton(userId,
            SimpleOpenNI.SKEL_RIGHT_HIP, rightHip);
      
      //PREBACIVANJE U PROSTORNE KOORDINATE
      PVector convertedRightHand = new PVector();
      kinect.convertRealWorldToProjective(rightHand, convertedRightHand);
      
      PVector convertedRightElbow = new PVector();
      kinect.convertRealWorldToProjective(rightElbow, convertedRightElbow);
      
      PVector convertedRightShoulder = new PVector();
      kinect.convertRealWorldToProjective(rightShoulder,
                                          convertedRightShoulder);
                                          
      PVector convertedRightHip = new PVector();
      kinect.convertRealWorldToProjective(rightHip,
                                          convertedRightHip);
      
      // PREBACIVANJE U 2D PROSTOR
      PVector rightHand2D = new PVector(rightHand.x, rightHand.y);
      PVector rightElbow2D = new PVector(rightElbow.x, rightElbow.y);
      PVector rightShoulder2D = new PVector(rightShoulder.x, rightShoulder.y);
      PVector rightHip2D = new PVector(rightHip.x, rightHip.y);
      
            // calculate the axes against which we want to measure our angles
      PVector torsoOrientation = PVector.sub(rightShoulder2D, rightHip2D);
      PVector upperArmOrientation = PVector.sub(rightElbow2D, rightShoulder2D);
      
      // calculate the angles of each of our arms
      float shoulderAngle =
        angleOf(rightElbow2D, rightShoulder2D, torsoOrientation);
      float elbowAngle =
        angleOf(rightHand2D, rightElbow2D, upperArmOrientation);
      
      // show the angles on the screen for debugging
      fill(255,0,0);
      scale(3);
      text("shoulder: " + int(shoulderAngle) + "\n" +
           " elbow: " + int(elbowAngle), 20, 20);
      
      int roundShoulder = round(shoulderAngle);
      int roundElbow = round(elbowAngle);
      
      String shoulder = str(roundShoulder);
      String elbow = str(roundElbow);
      
      char a;  a = 'A';
      char b;  b = 'B';
      char c;  c = 'C';
      String out = a + shoulder + b + elbow + c;
      println(out);
      port.write(out);
      
      
      
      
      /*byte out[] = new byte[2];
      out[0] = byte(shoulderAngle);
      out[1] = byte(elbowAngle);
      port.write(out);*/
    }
  }
}

float angleOf(PVector one, PVector two, PVector axis) {
  PVector limb = PVector.sub(two, one);
  return degrees(PVector.angleBetween(limb, axis));
}

void onNewUser(SimpleOpenNI curKinect, int userId)
{
  curKinect.startTrackingSkeleton(userId);
}

void onLostUser(SimpleOpenNI curKinect, int userId)
{
  println("onLostUser - userId " + userId);
}

void onVisibleUser(SimpleOpenNI curKinect, int userId)
{
  println("onVisibleUser - userId: " + userId);
}
#include <EEPROM.h>
#include <Stepper.h>
#define STEPS_PER_MOTOR_REVOLUTION 32   
#define STEPS_PER_OUTPUT_REVOLUTION 32 * 64    
Stepper shoulder(STEPS_PER_MOTOR_REVOLUTION, 8, 10, 9, 11);
Stepper elbow(STEPS_PER_MOTOR_REVOLUTION, 4, 6, 5, 7);

String inputString = "";
boolean stringComplete = false;

void setup(){
  shoulder.setSpeed(700); //brzina 
  elbow.setSpeed(700);    //motora
  
  Serial.begin(9600);
  inputString.reserve(200);
}

void loop() {
  serialEvent(); 
  if (stringComplete) {
    
    String ugao1 = inputString.substring(inputString.indexOf('A') + 1, 
                                         inputString.indexOf('B'));
    String ugao2 = inputString.substring(inputString.indexOf('B') + 1,
                                         inputString.indexOf('C'));
    Serial.println(inputString);

    int kutRame = ugao1.toInt();
    int kutLakat = ugao2.toInt();

    shoulder.step(kutRame*5.68888888888888);
    
   if ((kutRame > EEPROM.get(0,kutRame))){
      shoulder.step(1);
      }
      else if ((kutRame < EEPROM.get(0,kutRame))){
        shoulder.step(- 1);
      }
      else if ((kutRame = EEPROM.get(0,kutRame))){
        shoulder.step(0);
      }
      else if ((kutLakat > EEPROM.get(1,kutLakat))){
        elbow.step(1);
      }
      else if ((kutLakat < EEPROM.get(1,kutLakat))){
        elbow.step(-1);
      }
      else if ((kutLakat = EEPROM.get(1,kutLakat))){
        elbow.step(0);
      }
    
    EEPROM.update(0,kutRame);
    EEPROM.update(1,kutLakat);
    // clear the string:
    inputString = "";
    stringComplete = false;
  
 }
}
void serialEvent() {
  if (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}