Programming Servos with Kinect (Processing => Arduino)

Hi Guys,

I put this in the programming questions thread, but wasn’t sure if someone on this side might find it easier to deal with.

So I’m working on an interactive environment, using processing and arduino to control a set of servos which act as pulleys to draw back a tessellating structure. These servos react to the presence of a person in an area using a Kinect projecting from above (this uses TUIO to set the threashold for the Kinect).

I am having a problem which I think is something kind of simple (possibly), but I’m not certain. The problem I have is that the servo is constantly moving, even when something is detected within the field of vision–it opens, completing its movement to 150, and then closes, then it receives another byte of information saying open again.

I would like for the servos to stay open while something is detected in this area. I know its something to do with the fact that the loop is running constantly, so maybe its something like, only have processing send another byte of information is the boolean changes from false to true, or true to false?

Thanks for spending time reading this guys, and I look forward to hearing from you.

Tom

Processing code:

import vialab.SMT.*;
import processing.serial.*;
Serial myPort;

boolean firstContact = false;
boolean doorOpen = false;

float x, y, xPos;
String val;

void setup() {

  size(displayWidth, displayHeight, SMT.RENDERER);

  SMT.init(this, TouchSource.AUTOMATIC);
  SMT.add(new Zone("Space", 0, 0, width, height));

  //CREATE PORT FOR COMMUNICATION (CHANGE FOR DIFF USB TO COMMUNICATE W/ ARDUINO)
  String portName = Serial.list()[1];
  myPort = new Serial(this, portName, 9600);
  myPort.bufferUntil('\n');
  xPos = 0;
}

void draw() {
  background(255);
}

//  Search for touches
void touchSpace(Zone zone) {
  zone.drag();
}

void drawSpace(Zone zone) {
  if ((zone.getTouches().length>0) && (zone.getTouches().length<=1)) {
    myPort.write("s");
    //  Set touches to value "t"
    Touch t = zone.getTouches()[0];
    //  assign x & y to "t"
    x=t.x;
    y=t.y;
  }

  fill(50, 100, 250, 200);
  noStroke();
}

void serialEvent( Serial myPort) {
  val = myPort.readStringUntil('\n');
  if (val != null) {
    val = trim(val);
    println(val);

    if (firstContact == false) {
      if (val.equals("A")) {
        myPort.clear();
        firstContact = true;
        myPort.write("A");
        println("contact");
      }
    } else {
      println(val);

      if (x<width/2) {   
        xPos = (int)map(x, 0, width/2, 0, 10);
      } else if (x>width/2) {
        xPos = (int)map(x, width/2, width, 10, 0);
      }

      if (xPos > 2) {
        doorOpen = true;
      }

      if (doorOpen == true) {
        myPort.write('1');   
        println("open door!");
      } else {
        myPort.write('0');
      }


      myPort.write("A");
    }
  }
}

Arduino Code:

#include <VarSpeedServo.h>
#include <Adafruit_PWMServoDriver.h>
#include <Wire.h>

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

//SERVO VARIABLES
int FAST;
int SLOW;
//VARIABLE SPEED
int servoSpeed;

// Data received from the serial port
char val;

//DEBUGGING
int ledPin = 13; // Set the pin to digital I/O 13
boolean ledState = LOW; //to toggle our LED

//SERVO OBJECTS
VarSpeedServo myservo_varspeed2;
VarSpeedServo myservo_varspeed3;
VarSpeedServo myservo_varspeed4;
VarSpeedServo myservo_varspeed5;
VarSpeedServo myservo_varspeed6;
VarSpeedServo myservo_varspeed7;
VarSpeedServo myservo_varspeed8;

//SERVO PINS
const int servoPin2 = 2;
const int servoPin3 = 3;
const int servoPin4 = 4;
const int servoPin5 = 5;
const int servoPin6 = 6;
const int servoPin7 = 7;
const int servoPin8 = 8;

//INCOMING KINECT VALUE
int xVal;

void setup() {
  //SETUP DEBUGING LED
  pinMode(ledPin, OUTPUT);

  //INITIALIZE SERIAL COMMUNICATION
  Serial.begin(9600);
  establishContact();  // send a byte to establish contact until receiver responds

  //SERVO PWM
  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

  //SERVO VARIABLE SETUP
  SLOW = 30;
  FAST = 200;

  //SERVO VARIABLE MAP SPEED SETUP
  servoSpeed = map(xVal, 0, 1, 30, 200);

  //CONNECT SERVO OBJECTS TO PINS
  myservo_varspeed2.attach(servoPin2);
  myservo_varspeed3.attach(servoPin3);
  myservo_varspeed4.attach(servoPin4);
  myservo_varspeed5.attach(servoPin5);
  myservo_varspeed6.attach(servoPin6);
  myservo_varspeed7.attach(servoPin7);
  myservo_varspeed8.attach(servoPin8);
}

void loop() {

  //    delay(500);

  //LOOK FOR INCOMING DATA FROM KINECT
  if (Serial.available() > 0) {
    //FIND FIRST VALUE
    val = Serial.read();

    if (val == '1') {
      Serial.println("OPEN Door"); //send back a hello world

      //DEBUGGING, SWITCH LED ON

      ledState = HIGH;
      digitalWrite(ledPin, ledState);

      myservo_varspeed2.write(150, SLOW, true); //(position, speed, case)
            delay(50);

    } else {
      Serial.println("Close Door"); //send back a hello world

      ledState = LOW;
      digitalWrite(ledPin, ledState);

      delay(50);

      myservo_varspeed2.write(50, SLOW, true); //(position, speed, case)

    }
  }
}


void establishContact() {
  while (Serial.available() <= 0) {
    Serial.println("A");   // send a capital A
    delay(300);
  }
}

Hey, so I think I’ve got it, and I hope this can help someone in the future.

Its kinda simple, but hopefully you don’t have to spend time thinking about it.

One thing I’m still uncertain of, which I would love help with is being able to send a value to the arduino instead of a string (i know it has to be received as a string and then converted into a byte, but I don’t know how to do this yet)—this would help with mapping a speed variable if I wanted to add it based on the proximity of the object detected to the center of the field of detection.

So in the processing code, a small change at the end…

      if (doorOpen != newDoorOpen) {
        myPort.write('1');    
        println("open door!");
      } else {
        myPort.write('0');
      }

This means its not constantly running values, just looking to see if the the object has moved in or out of the bounds, which triggers the event.

processing code now reads like this…

import vialab.SMT.*;
import processing.serial.*;
Serial myPort;

boolean firstContact = false;
boolean doorOpen = false;
boolean newDoorOpen = false;

float x, y, xPos;
String val;

void setup() {
  size(displayWidth, displayHeight, SMT.RENDERER);

  SMT.init(this, TouchSource.AUTOMATIC);
  SMT.add(new Zone("Space", 0, 0, width, height));

  //CREATE PORT FOR COMMUNICATION (CHANGE FOR DIFF USB TO COMMUNICATE W/ ARDUINO)
  String portName = Serial.list()[1];
  myPort = new Serial(this, portName, 9600);
  myPort.bufferUntil('\n');
  xPos = 0;
  
}

void draw() {
  background(255);
}

//  Search for touches
void touchSpace(Zone zone) {
  zone.drag();
}

void drawSpace(Zone zone) {
  if ((zone.getTouches().length>0) && (zone.getTouches().length<=1)) {
    //  Set touches to value "t"
    Touch t = zone.getTouches()[0];
    //  assign x & y to "t"
    x=t.x;
    y=t.y;
  }
}

void serialEvent( Serial myPort) {
  val = myPort.readStringUntil('\n');
  if (val != null) {
    val = trim(val);
    println(val);

    if (firstContact == false) {
      if (val.equals("A")) {
        myPort.clear();
        firstContact = true;
        myPort.write("A");
        println("contact");
      }
    } else {
      println(val);

      if (x<width/2) {   
        xPos = (int)map(x, 0, width/2, 0, 10);
      } else if (x>width/2) {
        xPos = (int)map(x, width/2, width, 10, 0);
      }

      if (xPos > 2) {
        newDoorOpen = true;
      } else {
        newDoorOpen = false;
      }

      if (doorOpen != newDoorOpen) {
        myPort.write('1');    
        println("open door!");
      } else {
        myPort.write('0');
      }
    }
  }
}