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++;
}
}
// 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;
}