The Raspberry Pi Arm kit is a 4 degree of freedom robot manipulator. Its 4 servo motors are connected via a custom motor hat that put on top of the raspberry Pi. This hat is based on the PCA9685, an I2C motor controller. Using Python, we can program this arm.
In the last article, I showed how to implement a simple TCP based client-server protocol that enables any client to send python objects as bytes to the server via a two-stage header-payload protocol. This article explores now how to actually control the 4 servo motors of the arm, and gives valuable insights and critical lessons learned.
This article originally appeared at my blog admantium.com.
Controlling Servo Motors on the Raspberry Pi
For the PCA9685 motor hat that is compatible with the Raspberry Pi, I found two libraries: Adafruit and Sunfounders. The Adafruit library is marked as deprecated, but if you setup the Raspberry Pi with the steps explained in my very first article, it works out of the box.
To use this library, we will first install it via pip
. Be sure to install it with Python 3, otherwise it might not work.
pip3 install adafruit-pca9685
Then, connect a servo to any of the 16 ports, for example number 6. Open the Python repl on the Raspberry Pi, and insert these:
from Adafruit_PCA9685 import PCA9685
pca = PCA9685()
pca.set_pwm_freq(50)
SERVO_NUMBER = 1
pca.set_pwm(SERVO_NUMBER,0,300)
The servo motor should move. Try to put other values instead of 300, in the range of 100 to 500, and you will see that the servo rotates to different positions. But what is the meaning of this particular value?
In essence, servo motors are controlled by set defining the duty cycle of a PWM signal. The duty cycle defines the percentage of a PWM signal in which this signal is on. The most conventional hobbyist servo is the SG90, which hast a frequency of 20ms (=50Hz) and can take a duty cycle of 1–2ms. This information can be used to calculate the duty cycle as follows:
- 11 Bit Duty Cycle max value = 2024
- 50 Hz =20ms => 1 * 2024 = 2024
- 5Hz = 2ms => 0.1 * 2024 = 202,4
- 2.5Hz = 1ms => 0.05 * 2024 = 101,2
The PCA9986 library uses the formula on & 0xFF
, which effectively caps any input value to max 255. This value corresponds to 10% of 2024, which is the 5Hz maximum of the servo motor.
With this understanding, we can now implement movement commands for each servo.
Movement Functions
Setting PWM values to the servos is not the best mental model to work with. Therefore, I decided to work with degree values between 0 and 180. These values are translated to the corresponding duty cycle values as shown here:
The next step is calibration. First, I send movement commands to each servo. The 90-degree value needs to be the center position of each servo. Therefore, I dismantled each joint, loosened the servos, and put the nose directly in the 90-degree position. After additional fiddling with the joint’s fixtures — not to lose or the servos cannot hold the weight, not to tight or the servos will not move at all — it was done.
I continued with applying the full 180-degree movements to each joint. Naturally, not all movements make sense: The elbow join can point upwards or even backwards. Since I do not plan to grab things from the air, this joint should only be moveable to within 10 to 80 degrees. These limits are implemented as the following function to work as a safeguard of all commands I give to the arm.
def _safe_limits(self, servo, degree):
if servo == SERVO_X:
return degree
elif servo == SERVO_Y:
if degree < 10:
return 10
elif degree > 80:
return 80
else:
return degree
elif servo == SERVO_Z:
if degree < 15:
return 15
elif degree > 180:
return 180
else:
return degree
elif servo == SERVO_G:
if degree < 23:
return 23
elif degree > 103:
return 103
else:
return degree
else:
return degree
Finally, the basic movement function is as follows:
def move(self, servo, degree):
safe_degree = self._safe_limits(servo, degree)
duty_cycle = safe_degree * DUTY_COEFFICENT + MIN_DUTY
print('MOVE', servo, safe_degree, '=>', duty_cycle)
self._pca.set_pwm(servo, 0, int(duty_cycle))
Movement Coordination
Movements of individual joints are possible. Now, let’s work on natural movements.
With center
, each joint is moved so that the arm has a natural starting position.
def center(self):
for servo in [SERVO_X, SERVO_Y, SERVO_Z]:
self.move(servo, 90)
self.move(SERVO_G, 10)
When extend
is used, the arm will be stretched out as far as possible.
def extend(self):
self.move(SERVO_Z, 180)
sleep(0.500)
self.move(SERVO_Y, 0)
sleep(0.500)
self.move(SERVO_Z, 180)
Finally, grab
and release
control the gripper.
def grab(self):
self.move(SERVO_G, 180)
def release(self):
self.move(SERVO_G, 0)
Easing Servo Transitions
Up to now, applying movement actions changes the servos immediately and abrupt. It would be better if movements were smoother, gradually changing the servos position until it meets its goals.
We can solve this with these steps. First, we always need to save the position of each joint. Second, when a new movement command is applied, we need to calculate the distance between the current and the desired position. Third, to get from the current to the desired position, single movement steps are applied with a very short delay.
This is the code:
def __init__(self):
self._servo_x_position = 90
self._servo_y_position = 90
self._servo_z_position = 90
self._servo_g_position = 10
def _move(self, servo, goal):
current_degree, factor, safe_degree = 0, 1.0, 0
if servo == SERVO_X:
current_degree = self._servo_x_position
elif servo == SERVO_Y:
current_degree = self._servo_y_position
elif servo == SERVO_Z:
current_degree = self._servo_z_position
elif servo == SERVO_G:
current_degree = self._servo_g_position
if goal - current_degree < 0:
factor = -1.0
for change_degree in arange(current_degree*1.0, goal*1.0, factor*SERVO_STEPS):
safe_degree = self._safe_limits(servo, change_degree)
duty_cycle = safe_degree * DUTY_COEFFICENT + MIN_DUTY
self._pca.set_pwm(servo, 0, int(duty_cycle))
sleep(SERVO_DELAY)
sleep(0.500)
return safe_degree
Now, movements are applied very gradually.
Conclusion
This article detailed how to write a Python-based control software to move the 4 DOF robotic arm. In essence, the joints of the arm are connected via servo meters. Assembly should be careful and precise: In its neutral position, the nose showing forward, joints should be neither to lose or to fixated. The servos are controlled by applying a fine-tuned PWM duty cycle, which is produced by a custom software that translated degree values of 0 to 180 to duty cycles. Also, save margins are reserved for the movements dependent on the target joint. When each joint can be controlled, complex movements actions become possible. Finally, instead of applying movements immediately, we saw how to ease the servo movements by using individual movements steps between a joints current and desired position.