Soft Communication "Lucky Cat"

// Team Project
// Tian Li & Xinye Wang

int servoPin_01 = A4;
int servoPin_02 = A5;

Servo myServo_01;
Servo myServo_02;

void setup() {
    myServo_01.attach(servoPin_01);
    Particle.function("tail", handleMoveServo_01);

    myServo_02.attach(servoPin_02);
    Particle.function("hand", handleMoveServo_02);
}

int handleMoveServo_01(String cmd){
    if (cmd.toInt() > 215) {
        myServo_01.write(180);
        delay(1000);
        myServo_01.write(90);
        delay(1000);
        return 1;
    }
    return 0;
}

int handleMoveServo_02(String cmd){
    if (cmd.toInt()) {
        myServo_02.write(180);
        delay(2000);
        myServo_02.write(90);
        delay(2000);
        return 1;
    }
    return 0;
}

void loop() {

}

// ml5.js: Pose Estimation with PoseNet
// The Coding Train / Daniel Shiffman
// https://thecodingtrain.com/learning/ml5/7.1-posenet.html
// https://youtu.be/OIo-DIOkNVg
// https://editor.p5js.org/codingtrain/sketches/ULA97pJXR

let particle_access_token = "b2bc473484175c1f4f1f3bb864d9a633c9c05c45"; // ADD your API key here 
let particle_function_url = "https://api.particle.io/v1/devices/e00fce686aab9c87e750ab2c/tail?access_token=" + particle_access_token;

let particle_function_url2 = "https://api.particle.io/v1/devices/e00fce686aab9c87e750ab2c/hand?access_token=" + particle_access_token;

let video;
let poseNet;
let pose;
let skeleton;
let earWidth;
let passLeft = 0;
let passMid = 0;
let passRight = 0;
let timer = 0;
let enterTime = 0;
let passed = 0;

function setup() {
  createCanvas(640, 480);
  video = createCapture(VIDEO);
  video.hide();
  poseNet = ml5.poseNet(video, modelLoaded);
  poseNet.on('pose', gotPoses);
  setInterval( sendToParticleFunction , 2000 );
}

function gotPoses(poses) {
  //console.log(poses);
  if (poses.length > 0) {
    pose = poses[0].pose;
    skeleton = poses[0].skeleton;
  }
}

function modelLoaded() {
  console.log('poseNet ready');
}

function countPass(pose, timer) {
  if (passLeft == 0 && passRight == 0 && passMid == 0) {
    if (pose.leftElbow.x <= 300 || pose.rightElbow.x <= 300) {
      passLeft = 1;
    }
    else if (pose.leftElbow.x >= 400 || pose.rightElbow.x >= 400) {
      passLeft = 1;
    }
    enterTime = timer;
  }
  if (passLeft == 1 && passRight == 0 && passMid == 0) {
    if (timer - enterTime > 3) {
      passLeft = 0;
      passed = 0;
    } else if ((pose.leftElbow.x < 350 && pose.leftElbow.x > 300 ) || (pose.rightElbow.x > 300 && pose.rightElbow.x < 350)) {
      passMid = 1;
      enterTime = timer;
    }
  }
  if (passLeft == 0 && passRight == 1 && passMid == 0) {
    if (timer - enterTime > 3) {
      passRight = 0;
      passed = 0;
    } else if ((pose.leftElbow.x < 350 && pose.leftElbow.x > 300 ) || (pose.rightElbow.x > 300 && pose.rightElbow.x < 350)) {
      passMid = 1;
      enterTime = timer;
    }
  }
  if (passLeft == 1 && passRight == 0 && passMid == 1) {
    if (timer - enterTime > 3) {
      passLeft = 0;
      passMid = 0;
      passed = 0;
    } else if ((pose.leftElbow.x >= 350 ) || (pose.rightElbow.x >= 350)) {
      passed = 1;
      passLeft = 0;
      passMid = 0;
    }
  }
  if (passLeft == 0 && passRight == 1 && passMid == 1) {
    if (timer - enterTime > 3) {
      passLeft = 0;
      passMid = 0;
      passed = 0;
    } else if ((pose.leftElbow.x <= 300 ) || (pose.rightElbow.x <= 300)) {
      passed = 1;
      passRight = 0;
      passMid = 0;
    }
  }
}

function draw() {
  image(video, 0, 0);

  if (pose) {
    // console.log(pose)
    earWidth = pose.leftEar.x - pose.rightEar.x;

    if (frameCount % 60 == 0 && timer > 0) { 
        timer ++;
    }
    countPass(pose, timer);
      
    let eyeR = pose.rightEye;
    let eyeL = pose.leftEye;
    let d = dist(eyeR.x, eyeR.y, eyeL.x, eyeL.y);
    fill(255, 0, 0);
    ellipse(pose.nose.x, pose.nose.y, d);
    fill(0, 0, 255);
    ellipse(pose.rightWrist.x, pose.rightWrist.y, 32);
    ellipse(pose.leftWrist.x, pose.leftWrist.y, 32);

    for (let i = 0; i < pose.keypoints.length; i++) {
      let x = pose.keypoints[i].position.x;
      let y = pose.keypoints[i].position.y;
      fill(0, 255, 0);
      ellipse(x, y, 16, 16);
    }

    for (let i = 0; i < skeleton.length; i++) {
      let a = skeleton[i][0];
      let b = skeleton[i][1];
      strokeWeight(2);
      stroke(255);
      line(a.position.x, a.position.y, b.position.x, b.position.y);
    }
  }
}

function sendToParticleFunction( )
{  
  if (earWidth >= 215) {
    console.log("You are close!!!");
    var particle_data = { arg: earWidth };
    console.log(particle_data)
    // httpPost(particle_function_url , 'text', particle_data,  function(result) {
    //   console.log( result );
    // });
  }
  
  
  if (passed == 1) {
    var particle_data2 = { arg: passed };
  //   httpPost(particle_function_url2 , 'text', particle_data2,  function(result) {
  //   console.log( result );
  // });
    console.log("Passed!!!!!!");

    passed = 0;
    passLeft = 0;
    passMid = 0;
    passRight = 0;
  }
}