Boca Bearings Blog 2017

Tags

Archives

3D Printed Robotic Hand: Part 5 - Programming and Debugging

Oct 20, 2017

Gerardo Ramos


 

Now that I have finished building the hand and the glove, it was time to start writing the programs for the hand and glove. For the glove, I simply just used the code from an Instructables. It was created by Gabry295. The link to the instructable is:

 

http://www.instructables.com/id/Wireless-Controlled-Robotic-Hand/?ALLSTEPS

 

There you can find the code for the glove. I went through a few variations of the code but I just eventually just kept the code the way it was.

 

The code is as follows:

 

/*
Flex Glove

Created by Santin Gabriele, 2014
I.T.S.T. "J. F. Kennedy", cl. 5^A EA
Thanks to Elias De Lamper for suggestions to improve this program!
*/

int ResThumb = A4; // Variables of the analog read form the flex sensors connected to the
int ResIndex = A3; // analog pins of Arduino LilyPad
int ResMiddle = A2;
int ResRing = A1;
int ResPinky = A0;

int OpenedThumb =0; // Variables of the values when the hand is completely opened
int OpenedIndex =0; // This is needed for a continuous calibration
int OpenedMiddle =0;
int OpenedRing =0;
int OpenedPinky =0;

int ClosedThumb; // Variables of the values when the hand is completely closed
int ClosedIndex; // We can't set it to zero since that the minimum value reached
int ClosedMiddle; // in the analog read never reach zero. We'll assign the value of
int ClosedRing; // a first analog read, then the program in the loop will
int ClosedPinky; // automatically assigning lower values

int thumb =0; // Variables of the values to send
int index =0;
int middle =0;
int ring =0;
int pinky =0;

void setup()
{
Serial.begin(9600); // Activating serial communication, XBee Series 1 are pre- // programmed at 9600 baud/s

pinMode(ResThumb, INPUT); // The variables of the sensor are set as input
pinMode(ResIndex, INPUT);
pinMode(ResMiddle, INPUT);
pinMode(ResRing, INPUT);
pinMode(ResPinky, INPUT);

ClosedThumb = analogRead(ResThumb);
ClosedIndex = analogRead(ResIndex); 
ClosedMiddle = analogRead(ResMiddle); 
ClosedRing = analogRead(ResRing);
ClosedPinky = analogRead(ResPinky); 
}

void loop()
{
thumb = analogRead(ResThumb); 
index = analogRead(ResIndex); 
middle = analogRead(ResMiddle); 
ring = analogRead(ResRing);
pinky = analogRead(ResPinky); 

if(thumb > OpenedThumb) // Calibration reading and setting the maximum values. This needs you to completely open your hand a few times
OpenedThumb = thumb;
if(index > OpenedIndex)
OpenedIndex = index;
if(middle > OpenedMiddle)
OpenedMiddle = middle;
if(ring > OpenedRing)
OpenedRing = ring;
if(pinky > OpenedPinky)
OpenedPinky = pinky;

if(thumb < ClosedThumb) // Calibration reading and setting the minimum values. This needs you to completely close your hand a few times
ClosedThumb = thumb;
if(index < ClosedIndex)
ClosedIndex = index;
if(middle < ClosedMiddle)
ClosedMiddle = middle;
if(ring < ClosedRing)
ClosedRing = ring;
if(pinky < ClosedPinky)
ClosedPinky = pinky;

// The analog read has to be readapted in values between 0 and 180 to be used by the // servomotors.
// The minimum and maximum values from the calibrations are used to correctly set the //analog reads.

thumb = map(thumb ,ClosedThumb ,OpenedThumb ,0,180); 
index = map(index ,ClosedIndex ,OpenedIndex ,0,180); 
middle = map(middle ,ClosedMiddle ,OpenedMiddle ,0,180);
ring = map(ring,ClosedRing,OpenedRing,0,180); 
pinky = map(pinky ,ClosedPinky ,OpenedPinky ,0,180);

Serial.write("<"); // This character represent the beginning of the package of the five // values
Serial.write(thumb); // The values are sent via the Tx pin (the digital pin 1)
Serial.write(index);
Serial.write(middle);
Serial.write(ring);
Serial.write(pinky);

delay(50);
}

 

This code worked pretty well for the glove. I did a Serial.println of the thumb, index, middle, ring, and pinky values to check if the numbers made sense. I tested it by wearing the glove and opening and closing my hand and checking if the those values change. It did, so I knew the code was working properly.

 

For the robotic hand part, I had some difficulties trying to get it to communicate with the glove. At first, I assumed that it was receiving some kinda of data from the glove so I decided to Serial.println the values that it was supposed to receive. But doing this and checking the values in the serial monitor in the Arduino program, I got nothing. Nothing popped up or any kinda of random values or letters. So I thought to myself that it wasn't working or maybe that the servos didn't work.

 

But to test the servos, I decided to run one of the examples of the servo shield. The example was called servo. I uploaded it to Arduino. When uploading, I have to have the xBee's off or the upload would not complete. After uploading the example, the servos would go to its minimum position to its maximum one at a time. So now I knew my servos were working.

 

So I went online and started to look up how to make sure the XBee's were communicating. After spending some time learning about the XBee's, I decided to check the product page of the XBee shield that I am using on Sparkfun.com. They had a guide on:

https://learn.sparkfun.com/tutorials/xbee-shield-hookup-guide?_ga=1.257884875.17886098.1427765704

 

There I learned that the XBee shield had a serial select switch at the bottom left.

 

robotic hand

 

So I just simply switched it to the UART side and the XBee starting to receive data from the glove's XBee! So now I was getting some kind of data from the glove. The next step was to make the servos move in response to the movements of the fingers of the glove. 

 

The hand program from Gabry295 didn't work for my robotic hand.I figured that it had to do with the shield that I am using. I am using the Adafruit PWM/Servo shield. The command servo.write(angle) was not working for my servos. So my next step was to research servos and PWM (Pulse Width Modulation).

 

I went online and tried to learn what PWM was and how it worked. I found it difficult to understand it by just reading about it. So, I just decided to look over Arduino codes that used PWM to control servos. After many trial and errors, I came across just a few codes that implemented PWM. But from those codes, I learned a little more about PWM and how to include it in your program. I came across a program from:

https://solderspot.wordpress.com/2013/12/19/adafruits-servo-shield/

 

that uses the following function:

 

void setAngle(int channel, int angle)

{

// 4096 ticks is 20,000 us

// Angle 0 is 1000 us

// Angle 180 is 2,000 us

long ticks = ((1000L + 1000L*angle/180L))*4096L)/20000L;

// update the servo channel with the new pulse

pwm.setPWM(channel, 0, ticks);

}

 

This function converts angles to "ticks" that the servo can be set to by using the command pwm.setPWM(channel, 0, ticks). I decided to include this in my code, well in Gabry295's code. So after trying different ways to make the hand move, I ended up with the following code:

 

/*
Robot Hand

Created by Santin Gabriele, 2014
I.T.S.T "J.F. Kennedy", cl. 5^A EA
Thanks to Elias De Lamper for sugestions to improve this program!
*/

#include <Wire.h> // Library needed to use function for servomotors
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

//Assign the servos to a specific channel
int servothumb = 5;
int servoindex = 4;
int servomiddle = 3;
int servoring = 2;
int servopinky = 1;

byte startPackage; // Variable that will contain the character of start package set in Glove //sketch, "<"

//int pulseconstant;

int AngThumb = 0; //Variables with the values for the servomotors (between 0 and 180)
int AngIndex = 0;
int AngMiddle = 0;
int AngRing = 0;
int AngPinky = 0;

void setup()
{
Serial.begin(9600); // Serial communication is activated at 9600 baud/s

//pulseconstant = (1000000/60)/4096;

pwm.begin();
pwm.setPWMFreq(50);

Serial.println("Ready to receive.");

}
void setAngle(int channel, int angle)
{
// 4096 ticks is 20,000 us
// Angle 0 is 1000 us
// Angle 180 is 2,000 us
long ticks = ((1000L +(1000L*angle/180L))*4096L)/20000L;
//ticks = map(ticks,204.8,409.6,200,1200);
//long ticks=map(angle,0,180,150,600);
// update the servo channel with the new pulse
pwm.setPWM(channel,0,ticks);
}



void loop()
{
if(Serial.available()) { //Waiting for data incoming from the other XBee module

startPackage = Serial.read(); //The first value will be "<", the other are assigned to the finger
AngThumb = Serial.read();
AngIndex = Serial.read();
AngMiddle = Serial.read();
AngRing = Serial.read();
AngPinky = Serial.read();

//AngThumb=map(AngThumb,0,180,150,600);
//AngIndex=map(AngIndex,0,180,150,600);
//AngMiddle=map(AngMiddle,0,180,150,600);
//AngRing=map(AngRing,0,180,150,600);
//AngPinky=map(AngPinky,0,180,150,600);

if(startPackage == '<'){ // Verifying that the first value is "<"

Serial.println(AngThumb);
Serial.println(AngIndex);
Serial.println(AngMiddle);
Serial.println(AngRing);
Serial.println(AngPinky);

//if(AngThumb<180) 
//ServoThumb.write(AngThumb); 
//thumbr.write(AngThumb);
setAngle(servothumb, AngThumb);
//pwm.setPWM(servothumb,0,AngThumb);

//if(AngIndex<180)
//ServoIndex.write(AngIndex); 
//servoWrite(2,AngIndex);
//indexr.write(AngIndex);
setAngle(servoindex, AngIndex);
//pwm.setPWM(servoindex,0,AngIndex);

//if(AngMiddle<180)
//ServoMiddle.write(AngMiddle);
//servoWrite(3,AngMiddle);
//middler.write(AngMiddle);
setAngle(servomiddle, AngMiddle);
//pwm.setPWM(servomiddle,0,AngMiddle);

//if(AngRing<180)
//ServoRing.write(AngRing);
//servoWrite(4,AngRing);
//ringr.write(AngRing);
setAngle(servoring, AngRing);
//pwm.setPWM(servoring,0,AngRing);

//if(AngPinky<180)
//ServoPinky.write(AngPinky);
//servoWrite(5,AngPinky);
//pinkyr.write(AngPinky);
setAngle(servopinky, AngPinky);
//pwm.setPWM(servopinky,0,AngPinky); 
}
}
delay(50); // a delay to make the servomotors working correctly
}

 

As you can see in the code, there was a lot of things that was commented out, especially in the section of controlling the servos. Those commented out commands were the different several ways that I was trying. I also deleted some lines to not make the code too long. But this code was what eventually worked for me. Bear in mind that this my second Arduino project so I am fairly new to programming the Ardunio and learning the language of Arduino. So there is probably a better way of controlling the hand or even a simplier way.

 

So now that I got the code working, I had to recalibrate the strings of the fingers. I had some trouble with the movement of the fingers. At first, my problem was that I attached the strings to close to the center of the servos, so I had to reattach them further away from the center to provide a better movement of the finger. But this caused a problem for me for the middle servo of the 3 servo section. I already cut the horns to a small length where I could not attach the strings further away from the center of the servo. If I did attach a new horn, it would interfere the adjacent servos. So what Bryan from here at Boca Bearings recommended was to create some blocks that would raise the middle servo so I can attach a new horn without interfering with the other servos. And it worked!

 

Here's a video of the hand and the glove working together. It's the same video as shown at the top.

 

As you can see in the video, the servos wouldn't move to its maximum position. Maybe there could be something in the program that I can adjust to make the servos rotate all the way. I tried adding some numbers in the setAngle equation but it seemed that it didn't make much of a difference.

 

 

If anyone wants to help me out in figuring this or have any recommendations, please do by writing a comment in the blog. I'm still learning so anything will help me. Another issue that I was having was that sometimes the fingers don't move well and smoothly. I think that source of this problem would probably be individual parts of each finger. Maybe some of them don't interface with each other perfectly.

 

If you have any questions or would like to know anything about the setup, just drop a comment and I'll try to get back to you as soon as possible.