FYF_kinect_servo_processing_WIP

gistfile1.txt

PImage shadow;

import org.openkinect.freenect.*;
import org.openkinect.processing.*;
import processing.serial.*;
PVector a = new PVector(150, 0);
PVector b = new PVector(0, 150);
float angle;
PVector v1;
PVector v2;
float q;
float len;
float root;
Serial myPort; 

Kinect kinect;
PImage img;
float minThresh = 200;  
float maxThresh = 600;

float sumX = 0;
float sumY = 0;
float totalPixels = 0;

void setup() {
  size(500, 480);
  shadow = loadImage("shadow.png");
    String portName = Serial.list()[2]; //change the 0 to a 1 or 2 etc. to match your port
  myPort = new Serial(this, portName, 9600);
  
    kinect = new Kinect(this);
  kinect.initDepth();
  //kinect2.initDevice();
  img = createImage(kinect.width, kinect.height, RGB);
}

void draw() {
  background(255);
  
  img.loadPixels();
//minThresh = map(mouseX, 0, width, 0, 2048);
//maxThresh = map(mouseY, 0, height, 0, 2048);
sumX = 0; 
sumY = 0;
totalPixels = 0;

 
  // Get the raw depth as array of integers
  int[] depth = kinect.getRawDepth(); 


 
  for (int x = 0; x < kinect.width; x++) {
    for (int y = 0; y < kinect.height; y++) {
      int offset = x + y * kinect.width;
      int d = depth[offset];
      
      if (d>minThresh && d<maxThresh) {
        img.pixels[offset] = color(255, 0, 150);
        sumX += x;
        sumY += y;
        totalPixels ++;
      }
      else {
        img.pixels[offset] = color(0); 
      
    }
  }
  img.updatePixels();
  image(img,0,0);
  
  float avgX = sumX/ totalPixels;
  float avgY = sumY/ totalPixels;
  fill(255);
  ellipse(avgX, avgY, 20,20);
  println(avgX + "          " + avgY);

  /*
  fill(255);
  textSize(32);
  text(minThresh + " " + maxThresh, 10,64);
  */
   
  

  
  
  
  
  pushMatrix();
  translate(width/2, height/2);
  fill(0);
  line(0, 0, a.x, a.y);
  line(0, 0, b.x, b.y);
  b.set(avgX-width/2, avgY-height/2, 0);
  //if (mousePressed) {
  //  a.set(mouseX-width/2, mouseY-height/2, 0);
 // }

 // angle = angle(a, b);
  float an = atan2(b.y, b.x) - atan2(a.y, a.x);
  if (an < 0) an += TWO_PI;
   an = angle;

 // drawArc();
  //ellipse(0, 0, 50, 50);
  fill(255);
  text(int(degrees(angle)), 0, 0);
  q = degrees(angle);
  text(int(q), 50, 50);
  myPort.write(int(degrees(angle))/4);

  popMatrix();
  translate(width/2, height/2);
  rotate(radians(q+90));
  len = sq(avgX-300) + sq(avgY-200);
  root = sqrt(len);
  println(root);
  image(shadow, -50, 0, 100, root);
}

//void drawArc() {
 // float angleStart = angle(new PVector(1, 0), a);
 // float angleEnd = angle(new PVector(1, 0), b);
  //arc(0, 0, 150, 150, angleStart, angleEnd);
  //if (angleEnd < angleStart) {
   // arc(0, 0, 150, 150, angleStart, TWO_PI);
    //arc(0, 0, 150, 150, 0, angleEnd);
 // }
//}
 //float angle(PVector v1, PVector v2) {
 // float a = atan2(v2.y, v2.x) - atan2(v1.y, v1.x);
 // if (a < 0) a += TWO_PI;
 // return a;
}