Exemplo n.º 1
0
  public void onFrame(Controller controller) {

    Frame currFrame = controller.frame();
    Frame prevFrame = controller.frame(1);

    // get orientation of palm to decide orientation of robotic arm
    Double currOrt = getPalmOrientation(currFrame);
    Double prevOrt = getPalmOrientation(prevFrame);

    // track index finger and thumb to determine arm grip movement
    Double currGripDist = getGripDistance(currFrame);
    Double prevGripDist = getGripDistance(prevFrame);

    // check for gripper locking
    gripperLocked = checkGripperLock(currFrame);

    // use the command interface to send updated position of
    // arm and gripper
    if (frameCount == MAX_FRAME_COUNT) {

      // Update arm position
      if (currOrt > 0 && prevOrt > 0) {
        Double delta = currOrt - prevOrt;
        if (Math.abs(delta) > 0.01) {
          Integer pos = (int) (ARM_RATIO * (currOrt - LEAP_PALM_LOW)) + ARM_LOW;
          if (pos >= ARM_LOW && pos <= ARM_HIGH) {
            commander.sendXbee(pos);
            System.out.println("Sending to arm: " + pos);
          }
        } else {
          // User is not moving palm, check for movement between
          // forefinger and thumb
          if (currGripDist > 0 && prevGripDist > 0 && !gripperLocked) {
            Double dGrip = currGripDist - prevGripDist;
            if (Math.abs(dGrip) > 0.1) {
              Integer pos = (int) (GRIP_RATIO * (currGripDist - LEAP_GRIP_LOW)) + GRIP_LOW;
              int mid = (GRIP_LOW + GRIP_HIGH) / 2;
              pos = mid - (pos - mid);
              if (pos >= GRIP_LOW && pos <= GRIP_HIGH) {
                commander.sendXbee(pos);
                System.out.println("Sending to gripper: " + pos);
              }
            }
          }
        }
      }
      frameCount = 0; // reset frame counter
    } else {
      frameCount++;
    }
  }
Exemplo n.º 2
0
 // Returns the orientation of the palm given a frame of data
 // The double returned is a real number between 0 and 1 indicating
 // the component of the normal to the palm on the plane of the
 // computer screen
 private Double getPalmOrientation(Frame frame) {
   Double armOrt = -1.0;
   if (frame.isValid()) {
     if (!frame.hands().isEmpty()) {
       Vector normal = frame.hands().rightmost().palmNormal();
       armOrt = Math.sqrt(normal.getX() * normal.getX() + normal.getY() * normal.getY());
     }
   }
   return armOrt;
 }