59. Exploring the Pi B+'s Ports V - Controlling Servos and Reading the MPU-6050

Controlling servos using an Arduino is very easy.  Remember the light follower at my post number 45, which used the Arduino Servo.h library HERE?

The Arduino chip is good at this because it can deliver what the servo needs - very accurate PWM (Pulse Width Modulation) signals.  This is not possible to do on a Pi (see the RasPi.tv article HERE) because there are so many routines running and interrupting. In any case, the Raspberry Pi has only one hardware and one software PWM output.

The way to do it on a Pi is to use the Adafruit 16 Channel 12 Bit PWM and Servo Driver Breakout Board which does all of the hard work while the Raspberry Pi just tells it what to do. This board is based on a PCA9685 chip which was actually designed to be an LED controller. It is a PWM driver, controlled by I²C with a fast built-in clock, capable of operating at frequencies up to 1MHz. The 16 channels allow up to 16 PWM devices to be connected - ideal for robotics with lots of servos. 

The board only requires a 3V3 power supply for the chip, GND, and two connections, SCL (the clock line) and SDA (the data line), both of which hang off the I²C bus.  There are 2 further ports for external power and GND for the servos, which the Raspberry Pi would not be able to deliver.  The Adafruit board cost £11.98 including delivery from ModMiPi.  I also bought 2 x YKS New NEW SG90 9g Mini Servo motors from Amazon for £2.30 (free delivery).  These have a 180 degree range - even better than the previous ones I used in the Light Follower, which can rotate about 135 degrees.

The photos below give an idea of the circuitry:

From above, you can see the Pi with the Cyntech paddle board, connected as before to the GY-521 breakout board with the MPU6050 (on the blue breadboard).  I have taken the I²C lines and the 3V3 power and GND from that to the Adafruit servo driver.  The external power to the Adafruit board comes from an adjustable Step Down Supply Power Module with a 4V to 40V input and a 1.5V to 35V output.  It is capable of supplying a current of 2A.  I bought it about a year ago for about £2.  That's it on the white breadboard at the top of the picture.  It is powered from a mains supply of nominal 12V (capable of up to 1 amp). 

I tuned the output of the step down module to 5V for the servos, using its adjustable potentiometer. You can see the 2 servos joined at right angles to each other on the left - each of them can draw currents of some hundreds of mA.  The servos are connected to channels 11 and 15 of the Adafruit board.

Using the command:

i2cdetect -y 1 (the 1 refers to Revision 2 or later Raspberry Pis)

the following output was printed on the terminal:

     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- -- 
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
30: -- -- -- -- -- -- -- -- -- -- -- UU -- -- -- -- 
40: 40 -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- -- 
70: 70 -- -- -- -- -- -- --                       

This shows a map of the registers on the Pi's I²C bus.  Two extra entries at the hexadecimal addresses 0x40 and 0x70 show that two I²C devices (the 2 servos in this case) have been detected at those addresses. 

Here's the Python script which I put together from Andrew Birkett's and Adafruit's work.  This is another step on the way to controlling the servos from the signals from the MPU6050:

	#!/usr/bin/python
	# KC6050Servos.py Reads MPU6050 and also drives 2 servos
	# Initially written by Andrew Birkett and Adafruit and developed by S&S Dec 2014
	import smbus
	import math
	import time
	from Adafruit_PWM_Servo_Driver import PWM
	# Initialise the PWM device using the default address
	pwm = PWM(0x40)
	# Note if you'd like more debug output you can instead run:
	#pwm = PWM(0x40, debug=True)
	servoMin = 150 # Min pulse length out of 4096
	servoMax = 600 # Max pulse length out of 4096
	# Power management registers
	power_mgmt_1 = 0x6b
	power_mgmt_2 = 0x6c
	def read_byte(adr):
	return bus.read_byte_data(address, adr)
	def read_word(adr):
	high = bus.read_byte_data(address, adr)
	low = bus.read_byte_data(address, adr+1)
	val = (high << 8) + low
	return val
	def read_word_2c(adr):
	val = read_word(adr)
	if (val >= 0x8000):
	return -((65535 - val) + 1)
	else:
	return val
	def dist(a,b):
	return math.sqrt((a*a)+(b*b))
	def get_y_rotation(x,y,z):
	radians = math.atan2(x, dist(y,z))
	return -math.degrees(radians)
	def get_x_rotation(x,y,z):
	radians = math.atan2(y, dist(x,z))
	return math.degrees(radians)
	def setServoPulse(channel, pulse):
	pulseLength = 1000000 # 1,000,000 us per second
	pulseLength /= 60 # 60 Hz
	print "%d us per period" % pulseLength
	pulseLength /= 4096 # 12 bits of resolution
	print "%d us per bit" % pulseLength
	pulse *= 1000
	pulse /= pulseLength
	pwm.setPWM(channel, 0, pulse)
	def read_and_print():
	gyro_xout = read_word_2c(0x43)
	gyro_yout = read_word_2c(0x45)
	gyro_zout = read_word_2c(0x47)
	accel_xout = read_word_2c(0x3b)
	accel_yout = read_word_2c(0x3d)
	accel_zout = read_word_2c(0x3f)
	accel_xout_scaled = accel_xout / 16384.0
	accel_yout_scaled = accel_yout / 16384.0
	accel_zout_scaled = accel_zout / 16384.0
	xrot = get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
	yrot = get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
	print "x-rotation: %.1f" %xrot+" degrees. y-rotation: %.1f" %yrot+" degrees"
	pwm.setPWMFreq(60) # Set frequency to 60 Hz
	bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
	address = 0x68 # This is the address value read via the i2cdetect command
	# Now wake the 6050 up as it starts in sleep mode
	bus.write_byte_data(address, power_mgmt_1, 0)
	while (True):
	read_and_print()
	# Change speed of continuous servo on channel 15
	pwm.setPWM(15, 0, servoMin)
	time.sleep(1)
	read_and_print()
	# Change speed of continuous servo on channel 15
	pwm.setPWM(15, 0, servoMax)
	time.sleep(1)
	read_and_print()
	# Change speed of continuous servo on channel 11
	pwm.setPWM(11, 0, servoMin)
	time.sleep(1)
	read_and_print()
	# Change speed of continuous servo on channel 15
	pwm.setPWM(15, 0, servoMin)
	time.sleep(1)
	read_and_print()
	# Change speed of continuous servo on channel 15
	pwm.setPWM(15, 0, servoMax)
	time.sleep(1)
	read_and_print()
	# Change speed of continuous servo on channel 11
	pwm.setPWM(11, 0, servoMax)
	time.sleep(1)
	read_and_print()

view raw KC6050ServoA.py hosted with ❤ by GitHub

Note that the xrot and yrot outputs (rotations in degrees about the x- and y-axes respectively) which are printed on the screen at the beginning of the video, are calculated from accelerometer data only, even though the gyroscope data is read also.

And here's the video:

Hopefully I'm going in the right direction to make a self-balancing platform!!