GazeFromStream3d5829317252840234262.autosave

import processing.net.*; 
import processing.serial.*;

Client myClient; 

int w = 1920;
int h = 1080;
//int w = 600;
//int h = 600;
Boolean useSerial = true;
boolean usingGaze = true;
boolean renderVisuals = false;

void settings() {
  size(renderVisuals ? w : 1, renderVisuals ? h : 1, P3D);
}
     
 
long lastUpdateTime = millis();
float[][] targetGazes = new float[10][2];
float[][] currentGazes = new float[10][2];

int numGazes = 0;

Serial myPort; 

void setup() { 
  // Connect to the local machine at port 5204.
  // This example will not run if you haven't
  // previously started a server on this port.
  myClient = new Client(this, "127.0.0.1", 4001); 
  for(int i = 0; i < 10; i++) {
    targetGazes[i][0] = 0;
    targetGazes[i][1] = 0;
    currentGazes[i][0] = 0;
    currentGazes[i][1] = 0;
  }
  
  setupPoles();
  resetPoles();
  
  if (useSerial) {
    printArray(Serial.list());
//    myPort = new Serial(this,"/dev/tty.usbmodem00196521", 50000, 'N', 8, 2.0);
 myPort = new Serial(this,"/dev/ttyACM1", 9900, 'N', 8, 2.0);
  }
} 

void parseAndSetGazes(String gazeString) {
    if(gazeString.contains("_")){ 
        String[] outputStrings = gazeString.split("_");
        //print("output string", outputStrings[0]);
        numGazes = outputStrings.length;
        for(int i = 0; i < outputStrings.length; i++) {
          String[] parts = outputStrings[i].split(",");
          
          targetGazes[i][0] = float(parts[0]);
          targetGazes[i][1] = float(parts[1]);
        }
     }
     else {
        numGazes = 1;
        String[] parts = gazeString.split(",");
        targetGazes[0][0] = float(parts[0]);
        targetGazes[0][1] = float(parts[1]);
      }
      
  lastUpdateTime = millis();
}


float animationSpeed = 5.;
void moveGazesToTargets() {
  for (int i = 0; i < numGazes; i++) {
    //print("Grouped", currentGazes[i][0], targetGazes[i][0]);
    currentGazes[i][0] =currentGazes[i][0] += (targetGazes[i][0] - currentGazes[i][0]) / animationSpeed;
    currentGazes[i][1] =currentGazes[i][1] += (targetGazes[i][1] - currentGazes[i][1]) / animationSpeed;
  }  
}

int mapGazeX(float gazeX) {
  return round(map(gazeX, -maxWidthCm, maxWidthCm, 0, w));
}

int mapGazeY(float gazeY) {
  return round(map(gazeY, -2.5, -20, 0, h));
}

float maxWidthCm = 10;
float maxHeightCm = 5;
void drawGazes() {
  for(int i = 0; i < numGazes; i++) {
    int x = mapGazeX(currentGazes[i][0]);
    int y = mapGazeY(currentGazes[i][1]);

    fill(255, 0, 0);
    ellipse(x, y, 20, 20);
    //textSize(26);
    //text(currentGazes[i][0] + "," + currentGazes[i][1] + "cm", round(x) -100, round(y) + 40);
  }
}

int[][] getGazes() {
  int [][] gazes;
  
  if (usingGaze) {
    gazes = new int[numGazes][2];
    
    for (int i = 0; i < numGazes; i++) {
      gazes[i][0] = mapGazeX(currentGazes[i][0]);
      gazes[i][1] = mapGazeY(currentGazes[i][1]);
    }
    
    if (numGazes > 0)
      println("gaze:", gazes[0][0], gazes[0][1]);
  } else {
    gazes = new int[1][2];
    gazes[0][0] = mouseX;
    gazes[0][1] = mouseY;
  }
  return gazes;
}

int minServoQuarters = 992 * 4;
int maxServoQuarters = 2000 * 4;

void setServoValue(int servo, float percentage) {
   int servoX = int(map(percentage, 0, 1, minServoQuarters, maxServoQuarters));
   //if (servo == 0)
   //  println(servoX);
   
   myPort.write(0x84); 
   myPort.write(servo);
   myPort.write(byte(servoX) & 0x7F); 
   myPort.write(byte(servoX>>7) & 0x7F);
}

void updateMotorPositions() {
  for(int i = 0; i < numPoles; i++) {
    setServoValue(i, poleRotations[i]);
  }
}
 
boolean isActive() {
  if (usingGaze) return true;
  return (millis() - lastMouseMovedTime) / 1000. < 5.;
}
 
void draw() { 
  if (myClient.available() > 0) { 
    String gazeString = myClient.readStringUntil('\n');
    
    if (gazeString != null) {
      parseAndSetGazes(gazeString.replaceAll("\n", ""));
    }
  } 
  
  background(255);
  moveGazesToTargets();
  
  drawGazes();
  
  int[][] gazes = getGazes();
  
  if (gazes.length > 0 && isActive()) {
    updatePolesFromGazes(gazes);
  } else {
    updateIdleAnimation();
  }
  
  animatePoles();
  drawPoles();
  
  if (useSerial) {
    updateMotorPositions();
    //println(poleRotations[0]);
    
    delay(20);
  }
} 

long lastMouseMovedTime = 0;

void keyPressed() {
  if(key == TAB) {
    changeDesign();
  } else if(key == UP) {
    changeIdleMode();
  }
}

void mouseMoved() {
  lastMouseMovedTime = millis();
}