Introduction: Hand Tracking Mechanical Arm - Pyduino + Leap Motion
Hello!
In this tutorial I will show you guys and gals how to assemble a hand tracking mechanical arm with a Leap Motion Controller, Arduino device, a few motors, some balsa wood, paper clips, hot glue and a little python code. At the end of this you'll hopefully have a mechanical arm which we can control using a Leap Motion Controller. I will walk you guys through an in depth process on how to hook up your Leap Motion controller to get just one servo motor working and hopefully when you see how its done with one motor you can just copy and paste the code around a few times to make it work with 4. Of course I'll help you along the way and don't be shy about asking any questions you have along the way. I aim to provide a sufficient enough description of how this all works so that even if you're not a Python expert you can use the same process I take in my python code and apply it to other languages such that you can communicate with your Arduino device and Leap Motion controller.
This project is composed of several parts that we'll work through separately and then combine at the end.
The Steps
Project Theory + Explanation
Materials List
Setting up Pyduino Library
Setting up the Pyduino Sketch
Setting up just one servo motor (Circuit)
Controlling the servo using Pyduino
Setting up the Leap Motion Controller
Linking the Leap motion Controller to one servo motor
Assembling the arm
Working out the final circuit
Testing, Debugging and Optimizing the code
Step 1: Project Theory + Explanation
The first part and maybe most foreign part of this project to some is the hand tracking part of things. Luckily we do not have to develop the hand tracking software or hardware ourselves. Our good friends at Leap Motion have invented a very nifty controller for doing just what we want. We want to be able to control our arm without touching any sort of controller and we can do just that with a Leap Motion Controller. If you are unfamiliar with this controller please head over to the Leap Motion site and check out a few of their videos https://www.leapmotion.com/. The Leap Motion controller is composed of 3 infrared LEDS that emit around 850 nanometers and it has 2 IR cameras to capture motion. The idea of the controller is that the light from the IR LEDS reflects off our hands and gets captured by the two IR cameras on the device. The device then streams the data it captured to the Leap Motion tracking software on your computer. The tracking software probably uses some sort of parallax effect and other algorithms to reconstruct a 3D representation of what the device sees. The tracking algorithm then interprets the data such as what is the orientation and position of your hands and fingers. After the algorithm determines to the best of its ability where your hand is, the data is exported to the API which is ready for you to interface with and pull data from. The neat thing is the Leap Motion controller can interface with a variety of programming languages so if don't know Python or can't decipher my code you can use your own programming language of choice.
We will end up writing a little piece of software that listens for the data from the Leap Motion controller. Once we receive the data we will normalize it by the size of the Leap Motion controller's interaction area so that we can obtain a fraction of how far in the space our hand is. Once we have that fraction we can multiply it by the range of some motor and tell our Arduino to move our servo motor to that position. We will be able to synchronize our hand motion in 3 dimensions to 3 different motors. Moving around each axis will control a different motor. That makes sense because when we move our hand up further away from the Leap Motion controller we want that to move our arm up, which we can make happen.
If you're familiar with how an Arduino works you know that we have to upload a sketch onto the board before we can make use of the device. In order for us to dynamically interact with the Arduino board we need to write a little piece of software such that we can control our Arduino without having to constantly keep uploading sketches onto it. Luckily I have a little piece of code that will facilitate this process. I wrote two previous instructable tutorials on how to interface your Arduino with Python over serial communication. That's just a fancy way of saying we're going to have our Python code send a little message to our Arduino device and then our Arduino will interpret the message and then perform sent task. Don't worry I'll walk you guys through a basic example of how to set up the Pyduino library on your Arduino device and on your computer so that we can start talking to our Arduino. And if you don't know Python I have a solution for you! The sketch that we upload onto our Arduino device is versatile enough to interface with using any language capable of sending serial messages so if don't want to use Python to send messages to our Arduino you don't have to! But you will have to write a little extra code.
Let me direct your attention now to the figure above. Lets define some terminology so you can all understand what I'm talking about later. At the base of our arm we have 2 servo motors, we will call these motors the Azimuthal motor/angle and Altitude motor/angle. If you are up to date on your observational astronomy terminology the Azimuthal angle typically refers to some cardinal direction (N,W,E.S) so this is the motor that will move left and right and the Altitude angle is that which starts at the horizon and extends upward to some star. The primary arm is that which is attached to the Altitude motor. The secondary arm is attached to what I like to call the Wrist motor which is attached to the primary arm. Lastly we have The Claw. (insert Toy story clip here) The claw is responsible for grabbing things and is operated by a motor as well.
Step 2: Materials List
Get yo mats here!
Materials
Computer with Python installed (https://www.python.org/)
Leap Motion Controller
Arduino Uno (or other arduino device)
USB Cable
Breadboard
Lots of wires
2 Parallax Standard Servo Motors
2 Micro Servos (SG-90)
Balsa Wood Pack (I got mine at Michaels, picture attached)
Hot Glue
Paper Clips
Pliers
X-acto Knife
Sand Paper
Most of these supplies you can pick up at a Micro Center shop or online. For building the structure you shouldn't need to go to Home Depot or Lowes, you can pick up most of the supplies at your local craft shop or Michaels.
Balsa wood and hot glue is a stronger combination than you may think. The balsa wood really helps us keep the torque on the servos due to the structure at a minimum compared to using aluminum or denser wood. It also makes the servos more responsive since there is less weight to tug around.
Step 3: Setting Up Pyduino Library
In order for us to be able to move our motors on the fly we need to be able to tell our Arduino device; "Hey! Move this motor to this angle yo!" This can be done by sending a message through the USB port to our Arduino. For a basic overview on how serial communication works between Arduino and python check out http://playground.arduino.cc/interfacing/python. Before we begin let me inform you on some of the limitations that python has when creating Arduino projects. Python does not create Arduino sketches, nor are you going to be uploading more than 1 sketch to your board this entire time. Instead, python is going to be sending small strings (only a few chars) to our Arduino board, our Arduino board will have a preloaded sketch associated with the pyduino library to interpret our small message then perform said task. The nifty thing about the sketch we'll upload to our Arduino board is that it can be used with any programming language capable of sending serial messages. So if you don't like Python you can use another language
.
Check out the code below: You will want to save the code below to a file called: pyduino.py
You'll want to put that file in the directory you're working in. Take a moment to read over the piece of code and see what it does. There is also a piece of sample code at the bottom of the library showing you how to use it which I'll get into next. If you would like to see a working example of how to use the library to control something other than a servo motor check out my other instructables.
If you do not want to use Python then you'll have to take a moment and transcribe this piece of code to another language. For more information about the pyduino library check out https://github.com/lekum/pyduino
"""
A library to interface Arduino Servos through serial connection """ import serialclass Arduino(): """ Models an Arduino connection """
def __init__(self, serial_port='/dev/ttyACM0', baud_rate=9600, read_timeout=5): """ Initializes the serial connection to the Arduino board """ self.conn = serial.Serial(serial_port, baud_rate) self.conn.timeout = read_timeout # Timeout for readline() print 'Connection initiated' def servo_write(self, pin_number, digital_value): """ Writes the digital_value on pin_number Internally sends b'WS{pin_number}:{digital_value}' over the serial connection """ command = "WS{}:{}".format(str(pin_number),str(digital_value)).encode() self.conn.write(command)
def close(self): """ To ensure we are properly closing our connection to the Arduino device. """ self.conn.close() print 'Connection to Arduino closed'
Step 4: Setting Up Pyduino Sketch
In order for us to dynamically control our Arduino device we need to upload a sketch on it so that it can interpret the messages we send it from our Python code. The set up of this sketch is pretty straightforward. The Arduino device will communicate with our computer over the serial port. For more information on Serial functions for Arduino check out: http://www.arduino.cc/en/Reference/Serial . The Arduino device will check to see if it has any characters available in the serial receive buffer and if it does it will being to parse the incoming message. Upon receiving the whole message the device will then interpret what to do whether it be a read function, write function or setting a pin mode. This is sketch is versatile enough to be able to set pin modes, perform read and write of digital and analog values as well as perform servo writes. Any additional functions will need to be coded in yourself but for the purposes of this project we do not need to add anything.
Check out the piece of code below. This is what we're going to use to get one servo motor working with the Leap Motion Controller. We want to be able to get one servo working before we go ahead and get all 4 in there. This file is also available on the github page for this instructable at: https://github.com/theown1/pyduino_leapmotion/blob...
You can go ahead and upload this sketch to your Arduino device. When you upload it, you should get a picture that looks similar to the one above. One thing you might need to write down is the location of your Arduino device on your computer, you can find this at the bottom right of the arduino program after you upload a sketch. For me my arduino is located at: /dev/ttyACM0
/*
* Sketch to control the pins of Arduino via serial interface * * Commands implemented with examples: * * - RD13 -> Reads the Digital input at pin 13 * - RA4 - > Reads the Analog input at pin 4 * - WD13:1 -> Writes 1 (HIGH) to digital output pin 13 * - WA6:125 -> Writes 125 to analog output pin 6 (PWM) */#include
char operation; // Holds operation (R, W, ...) char mode; // Holds the mode (D, A) int pin_number; // Holds the pin number int digital_value; // Holds the digital value int analog_value; // Holds the analog value int value_to_write; // Holds the value that we want to write int wait_for_transmission = 5; // Delay in ms in order to receive the serial data
// create servo object to control a servo // a maximum of eight servo objects can be created Servo SERVO2; int SERVO2_PIN = 2; // this is where the servo is attached to on our board
void setup() { Serial.begin(9600); // Serial Port at 9600 baud Serial.setTimeout(500); // Instead of the default 1000ms, in order // to speed up the Serial.parseInt() SERVO2.attach(SERVO2_PIN); SERVO2.write(180); // reset to original position
}
void servo_write(int pin_number, int servo_value){ /* * Performs a servo write on pin_number with the servo_value * The value must be 0 to 180 (might change depending on servo) */ if (pin_number==SERVO2_PIN) { SERVO2.write(servo_value); delay(10); }
}
void loop() { // Check if characters available in the buffer if (Serial.available() > 0) { // parse information // courtesy of lekum operation = Serial.read(); delay(wait_for_transmission); // If not delayed, second character is not correctly read mode = Serial.read(); pin_number = Serial.parseInt(); // Waits for an int to be transmitted if (Serial.read()==':') { value_to_write = Serial.parseInt(); // Collects the value to be written }
// if we recieve proper input write servo if (operation == 'W') { if (mode == 'S') { servo_write(pin_number, value_to_write); } } } }
Step 5: Setting Up Just One Servo Motor (Circuit)
Alright so if you had the chance to look over the Arduino sketch on the previous step you may have noticed that we predefined a pin for our Servo Motor, Pin 2. Go ahead and set up your arduino with any one of your servo motors following the circuit schematic above. If you're using an SG-90 micro servo like I am you may have noticed that the wire colors do no match those above. Typically the darker colored wire will be the ground, so for me that wire is brown. Power will always be the middle wire which leaves us with just one wire left that we wire into one of our digital pins. We'll use that digital pin to tell the servo where to move.
Alright now that our circuit is set up its time to get this motor moving using python!
Step 6: Controlling the Servo Using Pyduino
Make sure your Arduino device is plugged into your computer. Before we get our Leap Motion controller set up we want to make sure we can successfully control one servo with our Pyduino library. We're going to follow an example similar to the sweep example on the Arduino site however just with different pin wirings. Our python code is pretty simple, we're going to import the pyduino library so that we can establish a serial connection to our Arduino device and then its as simple as using 2 lines of code to move our servo.
Save the piece of code below to a file called one_servo_test.py and make sure it is in the same directory as your pyduino.py file.
And then you can run it through the terminal by typing $ python one_servo_test.py
Don't worry the program will lag a little bit in the beginning which ensures a connection to your Arduino device and then you should see your servo start to move! The code should be documented enough for you to figure out everything that is going on. If you're unable to establish a connection to your arduino device you'll recieve an error that says something like: [serial.serialutil.SerialException: could not open port /dev/ttyACM0: [Errno 2] No such file or directory: '/dev/ttyACM0'] Then you'll need declare your arduino device in the code using a different serial port. The serial port for your Arduino device can be found at the bottom right of the Arduino software after you upload a sketch to your board. Another complication that you may run into is that if your servo is not wired up to PIN #2 on your board then you'll need to either change your wiring or change the Arduino sketch we uploaded onto our board, check the code documentation for how and where to do that.
from pyduino import *
import timeif __name__ == '__main__': # if your arduino was running on a serial port other than '/dev/ttyACM0/' # declare: a = Arduino(serial_port='/dev/tty') a = Arduino()
# sleep to ensure ample time for computer to make serial connection time.sleep(3)
# declare the pin our servo is attached to # make sure this matches line 26 of one_servo.ino # the line that says: int SERVO2_PIN = 2; PIN = 2
try: for i in range(0,1000): if i%2 == 0: print '180' # move servo on pin to an angle of 170 deg a.servo_write(PIN,170) else: print '10' # move servo on pin to an angle of 10 deg a.servo_write(PIN,10)
time.sleep(1)
except KeyboardInterrupt: # reset position of servo to 90 deg and close connection a.servo_write(PIN,90) a.close()
Step 7: Setting Up the Leap Motion Controller
If this is your first time using the Leap Motion controller I'll show you how to set up the files we need to be able to use the controller effectively. I am running all of this code from an Ubuntu machine so for those Windows and Mac users out there your experience may be slightly different. I'll guide you through installing the development kit from the Leap Motion developer site and then hopefully when you plug our hardware in it will work. Afterwards we can start to make our controller do custom things like control our servo motor! P.S. If you already have the Leap Motion SDK installed go to the Library section of this step to learn how to set up our first project directory.
Head over to: https://developer.leapmotion.com/ You'll need to sign in before you can download the SDK for your machine. For LInux: you will have to untar the SDK using a command like "tar -xvzf LeapDeveloperKit...." After extracting the Dev Kit read the "README" file for instructions on how to install the linux dependencies for Leap and then check the instructions on how to install the SDK afterwards. If you're not running on a linux machine read the "README.txt" file and it will have instructions on how to install the software for Windows and Mac.
Depending on which version of the API you're going use, check out https://developer.leapmotion.com/documentation/ind... for all the available platforms. The API for each language is well documented. Take a moment to read through the API of your choice a little bit so you can get a better understanding of how the Leap Motion Controller and software works together. Hopefully you were able to to install everything correctly. As your attorney I advise you to test your installation now before continuing. Plugging your LeapMotion device into your computer will lead to the OFF picture above. Simply running the visualizer while the controller is off will result in no output from the device and show no visuals. So to combat this issue we need to start the "leapd" service. On a linux machine this can done by typing "sudo leapd" into your terminal and letting that process run. After you do that you should see an output that says something like WebSocket server started, Leap Motion Controller detected and Firmware is up to date. If you do not see those outputs take a moment to consult the all mighty google about any issues you're having. I have to admit that I had some troubles when I first installed this controller and was able to find answers on google and stack overflow. After you get the leapd service running your Leap Device will turn on and you should be able to see the 3 LEDs light up. (See ON image above) You should now be ready to use the Visualizer. When we're testing and creating code we want to make sure the leapd service is always running otherwise we can't use our device.
LIBRARY SECTION
Before we can make a piece of code we'll need to set up the libraries for our project. To see which libraries you need to copy from the SDK tar ball above check out this link: https://developer.leapmotion.com/documentation/pyt... (You'll need a different link if you're not using the Python API) For our python code we'll need to go to the directory: LeapDeveloperKit_*_linux/LeapSDK/lib/ to find the libraries. Afterwards you'll need to copy the Leap.py file and the two shared object files in the x64 or x86 directory to whichever directory your other pieces of code are in.
In your working directory you should now have the following files
one_servo.ino # arduino sketch for serial interfacing with one servo
one_servo_test.py # python code to test one servo w.o leapmotion
pyduino.py # serial interfacing library for arduino
LeapLib/ # directory with Leap Motion libraries
Leap.py # these are from Leap SDK
LeapPython.so
libLeap.soIn the next step we'll set up our first Leap Motion script to stream data from the device and then use the data to move our one servo motor.
Step 8: Linking the Leap Motion Controller to One Servo Motor
Now that you hopefully have your Leap Motion device up and running we are going to create our first program to stream data from the device and then we're going to link it up to our servo motor with the pyduino library. Any time we run a script we need to have the leapd service running. To know that this service is running you should be able to see the 3 illuminated LEDs on your Leap device. (See ON image in previous step)
Our first Leap script will be composed of several key components. There is a controller class which connects to the Leap Device and a listener class which will get the data from the tracking software. For an in depth tutorial on how to set up a simple Leap script check out: https://developer.leapmotion.com/documentation/pyt...
I'll give a brief overview of what the piece of code below does and how it works. In the main part of the code we initialize an instance of our listener and controller classes afterwards we add the custom listener to our controller. The important part of the script is what occurs within the listener. The listener is a class that will obtain data from the Leap device and assess what to do with the information being streamed. The listener has the ability to obtain 60 frames a second from the device however that is a little too much information for what we want. You're certainly welcome to try and use the 60 frames a second to control your Arduino however keep in mind it takes a small amount of time to send a message to the Arduino device and have it respond. To fix this issue we're only going to work with 10 frames a second instead of 60. The listener has a few basic functions: On Initialize, On Connect, On Disconnect, On Exit, On Frame. They're all pretty self explanatory from the names but lets go through them one by one and explain how we're going to use them to control our servo.
On Initialize - Here we want to declare our Arduino and establish a serial connection to it
On Connect - We can enable some gestures for the controller, no need to do anything with the Arduino here
On Disconnect - Nothing really here.
On Exit - We are going to reset the servo position and close the connection to our Arduino device
On Frame - The first thing to do is get the time because we only want to get a frame every 10ms or so. Once we obtain the frame from the controller it will tell us which hand is in the frame and what its position is. From this we can extract finger and joint positions as well. We are going to use the normalized position of our hand in the interaction box and multiply that by the range on our motor. For instance if our hand is all the way left, the normalized position will be 0 and so the angle on our servo will be 0 as well. For more insight you can read up on the interaction box in the Leap API documentation: https://developer.leapmotion.com/documentation/pyt... Another useful link to look at is the one for the frame because the frame stores all our information about the hand and positions of everything. To see what information is accessible within our frame check out this link: https://developer.leapmotion.com/documentation/pyt...
We are going to use the same Arduino circuit that we created earlier. If you happened to change the servo pin to something other than 2 you will need to modify the code below (line 17). If you are unable to get the Arduino part working make sure you have the serial connection on the correct port (line 23). Lines 78,82 are what control the servo motor. When you run the code take some time to move your hand around and see what gets output. The position of your hand will be output in terms of the normalized position in the interaction box. Before you run this code make sure that your leapd service is running and that your Arduino is plugged into your computer.
my_first_leap.py
# Simple Leap motion program to track the position of your hand and move one servo
# import the libraries where the LeapMotionSDK is import sys sys.path.insert(0, "LeapLib/")import Leap, thread, time from Leap import CircleGesture, KeyTapGesture, ScreenTapGesture, SwipeGesture from pyduino import *
class SampleListener(Leap.Listener): oldtime = time.time() newtime = time.time()
# FIXME if servo is not attached to pin 2 SERVO_PIN = 2 # Azimuthal Servo motor pin AZIMUTHAL_LIMIT = 180 # we want our motor to go between 0 and 180
def on_init(self, controller):
# if your arduino was running on a serial port other than '/dev/ttyACM0/' # declare: a = Arduino(serial_port='/dev/ttyXXXX') self.a = Arduino() # sleep to ensure ample time for computer to make serial connection time.sleep(3) print "Initialized"
def on_connect(self, controller): print "Connected"
# Enable gestures controller.enable_gesture(Leap.Gesture.TYPE_CIRCLE); controller.enable_gesture(Leap.Gesture.TYPE_KEY_TAP); controller.enable_gesture(Leap.Gesture.TYPE_SCREEN_TAP); controller.enable_gesture(Leap.Gesture.TYPE_SWIPE);
def on_disconnect(self, controller): # Note: not dispatched when running in a debugger. print "Disconnected"
def on_exit(self, controller): # Reset servo position when you stop program self.a.servo_write(self.SERVO_PIN,90) self.a.close()
print "Exited"
def on_frame(self, controller):
# we only want to get the position of the hand every so often self.newtime = time.time() if self.newtime-self.oldtime > 0.1: # if difference between times is 10ms
# Get the most recent frame and report some basic information frame = controller.frame() interaction_box = frame.interaction_box # print some statistics print "Frame id: %d, timestamp: %d, hands: %d, fingers: %d, tools: %d, gestures: %d" % ( frame.id, frame.timestamp, len(frame.hands), len(frame.fingers), len(frame.tools), len(frame.gestures()))
# Get hands for hand in frame.hands:
handType = "Left hand" if hand.is_left else "Right hand" normalized_point = interaction_box.normalize_point(hand.palm_position,True) print " %s, id %d, x-position: %s" % (handType, hand.id, normalized_point.x ) print " %s, id %d, y-position: %s" % (handType, hand.id, normalized_point.y ) print " %s, id %d, z-position: %s" % (handType, hand.id, normalized_point.z )
# FIXME depending on orientation of servo motor # if motor is upright, Leap Device will register a 0 degree angle if hand is all the way to the left XPOS_servo = abs(AZIMUTHAL_LIMIT-normalized_point.x*AZIMUTHAL_LIMIT) print " Servo Angle = %d " %(int(XPOS_servo)) # write the value to servo on arduino self.a.servo_write(self.SERVO_PIN,int(XPOS_servo)) # turn LED on
# update the old time self.oldtime = self.newtime else: pass # keep advancing in time until 10 millisecond is reached
def main():
# Create a sample listener and controller listener = SampleListener() controller = Leap.Controller()
# Have the sample listener receive events from the controller controller.add_listener(listener)
# Keep this process running until Enter is pressed print "Press Enter to quit..." try: sys.stdin.readline() except KeyboardInterrupt: pass finally: # Remove the sample listener when done controller.remove_listener(listener)
if __name__ == "__main__": main()
Step 9: Assembling the Arm
Hopefully you can see how we're going to set up the rest of the project from our little example with moving just one servo. If you got the one servo working with the Leap Motion controller we pretty much have the rest of the project in the bag. All we need to do is set the ranges for our other servos and then tell the Arduino to move those servos and that's pretty much the rest of the coding that's required.
Unfortunately I do not have solid instructions on how to assemble the arm because it was a trial and error process for myself and I think it will be a good puzzle for you guys too :) I'll do my best to explain the design and show pictures of each arm but it will be up to you guys to do the assembly. I used only Balsa wood, hot glue and paper clips to assemble my arm. You certainly aren't restricted to creating it how I have, feel free to use your own judgement on this one. Some of the servo motors are screwed into the balsa wood for extra support, I use screws that are about as big as the ones you would use to screw an arm into one of your servo motors.
Step 10: Working Out the Final Circuit
The final circuit looks a little complicated but its really not. We only have a total of 6 pins attached to our arduino device when we use the breadboard. You will have 4 digital pins (2,3,4,5) occupied and then a power and ground one. On the breadboard you'll see 4 main rows of wires, each row corresponds to a single servo. The power for each servo motor is attached to the power column on the breadboard which is wired to the 5V out on the arduino. Remember to put the breadboard close to the arm so the wires connected to the servo are long enough to reach. I trust you guys to be able to wire in 4 servos, its not rocket science :p
Before you can use the Leap Motion to control all four servos we need to upload a new sketch onto our Arduino device that will let us do that. At the moment our current sketch can only support one motor. Check out the sketch below, if you are not planning on using pins 2,3,4,5 to control your servo motors then you will need to change lines 30,31,32,33.
https://github.com/theown1/pyduino_leapmotion/blob...
/*
* Sketch to control the servo pins of Arduino via serial interface * */#include
char operation; // Holds operation (R, W, ...) char mode; // Holds the mode (D, A) int pin_number; // Holds the pin number int digital_value; // Holds the digital value int analog_value; // Holds the analog value int value_to_write; // Holds the value that we want to write int wait_for_transmission = 5; // Delay in ms in order to receive the serial data
// create servo object to control a servo // a maximum of eight servo objects can be created Servo SERVO2; Servo SERVO3; Servo SERVO4;
int SERVO2_PIN = 2; // azimuth angle int SERVO3_PIN = 3; // altitude angle int SERVO4_PIN = 4; // wrist angle
void setup() { Serial.begin(9600); // Serial Port at 9600 baud Serial.setTimeout(500); // Instead of the default 1000ms, in order // to speed up the Serial.parseInt() SERVO2.attach(SERVO2_PIN); SERVO2.write(180); // reset to original position
SERVO3.attach(SERVO3_PIN); SERVO3.write(90); // reset to original position // servo3 limit 0-85
SERVO4.attach(SERVO4_PIN); SERVO4.write(90); // reset to original position }
void servo_write(int pin_number, int servo_value){ /* * Performs a servo write on pin_number with the servo_value * The value must be 0 to 180 (might change depending on servo) */ if (pin_number==SERVO2_PIN) { SERVO2.write(servo_value); delay(10); } else if (pin_number == SERVO3_PIN) { SERVO3.write(servo_value); delay(10); } else if (pin_number == SERVO4_PIN) { SERVO4.write(servo_value); delay(10); }
}
void loop() { // Check if characters available in the buffer if (Serial.available() > 0) { // parse information // courtesy of lekum operation = Serial.read(); delay(wait_for_transmission); // If not delayed, second character is not correctly read mode = Serial.read(); pin_number = Serial.parseInt(); // Waits for an int to be transmitted if (Serial.read()==':') { value_to_write = Serial.parseInt(); // Collects the value to be written }
// if we recieve proper input write servo if (operation == 'W') { if (mode == 'S') { servo_write(pin_number, value_to_write); } } } }
Step 11: Testing+Debugging the Code
Alright hopefully you have successfully assembled an arm or at least have four servos connected to your Arduino device which are ready to test. We're going to set the code up much like how we set it up with just one servo. Getting the claw work will involve a little more than what we did early but its still not too hard. For the claw all we're going to do is calculate a normalized distance between our pointer finger and thumb. To find the distance between our two points we will need to know a little vector arithmetic. We're going to find the difference between the vector pointing to our thumb and pointer finger. Afterwards we can calculate the distance/magnitude by finding the 2norm which you can sort of think of as a 3D Pythagorean theorem. To see how this is done check out lines 91 to 107 in the code below. If you have any comments about the code feel free to ask but it should be fairly straight forward if you've been able to follow along so far.
If you can't get the code to work make sure you have your leapd service running and your arduino device connected to your computer. You can also try to add in the control for the four servos one servo at a time if you got the code in the previous step to work.
Optimizing the code
There are a few tweaks to make like the normalization limit for your pointer and thumb finger as well as the ranges on the servos. I set the ranges on the servo based on how I attached the motors to the arm because I didn't want the motors moving the the arm into the ground or into the Arduino device itself. You can find the limit for the normalization between the thumb and pointer finger on line 104. Also the ranges for the motors can be found on lines 118,119,120.
The code can also be found here: https://github.com/theown1/pyduino_leapmotion/blob...
# Simple Leap motion program to track the position of your hand
# import the libraries where the LeapMotionSDK is import sys sys.path.insert(0, "LeapLib/")import Leap, thread, time from Leap import CircleGesture, KeyTapGesture, ScreenTapGesture, SwipeGesture from pyduino import * import numpy as np
class SampleListener(Leap.Listener): finger_names = ['Thumb', 'Index', 'Middle', 'Ring', 'Pinky'] bone_names = ['Metacarpal', 'Proximal', 'Intermediate', 'Distal'] state_names = ['STATE_INVALID', 'STATE_START', 'STATE_UPDATE', 'STATE_END'] oldtime = time.time() newtime = time.time()
def on_init(self, controller):
# if your arduino was running on a serial port other than '/dev/ttyACM0/' # declare: a = Arduino(serial_port='/dev/ttyXXXX') self.a = Arduino() # sleep to ensure ample time for computer to make serial connection time.sleep(3) # Define Pins for Arduino Servos self.PIN2 = 2 # Azimuthal self.PIN3 = 3 # Altitude self.PIN4 = 4 # Wrist self.PIN5 = 5 # Claw self.previous_angles = [0,0,0,0]
# allow time to make connection time.sleep(1)
print "Initialized"
def on_connect(self, controller): print "Connected"
# Enable gestures controller.enable_gesture(Leap.Gesture.TYPE_CIRCLE); controller.enable_gesture(Leap.Gesture.TYPE_KEY_TAP); controller.enable_gesture(Leap.Gesture.TYPE_SCREEN_TAP); controller.enable_gesture(Leap.Gesture.TYPE_SWIPE);
def on_disconnect(self, controller): # Note: not dispatched when running in a debugger. print "Disconnected"
def on_exit(self, controller): time.sleep(1) # Reset arduino when you stop program self.a.servo_write(self.PIN2,90) # Az self.a.servo_write(self.PIN3,0) # Alt self.a.servo_write(self.PIN4,100) # Wrist self.a.servo_write(self.PIN5,70) # Claw self.a.close()
print "Exited"
def on_frame(self, controller):
# we only want to get the position of the hand every so often self.newtime = time.time() if self.newtime-self.oldtime > 0.1: # every 10 ms get a frame
# Get the most recent frame and report some basic information frame = controller.frame() interaction_box = frame.interaction_box
print "Frame id: %d, timestamp: %d, hands: %d, fingers: %d, tools: %d, gestures: %d" % ( frame.id, frame.timestamp, len(frame.hands), len(frame.fingers), len(frame.tools), len(frame.gestures()))
# Get hands for hand in frame.hands:
handType = "Left hand" if hand.is_left else "Right hand" normalized_point = interaction_box.normalize_point(hand.palm_position,True)
self.XPOS = normalized_point.x self.YPOS = normalized_point.y self.ZPOS = normalized_point.z print " %s, id %d, x-position: %s" % (handType, hand.id, int(self.XPOS*180) ) print " %s, id %d, y-position: %s" % (handType, hand.id, int(self.YPOS*85) ) print " %s, id %d, z-position: %s" % (handType, hand.id, int(self.ZPOS*180) )
print 'my fingers =',len(hand.fingers) if len(hand.fingers) >= 2: x1 = hand.fingers[0].tip_position[0] y1 = hand.fingers[0].tip_position[1] z1 = hand.fingers[0].tip_position[2] # calc distance between two fingers x2 = hand.fingers[1].tip_position[0] y2 = hand.fingers[1].tip_position[1] z2 = hand.fingers[1].tip_position[2] # calc 2norm for difference between vector to thumb and pointer finger r = ( (x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2 )**0.5
# perform a crude normalization dist_norm = r/100. # may need to change the 100 to something else
print 'Finger Tip Distance = ',dist_norm # not really a normalized position, sometimes can be more than 1 if dist_norm >= 1: dist_norm=1 #for finger in hand.fingers: #print finger,' - ',finger.tip_position
# determine motors - adjust angle ranges here too XPOS_servo = abs(145-self.XPOS*145) # 0-Azimuth YPOS_servo = abs(85-self.YPOS*85) # 1-Altitude ZPOS_servo = 35+135*self.ZPOS # Wrist Angle # write the value to servo on arduino self.a.servo_write(self.PIN2,int(XPOS_servo)) # Azimuth self.a.servo_write(self.PIN3,int(YPOS_servo)) # Altitude self.a.servo_write(self.PIN4,int(ZPOS_servo)) # Wrist
# claw range CLAW_SERVO = abs(90-dist_norm*70) print 'Claw Angle =',CLAW_SERVO self.a.servo_write(self.PIN5,int(CLAW_SERVO))
# update the old time self.oldtime = self.newtime
# update previous values self.previous_angles[0] = XPOS_servo self.previous_angles[1] = YPOS_servo self.previous_angles[2] = ZPOS_servo self.previous_angles[3] = CLAW_SERVO else: pass # keep advancing in time until 1 second is reached
def main(): # Create a sample listener and controller listener = SampleListener() controller = Leap.Controller()
# Have the sample listener receive events from the controller controller.add_listener(listener)
# Keep this process running until Enter is pressed print "Press Enter to quit..." try: sys.stdin.readline() except KeyboardInterrupt: pass finally: # Remove the sample listener when done controller.remove_listener(listener)
if __name__ == "__main__": main()
Participated in the
Move It
