# Appendix

This is something more.

## Code

• Rotation visualization Python code

•  ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226``` ```import smbus import pygame import urllib import time import math from OpenGL.GL import * from OpenGL.GLU import * from math import radians from pygame.locals import * # Power management registers power_mgmt_1 = 0x6b power_mgmt_2 = 0x6c SCREEN_SIZE = (800, 600) SCALAR = .5 SCALAR2 = 0.2 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 get_z_rotation(x,y,z): radians = math.atan2(y,z) #radians = math.atan((math.sqrt(x*x+y*y)/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) def get_current_perf(axis): 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 if axis == "x": value = round(get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled), 2) print "x rotation: " , value return value elif axis == "y": value = round(get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled), 2) print "y rotation: " , value return value elif axis == "z": value = round(get_z_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled), 2) print "z rotation: " , value return value else: return False def resize(width, height): glViewport(0, 0, width, height) glMatrixMode(GL_PROJECTION) glLoadIdentity() gluPerspective(45.0, float(width) / height, 0.001, 10.0) glMatrixMode(GL_MODELVIEW) glLoadIdentity() def init(): glEnable(GL_DEPTH_TEST) glClearColor(0.0, 0.0, 0.0, 0.0) glShadeModel(GL_SMOOTH) glEnable(GL_BLEND) glEnable(GL_POLYGON_SMOOTH) glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST) glEnable(GL_COLOR_MATERIAL) glEnable(GL_LIGHTING) glEnable(GL_LIGHT0) glLightfv(GL_LIGHT0, GL_AMBIENT, (0.3, 0.3, 0.3, 1.0)); # def read_values(): # #link = "http://192.168.1.65:8080" # Change this address to your settings # #f = urllib.urlopen(link) # myfile = "45 45" # return myfile.split(" ") def run(): pygame.init() screen = pygame.display.set_mode(SCREEN_SIZE, HWSURFACE | OPENGL | DOUBLEBUF) resize(*SCREEN_SIZE) init() clock = pygame.time.Clock() cube = Cube((0.0, 0.0, 0.0), (.5, .5, .7)) glTranslatef(0.0,0.0, -5) while True: then = pygame.time.get_ticks() for event in pygame.event.get(): if event.type == QUIT: return if event.type == KEYUP and event.key == K_ESCAPE: return y_angle = get_current_perf('y') #z_angle = get_current_perf('z') x_angle = get_current_perf('x') glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) glColor((1.,1.,1.)) glLineWidth(1) glBegin(GL_LINES) for x in range(-20, 22, 2): glVertex3f(x/10.,-1,-1) glVertex3f(x/10.,-1,1) for x in range(-20, 22, 2): glVertex3f(x/10.,-1, -1) glVertex3f(x/10., 1, -1) for z in range(-10, 12, 2): glVertex3f(-2, -1, z/10.) glVertex3f( 2, -1, z/10.) for z in range(-10, 12, 2): glVertex3f(-2, -1, z/10.) glVertex3f(-2, 1, z/10.) for z in range(-10, 12, 2): glVertex3f( 2, -1, z/10.) glVertex3f( 2, 1, z/10.) for y in range(-10, 12, 2): glVertex3f(-2, y/10., -1) glVertex3f( 2, y/10., -1) for y in range(-10, 12, 2): glVertex3f(-2, y/10., 1) glVertex3f(-2, y/10., -1) for y in range(-10, 12, 2): glVertex3f(2, y/10., 1) glVertex3f(2, y/10., -1) glEnd() glPushMatrix() glRotate(float(y_angle), 1, 0, 0) glRotate(float(x_angle), 0, 0, 1) #glRotate(float(z_angle), 0, 1, 0) cube.render() glPopMatrix() pygame.display.flip() time.sleep(0.1) class Cube(object): def __init__(self, position, color): self.position = position self.color = color # Cube information num_faces = 6 vertices = [ (-1.0, -0.05, 0.5), (1.0, -0.05, 0.5), (1.0, 0.05, 0.5), (-1.0, 0.05, 0.5), (-1.0, -0.05, -0.5), (1.0, -0.05, -0.5), (1.0, 0.05, -0.5), (-1.0, 0.05, -0.5) ] normals = [ (0.0, 0.0, +1.0), # front (0.0, 0.0, -1.0), # back (+1.0, 0.0, 0.0), # right (-1.0, 0.0, 0.0), # left (0.0, +1.0, 0.0), # top (0.0, -1.0, 0.0) ] # bottom vertex_indices = [ (0, 1, 2, 3), # front (4, 5, 6, 7), # back (1, 5, 6, 2), # right (0, 4, 7, 3), # left (3, 2, 6, 7), # top (0, 1, 5, 4) ] # bottom def render(self): then = pygame.time.get_ticks() glColor(self.color) vertices = self.vertices # Draw all 6 faces of the cube glBegin(GL_QUADS) for face_no in xrange(self.num_faces): glNormal3dv(self.normals[face_no]) v1, v2, v3, v4 = self.vertex_indices[face_no] glVertex(vertices[v1]) glVertex(vertices[v2]) glVertex(vertices[v3]) glVertex(vertices[v4]) glEnd() if __name__ == "__main__": run() time.sleep(1) ```
• Rotation visualization Python code

•  ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263``` ```#!/usr/bin/python import smbus import math import time import RPi.GPIO as GPIO # Power management registers power_mgmt_1 = 0x6b power_mgmt_2 = 0x6c LOW = 0.02 motor_x = 0.0015 motor_y = 0.0015 motor_z = 0.0015 GPIO.setmode(GPIO.BCM) GPIO.setup(16, GPIO.OUT) # x axis GPIO.setup(17, GPIO.OUT) # y axis GPIO.setup(22, GPIO.OUT) # z axis px = GPIO.PWM(16, 50) py = GPIO.PWM(17, 50) pz = GPIO.PWM(22, 50) 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 get_z_rotation(x,y,z): radians = math.atan2(y,z) #radians = math.atan((math.sqrt(x*x+y*y)/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 "---------" class PID: """ Discrete PID control """ def __init__(self, P=2.0, I=0.0, D=1.0, Derivator=0, Integrator=0, Integrator_max=500, Integrator_min=-500): self.Kp=P self.Ki=I self.Kd=D self.Derivator=Derivator self.Integrator=Integrator self.Integrator_max=Integrator_max self.Integrator_min=Integrator_min self.set_point=0.0 self.error=0.0 def update(self,current_value): """ Calculate PID output value for given reference input and feedback """ self.error = self.set_point - current_value #print "Error: ", self.error self.P_value = self.Kp * self.error self.D_value = self.Kd * ( self.error - self.Derivator) self.Derivator = self.error self.Integrator = self.Integrator + self.error if self.Integrator > self.Integrator_max: self.Integrator = self.Integrator_max elif self.Integrator < self.Integrator_min: self.Integrator = self.Integrator_min self.I_value = self.Integrator * self.Ki PID = self.P_value + self.I_value + self.D_value return PID def setpoint(self,set_point): """ Initilize the setpoint of PID """ self.set_point = set_point self.Integrator=0 self.Derivator=0 def setIntegrator(self, Integrator): self.Integrator = Integrator def setDerivator(self, Derivator): self.Derivator = Derivator def setKp(self,P): self.Kp=P def setKi(self,I): self.Ki=I def setKd(self,D): self.Kd=D def getPoint(self): return self.set_point def getError(self): return self.error def getIntegrator(self): return self.Integrator def getDerivator(self): return self.Derivator def get_current_perf(axis): 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 if axis == "x": value = round(get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled), 2) print "x rotation: " , value return value elif axis == "y": value = round(get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled), 2) print "y rotation: " , value return value elif axis == "z": value = round(get_z_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled), 2) print "z rotation: " , value return value else: return False def set_pwm_output(axis, angle): global LOW global motor_x global motor_y global motor_z if axis == "x": value = pidx.update(angle) value =deadzone(value) val = float(value/90)/1000 motor_x = motor_x + val motor_x = saturation(motor_x) duty_x = 100 * motor_x / 0.02 #print "motor_x: ", motor_x f = 1 / (motor_x + LOW) px.ChangeFrequency(f) px.start(duty_x) if axis == "y": value = pidy.update(angle) print "PID value: ", value value = deadzone(value) val = float(value)/90/1000 motor_y = motor_y + val motor_y = saturation(motor_y) f = 1 / (motor_y + LOW) duty_y = round(100 * motor_y * f, 2) if duty_y < 6.5: duty_y = 6.5 motor_y = motor_y - val print "Duty_y: " , duty_y py.ChangeFrequency(f) py.start(duty_y) if axis == "z": value = pidz.update(angle) value =deadzone(value) val = float(value/90)/1000 motor_z = motor_z - val motor_z = saturation(motor_z) duty_z = 100 * motor_z / 0.02 #print "motor_z: ", motor_z f = 1 / (motor_z + LOW) pz.ChangeFrequency(f) pz.start(duty_z) def saturation(value): if value > 0.0023: # the maximum angle is 160 degree value = 0.0023 if value < 0.0007: # the minmum angle is 20 degree value = 0.0007 return value def deadzone(value): if value < 1 and value > -1: value = 0 return value start_time = time.time() while time.time()-start_time < 2: duty_x = 100 * motor_x / 0.02 px.start(duty_x) duty_y = 100 * motor_y / 0.02 py.start(duty_y) duty_z = 100 * motor_z / 0.02 #print duty_z pz.start(duty_z) pidx = PID(0.2, 0.02, 0.01) pidy = PID(0.21, 0.022, 0.01) pidz = PID(0.2, 0, 0) cmd = None #while cmd != "y": #cmd = raw_input("Set the initial performance? y/n\n") x_init = get_current_perf("x") y_init = get_current_perf("y") z_init = get_current_perf("z") pidx.setpoint(x_init) pidy.setpoint(y_init) pidz.setpoint(z_init) while True: time.sleep(0.11) # 10 Hz angle_x = get_current_perf("x") angle_y = get_current_perf("y") #angle_z = get_current_perf("z") set_pwm_output("x", angle_x) set_pwm_output("y", angle_y) #set_pwm_output("z", angle_z) GPIO.cleanup() ```

## References

• Interfacing Raspberry Pi and MPU-6050.
• 3D OpenGL visualisation of the data from an MPU-6050 connected to a Raspberry Pi.

• ## Contributions

• Yuzhuo Sun:
1, Implemented Pygame and OpenGL module.
2, Tested PID control strategy with different values.
3, Project website.
4, Implement rotation calculation function from MPU-6050 acceleration data.

Hanxiang Hao:
1, Circuit design.
2, Implemented PID control strategy.
3, Project website.
4, MPU-6050 sensor data read and rotation calculation.

• ## Budget

 Item Price Number 3-axis camera mount \$56.08 1 Regular servo motor \$3.62 2 MPU6050 \$6.05 2 Sum \$75.42