Monday, December 22, 2014

60. Exploring the Pi B+'s Ports VI - Controlling Servos Using the MPU-6050

27,000 page views!!
Hopefully I'm getting a bit closer to a self-balancing platform.  The stage I'm at now effectively has the accelerometer signal being sent to the 2 servos, rotation of the MPU-6050 about its x-axis, rotating one servo, and rotation about the y-axis controlling the second servo.

 Here's the code:
#!/usr/bin/python
# KC6050Servos.py Reads MPU6050 and also drives 2 servos
# Initially written by 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)
xrot = 0.0
yrot = 0.0
def read_data():
global xrot
global yrot
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)
# Map servo rotation to MPU6050 orientation
slope = (servoMax-servoMin)/90
middle = servoMin+(servoMax-servoMin)/2
pwm.setPWM(11, 0, middle)
pwm.setPWM(15, 0, middle)
while (True):
servoPosx = middle+int(slope*xrot)
servoPosy = middle+int(slope*yrot)
read_data()
pwm.setPWM(15, 0, servoPosx)
pwm.setPWM(11, 0, servoPosy)
view raw KC6050ServoB.py hosted with ❤ by GitHub


Here's the video:

 The sensitivity can be changed by adjusting the code
Posted by at No comments:

Saturday, December 20, 2014

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 I2C 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 I2C 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 I2C 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 I2C bus.  Two extra entries at the hexadecimal addresses 0x40 and 0x70 show that two I2C 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!!
Posted by at No comments:

Friday, December 12, 2014

58. Exploring the Pi B+'s Ports IV - Reading the MPU-6050 using Python

26,000 page views!!
The last post described the 3D wireframe demonstration of the MPU-6050's rotations (ie the GY-521 breakout board) about the x-, y- and z-axes, and other scripts which displayed the readout of quaternions etc.  Those test programs were written in the C++ programming language. 

 This time test programs are written in Python, giving me a better chance of understanding what's going on!  The circuitry is identical to that used in the last post, and of course, the I2C interface has previously been enabled.

 smbus is a Python module which allows SMBus (system management bus - a sub-set of the I2C protocol) access through the I2C interface on Linux hosts. This is the key module for Python access.

Reading Accelerometer Data


In this first program, in addition to reading the data from the accelerometer, the output values are sent to a web page, just for fun!  The address of the web page depends on the Pi's IP address, which can be found using the terminal command

 ifconfig

 and appending :8080 to the end.   For example,

 http://ip-address-of-your-pi:8080

 Here's the Python code for server.py:

#!/usr/bin/python
#This python program serves a web page which can be accessed via
#local wifi network, giving x and y rotations of the MPU-6050
# Written by Andrew Birkett
# (http://blog.bitify.co.uk/2013/11/3d-opengl-visualisation-of-data-from.html)
# and modified by S&S Dec 2014
import web
import smbus
import math
urls = (
'/', 'index'
)
# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
bus = smbus.SMBus(1) # or bus = smbus.SMBus(0) for pre-Revision 2 boards
address = 0x68 # This is the address value read via the i2cdetect command
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)
class index:
def GET(self):
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)
# Return angle values to 1 decimal place
return "x-rotation: %.1f" %xrot+" degrees. y-rotation: %.1f" %yrot+" degrees"
if __name__ == "__main__":
# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)
app = web.application(urls, globals())
app.run()
view raw server.py hosted with ❤ by GitHub
Andrew Birkett's really neat code reads the accelerometer part of the MPU-6050 and prints out the x- and y-axis rotations which are calculated by the program.  The output via the web page looks like this:
x-rotation: 0.2 degrees.   y-rotation: -2.2 degrees.


The values of course change as the MPU-6050 is rotated about its x- and y-axes. for example:


x-rotation: 50.8 degrees.   y-rotation: 18.4 degrees.


The web page has to be refreshed to see new values.



Reading Gyro & Accelerometer Data


This program, Reading6050.py, also written by Andrew Birkett, looks like this:



    

      

        

  

    
    


        



  


  

        

          
          #!/usr/bin/python
        

        

          
          

        

        

          
          import smbus
        

        

          
          import math
        

        

          
          

        

        

          
          # 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)
        

        

          
          

        

        

          
          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)
        

        

          
          

        

        

          
          print "gyro data"
        

        

          
          print "---------"
        

        

          
          

        

        

          
          gyro_xout = read_word_2c(0x43)
        

        

          
          gyro_yout = read_word_2c(0x45)
        

        

          
          gyro_zout = read_word_2c(0x47)
        

        

          
          

        

        

          
          print "gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131)
        

        

          
          print "gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131)
        

        

          
          print "gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131)
        

        

          
          

        

        

          
          print
        

        

          
          print "accelerometer data"
        

        

          
          print "------------------"
        

        

          
          

        

        

          
          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
        

        

          
          

        

        

          
          print "accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled
        

        

          
          print "accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled
        

        

          
          print "accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled
        

        

          
          

        

        

          
          print "x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
        

        

          
          print "y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
        

  





    


  




      

      

        view raw
        
          Reading6050.py
        
        hosted with ❤ by GitHub
      

    








The program produces results like this:




pi@raspberrypi ~ $ sudo python Reading6050.py




gyro data




---------




gyro_xout:  -202  scaled:  -2




gyro_yout:  -191  scaled:  -2




gyro_zout:  11  scaled:  0










accelerometer data




------------------




accel_xout:  176  scaled:  0.0107421875




accel_yout:  160  scaled:  0.009765625




accel_zout:  15364  scaled:  0.937744140625




x rotation:  0.596614942501




y rotation:  -0.656278927449




pi@raspberrypi ~ $ 









































So what does the program do and what do these results mean?










Posted by



at





No comments:
  


































        

      









Subscribe to:
Posts (Atom)