Robotic Arm: Movement Controller Software
Tags: raspberrypi, robot
Published: 2023-04-20 06:56:39

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](https://admantium.com/blog/robokit03_arm_manipulator_movements/)_.

## Controlling Servo Motors on the Raspberry Pi

For the PCA9685 motor hat that is compatible with the Raspberry Pi, I found two libraries: [Adafruit](https://github.com/adafruit/Adafruit_Python_PCA9685) and [Sunfounders](https://github.com/sunfounder/SunFounder_PCA9685). 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.

```bash
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:

```python
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.

```python
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:

```python
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.

```python
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.

```python
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.

```python
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:

```python
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.