Motion-controlled Servos with Leap Motion & Raspberry Pi

The ability to make a physical object mirror the movement of your hands is something out of a science fiction movie. But here at PubNub, we just made it a reality using Leap Motion, Raspberry Pi, several micro-servos and PubNub Data Streams. And even better, you can control the physical object from anywhere on Earth.

Project Overview

The user controls the servos using the Leap Motion. The two servos mirror the movement of the user’s two individual hands. Attached to the servos are 8×8 RGB LED Matrices, which react to each finger movement on your hand. The Leap Motion communicates directly with the Raspberry Pi via PubNub Data Streams with minimal latency, and the Raspberry Pi then drives the servos.

Here’s the final project in-action:

In this tutorial, we’ll show you how to build the entire thing. Although in this example we use PubNub to communicate with our Raspberry Pi to control servos, the same techniques can be applied to control any internet connected device. PubNub is simply the communication layer used to allow any two devices to speak. In this case we use the Leap Motion as a Raspberry Pi controller, but you can imagine using PubNub to initially configure devices or for realtime data collection.

All the code you need for the project is available in our open source GitHub repository here.

Additionally, we have a separate tutorial for assembling the servos, as well as creating the driver for the LED matrices.

What You’ll Need

Before we begin this tutorial, you will need several components:

• Leap Motion + Leap Motion Java SDK
• Raspberry Pi Model B+
• Tower Pro Micro Servo (x4)
• Adafruit PWM Servo Driver
• Optional: Display case (we ordered our’s from Ponoko)

To bring it all together, we had a custom display case built to house our project

Communicating from Leap Motion to Pi

Leap Motion is a powerful device equipped with two monochromatic IR cameras and three infrared LEDs. In this project, the Leap Motion is just going to capture the pitch and yaw of the user’s hands and publish them to a channel via PubNub. Attributes like pitch, yaw and roll of hands are all pre-built into the Leap Motion SDK.

To recreate realtime mirroring, the Leap Motion publishes messages 20x a second with information about each of your hands and all of your fingers to PubNub. On the other end, our Raspberry Pi is subscribed to the same channel and parses these messages to control the servos and the lights.

First, open up a Java IDE and create a new project. We used IntelliJwith JDK8.

If you’ve never worked with a Leap Motion in Java before, you should first check out this getting started guide for the Leap Motion Java SDK.

Next, install the PubNub Java SDK. If your project has all the proper imports at the top of the file you should see the following:

     import java.io.IOException;
        import java.lang.Math;
        import com.leapmotion.leap.*;
        import com.pubnub.api.*;
        import org.json.*;

Now that the project has both Leap Motion and PubNub SDKs installed, let’s get started.

It is crucial that you make your project implement Runnable so that we can have all Leap activity operate in its own thread. Begin by setting up the project main, an implementation of the Runnable interface and initializing of global variables we will be using later like so:

	public class LeapToServo implements Runnable{
	//Define Channel name
	public static final String CHANNEL = “my_channel“;
	//Create PubNub instance
	private Pubnub pubnub;
	//Create Leap Controller instance
	private Controller controller;
	//is Runnable running?
	private boolean running;
	//Last value of Left Yaw
	int oldLeftYaw = 0;
	//Last value of Left Pitch
	int oldLeftPitch = 0;
	//Last value of Right Yaw
	int oldRightYaw = 0;
	//Last value of Right Pitch
	int oldRightPitch = 0;
	//Takes in Pubkey and Subkey and sets up PubNub Configuration
	public LeapToServo(String pubKey, String subKey){
	pubnub = new Pubnub(pubKey, subKey);
	pubnub.setUUID(“LeapController“);
	}
	/**
	* Implementation of the Runnable interface.
	*/
	public void run(){
	for(;;) {
	if (!running) break;
	captureFrame(this.controller);
	try {
	Thread.sleep(50);
	} catch (InterruptedException e) {
	e.printStackTrace();
	}
	}
	}
	public static void main(String[] args) {
	String pubKey = “pubnub_publish_key“;
	String subKey = “pubnub_subscribe_key“;
	LeapToServo s = new LeapToServo(pubKey, subKey);
	s.startTracking();
	}
	}

view rawMain.java hosted with ❤ by GitHub

The Leap Motion captures about 300 frames each second. Within each frame, we have access to tons of information about our hands, such as the number of fingers extended, pitch, yaw, and hand gestures. The servos move in a sweeping motion with 180 degrees of rotation. In order to simulate a hand’s motion, we use two servos where one servo monitors the pitch (rotation around X-axis) of the hand and the other monitors the yaw (rotation around Y-axis).

The result is a servo which can mimic most of a hand’s movements.

Use a function to start tracking the user’s hands, which will initialize a new Leap Controller, start a new thread, and then process all the information about the hands. The function named startTracking() looks like this:

	public void startTracking(){
	// Create a controller
	this.controller = new Controller();
	this.running = true;
	Thread t = new Thread(this);
	t.start();
	// Keep this process running until Enter is pressed
	System.out.println(“Press Enter to quit…“);
	try {
	System.in.read();
	this.running=false;
	t.join();
	cleanup();
	} catch (IOException e) {
	e.printStackTrace();
	} catch (InterruptedException e){
	e.printStackTrace();
	}
	}

view rawstartTracking.java hosted with ❤ by GitHub

When the thread starts running, it calls captureFrame(), which will look at the most recent frame for any hands. If a hand is found, it calls the function handleHand(), which will get the values of the hand’s pitch & yaw. If the frame does in fact have hands,  a message is published containing all relevant hand information.

This was implemented like so:

	public JSONObject handleHand(Hand hand){
	boolean isLeft = hand.isLeft();
	String handName = (isLeft) ? “left“ : “right“;
	Vector direction = hand.direction();
	int yaw = (int) Math.toDegrees(direction.yaw());
	int pitch = (int) Math.toDegrees(direction.pitch());
	// Normalize Yaw and Pitch
	yaw = normalizeDegree(yaw);
	pitch *= (isLeft) ? –1 : 1;
	pitch = normalizeDegree(pitch);
	// Get PWM Values
	yaw = (int) yawDegreeToPWM(yaw);
	pitch = (int) pitchDegreeToPWM(pitch);
	JSONObject payload = new JSONObject();
	int theByte = fingersToByte(hand);
	int oldYaw = (isLeft) ? oldLeftYaw : oldRightYaw;
	int oldPitch = (isLeft) ? oldLeftPitch : oldRightPitch;
	if( (Math.abs(oldPitch – pitch) > 5) || (Math.abs(oldYaw – yaw) > 5) ) {
	try {
	payload.put(handName + “_yaw“, yaw);
	payload.put(handName + “_pitch“, pitch);
	} catch (JSONException e) {
	e.printStackTrace();
	}
	if (isLeft) {
	this.oldLeftYaw = yaw;
	this.oldLeftPitch = pitch;
	} else {
	this.oldRightYaw = yaw;
	this.oldRightPitch = pitch;
	}
	}
	else{
	try {
	payload.put(handName + “_yaw“, oldYaw);
	payload.put(handName + “_pitch“, oldPitch);
	} catch (JSONException e) {
	e.printStackTrace();
	}
	}
	return payload;
	}
	public void captureFrame(Controller controller) {
	// Get the most recent frame and report some basic information
	Frame frame = controller.frame();
	JSONObject payload = new JSONObject();
	for (Hand hand : frame.hands()) {
	try {
	if (hand.isLeft()) {
	payload.put(“left_hand“, handleHand(hand));
	} else {
	payload.put(“right_hand“, handleHand(hand));
	}
	} catch (JSONException e) {
	e.printStackTrace();
	}
	}
	if(!payload.toString().equals(“{}“)) {
	pubnub.publish(CHANNEL, payload, new Callback() { });
	}
	}

view rawcaptureFrame.java hosted with ❤ by GitHub

The values the Leap Motion outputs for pitch and yaw are in radians, however the servos are expecting a pulse width modulation (or PWM) between 150 and 600MHz. Thus, we do some conversions to take the radians and convert them into degrees and then normalize the degrees into the corresponding PWM value.

These functions look like this:

	/**
	* Take radian reading and return degree value adjusted for our desired range/midpoint of servo range
	* @param radians Radian value to be converted
	* @return Adjusted degree value
	*/
	public static int radiansToAdjustedDegrees(int radians){
	int degrees = (int) (radians * (180 / Math.PI));
	degrees = (int) Math.floor(degrees + 90);
	return degrees;
	}
	/**
	* Get a PWM value from degree closely modeled by a quadratic equation
	* @param degree pitch degree value
	* @return PWM value
	*/
	public static double pitchDegreeToPWM(double degree){
	double a = 0.00061728395;
	double b = 2.38888888889;
	double c = 150;
	return a*(degree*degree) + b*degree + c;
	}
	/**
	* Get a PWM value from degree closely modeled by a quadratic equation
	* @param degree pitch degree value
	* @return PWM value
	*/
	public static double yawDegreeToPWM(double degree){
	double a = 0.0;
	double b = 3.19444444;
	double c = 150;
	return a*(degree*degree) + b*degree + c;
	}
	/**
	* Force a value to be between 0 and 180 degrees for servo
	* @param value degree value returned by Leap Controller
	* @return normalized value between 0-180
	*/
	public static int normalizeDegree(int value){
	value = (value > 90) ? 90 : value;
	value = (value < –90) ? –90 : value;
	return value+90;
	}

view rawnormalize.java hosted with ❤ by GitHub

The last piece of code is the cleanup for when we press enter to kill the program and center our servos. This code looks like this:

	public void cleanup(){
	try {
	JSONObject payload = new JSONObject();
	JSONObject left = new JSONObject();
	JSONObject right = new JSONObject();
	left.put(“left_yaw“, 400);
	left.put(“left_pitch“,400);
	right.put(“right_yaw“, 400);
	right.put(“right_pitch“,400);
	payload.put(“left_hand“, left);
	payload.put(“right_hand“, right);
	this.pubnub.publish(CHANNEL, payload, new Callback() {});
	} catch (JSONException e){
	e.printStackTrace();
	}
	}

view rawcleanup.java hosted with ❤ by GitHub

Try running the program and check out the PubNub debug console to view what the Leap Motion is publishing. If all is correct, you should see JSON that looks something like this:

{
    "right_hand":{
                    "right_yaw":450,
                    "right_pitch":300
                },
    "left_hand":{
                    "left_yaw":450,
                    "left_pitch":300
                }
}

Now that the Leap Motion is publishing data, we need to do is set up our Raspberry Pi to subscribe and parse the retrieved data to drive and control the servos.

Controlling Servos With A Raspberry Pi

Out of the box, Raspberry Pi has native support for PWM. However, there is only one PWM channel available to users at GPIO18. In this project, we need to drive 4 Servos simultaneously, so we will need a different solution.

Thankfully, the Raspberry Pi has HW I2C available, which can be used to communicate with a PWM driver like the Adafruit 16-channel 12-bit PWM/Servo Driver. In order to use the PWM Servo Driver, the Pi needs to be configured for I2C (to do this, check out this Adafruit tutorial).

Don’t worry about having the Adafruit Cobbler, it’s not needed for this project. If you do this part correctly, you should be able to run the example and see the servo spin. The program needs to do the following things:

1. Subscribe to PubNub and receive messages published from the Leap Motion
2. Parse the JSON
3. Drive the servos using our new values

Connecting the Raspberry Pi, PWM Driver and Servos

Begin by connecting the PWM driver to the Raspberry Pi, and then the servos to the PWM driver. Check out the schematic below: First, note that an external 5V power source for the servos is required. The Raspberry Pi can’t draw enough power to power all four servos, so an external 5V power source needs to be attached. Second, in this schematic, we attached a servo to channel 1 of the PWM driver. In the code, we skip channel 0 and use channels 1-4. In the project, set up the servos like so:

• Channel 1 is Left Yaw
• Channel 2 is Left Pitch
• Channel 3 is Right Yaw
• Channel 4 is Right Pitch

 

Setting up your Raspberry Pi with PubNub Begin by getting all the necessary imports for the project by adding the following:

 from Pubnub import Pubnub                                                                                                                            
 from Adafruit_PWM_Servo_Driver import PWM

 import RPi.GPIO as GPIO
 import time
 import sys, os
 import json, httplib
 import base64
 import serial
 import smbus

 GPIO.setmode(GPIO.BCM)

Also, add the following code which will subscribe to PubNub, and also defines all of the callbacks:

	#Kill PubNub subscription thread
	def _kill(m, n):
	pubnub.unsubscribe(subchannel)
	# ==============================Define Main==================================
	def main():
	print(“Starting Main“)
	pubnub = Pubnub(publish_key = ‘pubnub_publish_key‘,
	subscribe_key = ‘pubnub_subscribe_key‘,
	uuid = “pi“)
	channel = ‘my_channel‘
	def _callback(m,n):
	# ==============================Left Hand==============================
	left_hand = m.get(“left_hand“,{})
	if left_hand != {}:
	handleLeft(left_hand)
	# ==============================Right Hand=============================
	right_hand = m.get(“right_hand“,{})
	if right_hand != {}:
	handleRight(right_hand)
	#Catch and Print Error
	def _error(m):
	print(m)
	#Subscribe to subchannel, set callback function to _callback and set error fucntion to _error
	pubnub.subscribe(channels=channel, callback=_callback, error=_error, connect=_error)
	# ==============================Call Main====================================
	main()

view rawmain.py hosted with ❤ by GitHub

In the callback, we call handleLeft and handleRight which will parse the JSON object and use the values to drive our servos. In order for this to work, we need to initialize the PWM device.

We do all of this like so:

	def handleLeft(left_hand):
	left_yaw = left_hand[“left_yaw“]
	left_pitch = left_hand[“left_pitch“]
	pwm.setPWM(1, 1, left_yaw)
	pwm.setPWM(2, 2, left_pitch)
	def handleRight(right_hand):
	right_yaw = right_hand[“right_yaw“]
	right_pitch = right_hand[“right_pitch“]
	pwm.setPWM(3, 3, right_yaw)
	pwm.setPWM(4, 4, right_pitch)
	# ==============================Servo & PWM Set Up==============================
	# Initialise the PWM device using the default address
	#pwm = PWM(0x40, debug=True)
	pwm = PWM(0x40)
	servoMin = 150 # Min pulse length out of 4096
	servoMax = 600 # Max pulse length out of 4096
	pwm.setPWMFreq(60) # Set frequency to 60 Hz

view rawPWM.py hosted with ❤ by GitHub

That is all we need to do! Go ahead and power up your Raspberry Pi and Leap Motion and try it out for yourself! The project used a custom built driver to speak to our 8×8 LED matrices. For instructions on how to set that up, look here.

Wrapping Up

Although this is where our tutorial ends, you can certainly use the code and take this project even further, to basically control any servos in realtime from anywhere on Earth. We have a ton of tutorials on Raspberry Pi, remote control, and the Internet of Things in general. Check them out!

UPDATE: Apparently the people at IoTEvolution Battle of the Platforms though this demo was pretty cool, and we took home the “Best Enterprise Support Solution” award after our live demonstration!

Share This:

View more at: http://bit.ly/1XReQFr