How to control the "Robot Hand"

In the robotlab, currently there is only one robot hand which is used for the promobot. Here is the picture:

As you can see each finger is connected to a servo. And these servo can be controlled with a microcontroller. In this case we are using an Arduino UNO as the microcontroller. Here are the steps on how to control it:

  1. Connect all the servo pins
    Each servo has 3 pin: red (5V), brown (GND), and orange (signal/PWM). You can connect the 5V and GND in series. And for each signal pin, you can connect it to one Digital Output (DO) in the Arduino UNO for each pin.

  2. Code for the control
    For controlling the servo you can use the servo library that is available to download in the Arduino IDE. After that you can include the library, and then set all of the servo pin in the beginning of the code. Here’s an example code:

// example code used in rock paper scissor game
#include <Servo.h>

Servo servos[5];

int servo_pins[] = {3,5,6,9,10};
// 0 Thumb
// 1 Index finger
// 2 Middle finger
// 3 Ring finger
// 4 Little finger

void setup() {
    Serial.begin(9600);

//initial position for the fingers (clenching)
    int servo_angles[] = {180, 0, 0, 0, 0};
    for (int i = 0; i < sizeof(servos); i++){
        servos[i].attach(servo_pins[i]);
        servos[i].write(servo_angles[i]);
    } 
}
char command = '0';

void loop() {
    
    if (Serial.available() > 0) {
        command = Serial.read();
    }
	// change the hand pose by giving a serial input
    switch (command) {
    case '0':  // Stone position
        set_servos(0, 0, 0, 0, 0);
        break;
    case '1':  // Paper position
        set_servos(180, 180, 180, 180, 180);
        break;
    case '2':  // Scissors position
        set_servos(0, 180, 180, 0, 0);
        break;
    case '3':  // thunb up
        set_servos(180, 0, 0, 0, 0);
        break; 
 }
  delay(100);
}

void set_servos(int angle_s0, int angle_s1, int angle_s2, int angle_s3, int angle_s4){
int servo_angles[] = {180 - angle_s0, angle_s1, angle_s2, angle_s3, angle_s4};
    for (int i = 0; i < sizeof(servos); i++){
        servos[i].write(servo_angles[i]);
    }
}

As you can see, The code reads the serial input and depending on the input it will change the angles of the fingers.

With this setup you can change the hand pose to “rock”, “paper”, “scissors” and “thum up” by sending the value 0-3 using a serial COM-port. Than the angle for each servo will turn accordingly for each finger.

For more control you can use the following setup:

  1. Use a servo driver
    In our use case we needed acces to the pwm pins of the arduino and there were not enough to use with all the servo’s. So we use a servo driver (I2C PWM port expender) like pca9685. By using this you can control up to 16 servo’s.


Complete Datasheet

  1. Code for the control
    In the previous method we control the hand by sending a number and define on the arduino what that number means. if you want all the options you need 5 fingers x 180 degrees = 900 different positions. Because we would never use all of them we limeted the code to use 10 positions for each finger. This lowered the amount of positions to 5 fingers x 10 degrees = 50 different positions. To have control for each finger we read for 5 characters and a new line to be recieved. When the new line is recieved we know that the end of the message is recieved. then we convert the 5 numbers to angles from 0-180 degrees to set the servo’s.
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
 
// PWM channels for each servo
// 0 Thumb
// 1 Index finger
// 2 Middle finger
// 3 Ring finger
// 4 Little finger

void setup() {
    Serial.begin(9600);

    // Initialize the PWM driver
    pwm.begin();
    pwm.setPWMFreq(47);  // Set frequency to 50 Hz (standard for servos), customizable

    // Set initial servo positions
    set_servos(0, 0, 0, 0, 0);
}

void loop() {
	char last_received = '\0';
	String receive_buffer = "";

	while(last_received != '\n'){
		if (Serial.available()){
			last_received = Serial.read();
			if (last_received != '\n'){
				receive_buffer += last_received;
			}
		}
	}
	//Serial.println(receive_buffer);		//for debugging

	if (receive_buffer != ""){
    	if (receive_buffer.length() == 5) {
        	//for (uint8_t i = 0; i < 5; i++) { Serial.println(receive_buffer[i]); }	// for debugging only

        	uint8_t legal_input = 1; //checks if the 5 character input is a legal input for the convertion.
        	for (uint8_t i = 0; i < 5; i++) {
            	if (receive_buffer[i] < '0' || receive_buffer[i] > '9') {
            	    legal_input = 0;
            	}
        	}

        	if (legal_input) {
            	set_servos(
            	    20 * (receive_buffer[0] - '0'),
            	    20 * (receive_buffer[1] - '0'),
            	    20 * (receive_buffer[2] - '0'),
            	    20 * (receive_buffer[3] - '0'),
            	    20 * (receive_buffer[4] - '0')
            	);
        	}
    	} else if (receive_buffer != "") {
        		Serial.println(receive_buffer);
			}
    	}
	}
}

// Function to set servos to the given angles
void set_servos(uint8_t angle_s0, uint8_t angle_s1, uint8_t angle_s2, uint8_t angle_s3, uint8_t angle_s4) {
    const uint16_t MIN_PULSE = 50;  // Minimum pulse length, neeed to be calibrated according to the servos
    const uint16_t MAX_PULSE = 200;  // Maximum pulse length, neeed to be calibrated according to the servos
    
    uint16_t servo_pulses[] = {
        map(180 - angle_s0, 0, 180, MIN_PULSE, MAX_PULSE),  // Thumb (reversed)
        map(angle_s1, 0, 180, MIN_PULSE, MAX_PULSE),       // Index
        map(angle_s2, 0, 180, MIN_PULSE, MAX_PULSE),       // Middle
        map(angle_s3, 0, 180, MIN_PULSE, MAX_PULSE),       // Ring
        map(angle_s4, 0, 180, MIN_PULSE, MAX_PULSE)        // Little
    };

    for (uint8_t i = 0; i < 5; i++) {
        pwm.setPWM(i, 4095-servo_pulses[i], servo_pulses[i]);
    }
}

With this code you can also control the hand through a python code by uploading code above to the Arduino first. Then you can use the following function to control the hand by sending 5 character of 0-9 number from python to Arduino. Here is an example:

import serial

class Singleton(type):
	_instances = {}
	def __call__(cls, *args, **kwargs):
		if cls not in cls._instances:
			cls._instances[cls] = super(Singleton, cls).__call__(*args, **kwargs)
		return cls._instances[cls]

class Robot_Hand(metaclass=Singleton):
	def __init__(self, port:str = "COM3", Test_mode:bool=False) -> None:
		if not isinstance(port, str):
			raise TypeError("port must be a string")
		if not isinstance(Test_mode, bool):
			raise TypeError("Test_mode must be a bool")
		self.Test_mode = Test_mode
		if self.Test_mode == False:
			try:
				self.Serial = serial.Serial()
				self.Serial.port = port
				self.Serial.baudrate = 9600
				self.Serial.bytesize = 8
				self.Serial.parity = "N"
				self.Serial.stopbits = 1
				self.Serial.timeout = None
				self.Serial.open()
			except:
				if sys.platform == "win32":
					print ("You are using windows and your port is wrong so I opened device manager for you :)")
					os.system('devmgmt.msc')
				raise ConnectionError("Could not open the connection on Serial {0}".format(self.Serial.port))
			sleep(0.1)
		self.__send("00000")

	def fingers(self, thumb:int, index:int, middle:int, ring:int, pinkie:int):
		if not isinstance(thumb, int):
			raise TypeError("thumb must be an int")
		if thumb < 0 or thumb > 9:
			raise ValueError("thumb value must be 0-9")
		if not isinstance(index, int):
			raise TypeError("index must be an int")
		if index < 0 or index > 9:
			raise ValueError("index value must be 0-9")
		if not isinstance(middle, int):
			raise TypeError("middle must be an int")
		if middle < 0 or middle > 9:
			raise ValueError("middle value must be 0-9")
		if not isinstance(ring, int):
			raise TypeError("ring must be an int")
		if ring < 0 or ring > 9:
			raise ValueError("ring value must be 0-9")
		if not isinstance(pinkie, int):
			raise TypeError("pinkie must be an int")
		if pinkie < 0 or pinkie > 9:
			raise ValueError("pinkie value must be 0-9")
		self.__send("{0}{1}{2}{3}{4}".format(thumb, index, middle, ring, pinkie))

        def __send(self, data:str):
		data = data + "\n"
		try:
		self.Serial.write(data.encode('utf-8'))
		except:
	        raise ConnectionError ("Serial device disconnected")
	        sleep(0.1)
		received = self.Serial.read_all().decode()
		if (received != ""):
			print(received)
			return received
	

Save above function in separate python file (for example arduino_class.py) Then you can call the function above like this:

from arduino_class import Robot_Hand 

Hand = Robot_Hand("COM9") //change the port accordingly

Hand.fingers(9,9,0,0,9) //metal sign. you can change this