[PYTHON] Try moving 3 servos with Raspberry Pi

Introduction

I wrote a program to control 3 servos with Python from Raspberry Pi for a small robot development kit called Bezelie, but I think that it can be applied to applications other than Bezelie. We will publish it here.

Target person

――I wanted to run 3 or more servos using Python on Raspberry Pi.

Feature

--Raspberry Pi controls 3 RC servos. --The servo can be moved by setting the centering position of the servo to 0 degrees and specifying an angle of plus or minus 90 degrees. --You can set the rotation limit angle for each servo. --You can specify the rotation speed.

Preparation

--Raspberry Pi. Please enable I2C. --Servo driver board with PCA9685 --PCA9685 is NXP's PWM controller. ――Servo driver boards equipped with this are available from various companies such as Adafruit. --Connect with Raspberry Pi via I2C. --Small servo SG90 digital ✕ 3 pieces --Tower Pro's inexpensive RC servo. --Connect the Raspberry Pi, servo driver board, servo, and servo power supply. -This page may be helpful.

code

bezelie.py

# -*- coding: utf-8 -*-
# Bezelie Python Module for Raspberry Pi
import RPi.GPIO as GPIO
from time import sleep
import smbus
import math

bus = smbus.SMBus(1)
address_pca9685 = 0x40 # If you connect other I2C devices, you might change thi$

# Constants
dutyMax = 490     #
dutyMin = 110     #
dutyCenter = 300  #
steps = 1         #

# Global Valiables
headNow = backNow = stageNow = dutyCenter

# Definitions
def initPCA9685():
  bus.write_byte_data(address_pca9685, 0x00, 0x00)
  freq = 0.9*50
  prescaleval = 25000000.0    # 25MHz
  prescaleval /= 4096.0       # 12-bit
  prescaleval /= float(freq)
  prescaleval -= 1.0
  prescale = int(math.floor(prescaleval + 0.5))
  oldmode = bus.read_byte_data(address_pca9685, 0x00)
  newmode = (oldmode & 0x7F) | 0x10
  bus.write_byte_data(address_pca9685, 0x00, newmode)
  bus.write_byte_data(address_pca9685, 0xFE, prescale)
  bus.write_byte_data(address_pca9685, 0x00, oldmode)
  sleep(0.005)
  bus.write_byte_data(address_pca9685, 0x00, oldmode | 0xa1)

def setPCA9685Duty(channel, on, off):
  channelpos = 0x6 + 4*channel
  try:
    bus.write_i2c_block_data(address_pca9685, channelpos, [on&0xFF, on>>8, off&$
  except IOError:
    pass

def moveServo (id, degree, adj, max, min, speed, now):
  dst = (dutyMin-dutyMax)*(degree+adj+90)/180 + dutyMax
  if speed == 0:
    setPCA9685Duty(id, 0, dst)
    sleep(0.001 * math.fabs(dst-now))
    now = dst
  if dst > max: dst = max
  if dst < min: dst = min
  while (now != dst):
    if now < dst:
      now += steps
      if now > dst: now = dst
    else:
      now -= steps
      if now < dst: now = dst
    setPCA9685Duty(id, 0, now)
    sleep(0.004 * steps *(speed))
  return (now)

def moveHead (degree, speed=1):
  adj = 0       # Head servo adjustment
  max = 490     # Downward limit
  min = 110     # Upward limit
  global headNow
  headNow = moveServo (2, degree, adj, max, min, speed, headNow)

def moveBack (degree, speed=1):
  adj = 0       # Back servo adjustment
  max = 490     # AntiClockwise limit
  min = 110     # Clockwise limit
  global backNow
  backNow = moveServo (1, degree, adj, max, min, speed, backNow)

def moveStage (degree, speed=1):
  adj = 0      # Stage servo adjustment
  max = 490    # AntiClockWise limit
  min = 110    # Clocwise limit
  global stageNow
  stageNow = moveServo (0, degree, adj, max, min, speed,stageNow)

Commentary

-For convenience, the three servos are named Head, Back, and Stage, and moveHead (), moveBack (), and moveStage () are the commands to move each. -Enter an angle in the range of plus or minus 90 degrees in the argument degree. 0 is the center position. If you enter a positive value, it will rotate clockwise. -If the argument speed is omitted, it will be 1, and the larger the value, the slower the movement. If you enter 0, it will run at the fastest speed, but if you do not insert time.sleep () of an appropriate length after execution, the process will accumulate and the behavior will be strange. -The Servo SG90 can rotate 180 degrees, but due to the structure of the robot etc., you may want to limit the rotation angle. In such a case, adjust max and min for each servo. -If you want to fine-tune the servo centering position, try changing the adj value.

sample

--The sample code including the above bezelie.py is available on github. ――For how to use the sample program, please refer to Bezelie official page.

Acknowledgments

In creating the program, I referred to Takashi Kanamaru's book "Electronic work learned with Raspberry Pi" and Adafruit's library. I would like to thank you.