├── 6050 ├── PID.pyc ├── Util.pyc ├── motor.pyc ├── MPU6050.pyc ├── motor.py ├── Util.py ├── PID.py ├── combine.py └── MPU6050.py └── README.md /6050/PID.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebdcoder/balancebot/HEAD/6050/PID.pyc -------------------------------------------------------------------------------- /6050/Util.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebdcoder/balancebot/HEAD/6050/Util.pyc -------------------------------------------------------------------------------- /6050/motor.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebdcoder/balancebot/HEAD/6050/motor.pyc -------------------------------------------------------------------------------- /6050/MPU6050.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebdcoder/balancebot/HEAD/6050/MPU6050.pyc -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # balancebot 2 | Python Code for Self Balancing Robot using MPU6050 and Raspberry Pi 3 | 4 | It's not completed yet . 5 | -------------------------------------------------------------------------------- /6050/motor.py: -------------------------------------------------------------------------------- 1 | # A program to control the movement of a single motor using the RTK MCB! 2 | # Composed by The Raspberry Pi Guy to accompany his tutorial! 3 | # Let's import the modules we will need! 4 | import time 5 | import RPi.GPIO as GPIO 6 | # Next we setup the pins for use! 7 | GPIO.setmode(GPIO.BCM) 8 | GPIO.setwarnings(False) 9 | 10 | mhz = 100 11 | 12 | GPIO.setup(21,GPIO.OUT) 13 | GPIO.setup(20,GPIO.OUT) 14 | GPIO.setup(16,GPIO.OUT) 15 | GPIO.setup(12,GPIO.OUT) 16 | 17 | m1 = GPIO.PWM(21, mhz) 18 | m2 = GPIO.PWM(20, mhz) 19 | m3 = GPIO.PWM(16, mhz) 20 | m4 = GPIO.PWM(12, mhz) 21 | 22 | def backward(speed): 23 | if(speed > 100): 24 | speed = 100 25 | m1.start(speed) 26 | GPIO.output(20, False) 27 | m3.start(speed) 28 | GPIO.output(12, False) 29 | 30 | def forward(speed): 31 | if(speed > 100): 32 | speed = 100 33 | m2.start(speed) 34 | GPIO.output(21, False) 35 | m4.start(speed) 36 | GPIO.output(16, False) 37 | 38 | def stop(): 39 | m1.stop() 40 | m2.stop() 41 | m3.stop() 42 | m4.stop() 43 | 44 | def finish(): 45 | print('Finishing up!') 46 | GPIO.output(21, False) 47 | GPIO.output(20, False) 48 | GPIO.output(16, False) 49 | GPIO.output(12, False) 50 | quit() 51 | -------------------------------------------------------------------------------- /6050/Util.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | def i2c_raspberry_pi_bus_number(): 4 | """Returns Raspberry Pi I2C bus number (integer, 0 or 1). 5 | 6 | Looks at `/proc/cpuinfo` to identify if this is a revised model 7 | of the Raspberry Pi (with 512MB of RAM) using `/dev/i2c-1`, or 8 | the original version (with 256MB or RAM) using `/dev/i2c-0`. 9 | """ 10 | cpuinfo = 1 11 | with open('/proc/cpuinfo','r') as f: 12 | for line in f: 13 | if line.startswith('Revision'): 14 | cpuinfo = line.strip()[-1:] 15 | return (1 if (cpuinfo >'3') else 0) 16 | 17 | def i2c_read_byte(bus, address, register): 18 | return bus.read_byte_data(address, register) 19 | 20 | def i2c_read_word_unsigned(bus, address, register): 21 | high = bus.read_byte_data(address, register) 22 | low = bus.read_byte_data(address, register+1) 23 | return (high << 8) + low 24 | 25 | def i2c_read_word_signed(bus, address, register): 26 | value = i2c_read_word_unsigned(bus, address, register) 27 | if (value >= 0x8000): 28 | return -((0xffff - value) + 1) 29 | else: 30 | return value 31 | 32 | def i2c_write_byte(bus, address, register, value): 33 | bus.write_byte_data(address, register, value) 34 | 35 | def i2c_read_block(bus, address, start, length): 36 | return bus.read_i2c_block_data(address, start, length) 37 | 38 | def twos_compliment(high_byte, low_byte): 39 | value = (high_byte << 8) + low_byte 40 | if (value >= 0x8000): 41 | return -((0xffff - value) + 1) 42 | else: 43 | return value 44 | 45 | if __name__ == "__main__": 46 | print i2c_raspberry_pi_bus_number() 47 | -------------------------------------------------------------------------------- /6050/PID.py: -------------------------------------------------------------------------------- 1 | #The recipe gives simple implementation of a Discrete Proportional-Integral-Derivative (PID) controller. PID controller gives output value for error between desired reference input and measurement feedback to minimize error value. 2 | #More information: http://en.wikipedia.org/wiki/PID_controller 3 | # 4 | #cnr437@gmail.com 5 | # 6 | ####### Example ######### 7 | # 8 | #p=PID(3.0,0.4,1.2) 9 | #p.setPoint(5.0) 10 | #while True: 11 | # pid = p.update(measurement_value) 12 | # 13 | # 14 | 15 | 16 | class PID: 17 | """ 18 | Discrete PID control 19 | """ 20 | 21 | def __init__(self, P=2.0, I=0.0, D=1.0, Derivator=0, Integrator=0, Integrator_max=500, Integrator_min=-500): 22 | 23 | self.Kp=P 24 | self.Ki=I 25 | self.Kd=D 26 | self.Derivator=Derivator 27 | self.Integrator=Integrator 28 | self.Integrator_max=Integrator_max 29 | self.Integrator_min=Integrator_min 30 | 31 | self.set_point=0.0 32 | self.error=0.0 33 | 34 | def update(self,current_value): 35 | """ 36 | Calculate PID output value for given reference input and feedback 37 | """ 38 | 39 | self.error = self.set_point - current_value 40 | 41 | self.P_value = self.Kp * self.error 42 | self.D_value = self.Kd * ( self.error - self.Derivator) 43 | self.Derivator = self.error 44 | 45 | self.Integrator = self.Integrator + self.error 46 | 47 | if self.Integrator > self.Integrator_max: 48 | self.Integrator = self.Integrator_max 49 | elif self.Integrator < self.Integrator_min: 50 | self.Integrator = self.Integrator_min 51 | 52 | self.I_value = self.Integrator * self.Ki 53 | 54 | PID = self.P_value + self.I_value + self.D_value 55 | 56 | return PID 57 | 58 | def setPoint(self,set_point): 59 | """ 60 | Initilize the setpoint of PID 61 | """ 62 | self.set_point = set_point 63 | self.Integrator=0 64 | self.Derivator=0 65 | 66 | def setIntegrator(self, Integrator): 67 | self.Integrator = Integrator 68 | 69 | def setDerivator(self, Derivator): 70 | self.Derivator = Derivator 71 | 72 | def setKp(self,P): 73 | self.Kp=P 74 | 75 | def setKi(self,I): 76 | self.Ki=I 77 | 78 | def setKd(self,D): 79 | self.Kd=D 80 | 81 | def getPoint(self): 82 | return self.set_point 83 | 84 | def getError(self): 85 | return self.error 86 | 87 | def getIntegrator(self): 88 | return self.Integrator 89 | 90 | def getDerivator(self): 91 | return self.Derivator 92 | -------------------------------------------------------------------------------- /6050/combine.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import smbus 4 | import math 5 | import time 6 | from MPU6050 import MPU6050 7 | from PID import PID 8 | import motor as MOTOR 9 | 10 | gyro_scale = 131.0 11 | accel_scale = 16384.0 12 | RAD_TO_DEG = 57.29578 13 | M_PI = 3.14159265358979323846 14 | 15 | address = 0x68 # This is the address value read via the i2cdetect command 16 | bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards 17 | 18 | now = time.time() 19 | 20 | K = 0.98 21 | K1 = 1 - K 22 | 23 | time_diff = 0.01 24 | 25 | sensor = MPU6050(bus, address, "MPU6050") 26 | sensor.read_raw_data() # Reads current data from the sensor 27 | 28 | rate_gyroX = 0.0 29 | rate_gyroY = 0.0 30 | rate_gyroZ = 0.0 31 | 32 | gyroAngleX = 0.0 33 | gyroAngleY = 0.0 34 | gyroAngleZ = 0.0 35 | 36 | raw_accX = 0.0 37 | raw_accY = 0.0 38 | raw_accZ = 0.0 39 | 40 | rate_accX = 0.0 41 | rate_accY = 0.0 42 | rate_accZ = 0.0 43 | 44 | accAngX = 0.0 45 | 46 | CFangleX = 0.0 47 | CFangleX1 = 0.0 48 | 49 | K = 0.98 50 | 51 | FIX = -12.89 52 | 53 | #print "{0:.4f} {1:.2f} {2:.2f} {3:.2f} {4:.2f} {5:.2f} {6:.2f}".format( time.time() - now, (last_x), gyro_total_x, (last_x), (last_y), gyro_total_y, (last_y)) 54 | 55 | def dist(a, b): 56 | return math.sqrt((a * a) + (b * b)) 57 | 58 | def get_y_rotation(x,y,z): 59 | radians = math.atan2(x, dist(y,z)) 60 | return -math.degrees(radians) 61 | 62 | def get_x_rotation(x,y,z): 63 | radians = math.atan2(y, dist(x,z)) 64 | return math.degrees(radians) 65 | 66 | p=PID(1.0,-0.04,0.0) 67 | p.setPoint(0.0) 68 | 69 | for i in range(0, int(300.0 / time_diff)): 70 | time.sleep(time_diff - 0.005) 71 | 72 | sensor.read_raw_data() 73 | 74 | # Gyroscope value Degree Per Second / Scalled Data 75 | rate_gyroX = sensor.read_scaled_gyro_x() 76 | rate_gyroY = sensor.read_scaled_gyro_y() 77 | rate_gyroZ = sensor.read_scaled_gyro_z() 78 | 79 | # The angle of the Gyroscope 80 | gyroAngleX += rate_gyroX * time_diff 81 | gyroAngleY += rate_gyroY * time_diff 82 | gyroAngleZ += rate_gyroZ * time_diff 83 | 84 | # Accelerometer Raw Value 85 | raw_accX = sensor.read_raw_accel_x() 86 | raw_accY = sensor.read_raw_accel_y() 87 | raw_accZ = sensor.read_raw_accel_z() 88 | 89 | # Accelerometer value Degree Per Second / Scalled Data 90 | rate_accX = sensor.read_scaled_accel_x() 91 | rate_accY = sensor.read_scaled_accel_y() 92 | rate_accZ = sensor.read_scaled_accel_z() 93 | 94 | # http://ozzmaker.com/2013/04/18/success-with-a-balancing-robot-using-a-raspberry-pi/ 95 | accAngX = ( math.atan2(rate_accX, rate_accY) + M_PI ) * RAD_TO_DEG 96 | CFangleX = K * ( CFangleX + rate_gyroX * time_diff) + (1 - K) * accAngX 97 | 98 | # http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html 99 | accAngX1 = get_x_rotation(rate_accX, rate_accY, rate_accX) 100 | CFangleX1 = ( K * ( CFangleX1 + rate_gyroX * time_diff) + (1 - K) * accAngX1 ) 101 | 102 | # Followed the Second example because it gives resonable pid reading 103 | pid = int(p.update(CFangleX1)) 104 | speed = pid * 30 105 | 106 | if(pid > 0): 107 | MOTOR.forward(speed) 108 | elif(pid < 0): 109 | MOTOR.backward( abs(speed) ) 110 | else: 111 | MOTOR.stop() 112 | 113 | print "{0:.2f} {1:.2f} {2:.2f} {3:.2f} | {4:.2f} {5:.2f} | {6:.2f} | {7:.2f} ".format( gyroAngleX, gyroAngleY , accAngX, CFangleX, accAngX1, CFangleX1, pid, speed) 114 | #print "{0:.2f} {1:.2f}".format( sensor.read_pitch(), sensor.read_roll()) 115 | -------------------------------------------------------------------------------- /6050/MPU6050.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import Util as I2CUtils 4 | 5 | class MPU6050(object): 6 | ''' 7 | Simple MPU-6050 implementation 8 | ''' 9 | 10 | PWR_MGMT_1 = 0x6b 11 | 12 | FS_SEL = 0x1b 13 | FS_250 = 0 14 | FS_500 = 1 15 | FS_1000 = 2 16 | FS_2000 = 3 17 | 18 | AFS_SEL = 0x1c 19 | AFS_2g = 0 20 | AFS_4g = 1 21 | AFS_8g = 2 22 | AFS_16g = 3 23 | 24 | ACCEL_START_BLOCK = 0x3b 25 | ACCEL_XOUT_H = 0 26 | ACCEL_XOUT_L = 1 27 | ACCEL_YOUT_H = 2 28 | ACCEL_YOUT_L = 3 29 | ACCEL_ZOUT_H = 4 30 | ACCEL_ZOUT_L = 5 31 | 32 | ACCEL_SCALE = { AFS_2g : [ 2, 16384.0], AFS_4g : [ 4, 8192.0], AFS_8g : [ 8, 4096.0], AFS_16g : [16, 2048.0] } 33 | 34 | TEMP_START_BLOCK = 0x41 35 | TEMP_OUT_H = 0 36 | TEMP_OUT_L = 1 37 | 38 | GYRO_START_BLOCK = 0x43 39 | GYRO_XOUT_H = 0 40 | GYRO_XOUT_L = 1 41 | GYRO_YOUT_H = 2 42 | GYRO_YOUT_L = 3 43 | GYRO_ZOUT_H = 4 44 | GYRO_ZOUT_L = 5 45 | 46 | GYRO_SCALE = { FS_250 : [ 250, 131.0], FS_500 : [ 500, 65.5], FS_1000 : [1000, 32.8], FS_2000 : [2000, 16.4] } 47 | 48 | K = 0.98 49 | K1 = 1 - K 50 | 51 | def __init__(self, bus, address, name, fs_scale=FS_250, afs_scale=AFS_2g): 52 | ''' 53 | Constructor 54 | ''' 55 | 56 | self.bus = bus 57 | self.address = address 58 | self.name = name 59 | self.fs_scale = fs_scale 60 | self.afs_scale = afs_scale 61 | 62 | self.raw_gyro_data = [0, 0, 0, 0, 0, 0] 63 | self.raw_accel_data = [0, 0, 0, 0, 0, 0] 64 | self.raw_temp_data = [0, 0] 65 | 66 | self.gyro_raw_x = 0 67 | self.gyro_raw_y = 0 68 | self.gyro_raw_z = 0 69 | 70 | self.gyro_scaled_x = 0 71 | self.gyro_scaled_y = 0 72 | self.gyro_scaled_z = 0 73 | 74 | self.raw_temp = 0 75 | self.scaled_temp = 0 76 | 77 | self.accel_raw_x = 0 78 | self.accel_raw_y = 0 79 | self.accel_raw_z = 0 80 | 81 | self.accel_scaled_x = 0 82 | self.accel_scaled_y = 0 83 | self.accel_scaled_z = 0 84 | 85 | self.pitch = 0.0 86 | self.roll = 0.0 87 | 88 | # We need to wake up the module as it start in sleep mode 89 | I2CUtils.i2c_write_byte(self.bus, self.address, MPU6050.PWR_MGMT_1, 0) 90 | # Set the gryo resolution 91 | I2CUtils.i2c_write_byte(self.bus, self.address, MPU6050.FS_SEL, self.fs_scale << 3) 92 | # Set the accelerometer resolution 93 | I2CUtils.i2c_write_byte(self.bus, self.address, MPU6050.AFS_SEL, self.afs_scale << 3) 94 | 95 | def read_raw_data(self): 96 | ''' 97 | Read the raw data from the sensor, scale it appropriately and store for later use 98 | ''' 99 | self.raw_gyro_data = I2CUtils.i2c_read_block(self.bus, self.address, MPU6050.GYRO_START_BLOCK, 6) 100 | self.raw_accel_data = I2CUtils.i2c_read_block(self.bus, self.address, MPU6050.ACCEL_START_BLOCK, 6) 101 | self.raw_temp_data = I2CUtils.i2c_read_block(self.bus, self.address, MPU6050.TEMP_START_BLOCK, 2) 102 | 103 | self.gyro_raw_x = I2CUtils.twos_compliment(self.raw_gyro_data[MPU6050.GYRO_XOUT_H], self.raw_gyro_data[MPU6050.GYRO_XOUT_L]) 104 | self.gyro_raw_y = I2CUtils.twos_compliment(self.raw_gyro_data[MPU6050.GYRO_YOUT_H], self.raw_gyro_data[MPU6050.GYRO_YOUT_L]) 105 | self.gyro_raw_z = I2CUtils.twos_compliment(self.raw_gyro_data[MPU6050.GYRO_ZOUT_H], self.raw_gyro_data[MPU6050.GYRO_ZOUT_L]) 106 | 107 | self.accel_raw_x = I2CUtils.twos_compliment(self.raw_accel_data[MPU6050.ACCEL_XOUT_H], self.raw_accel_data[MPU6050.ACCEL_XOUT_L]) 108 | self.accel_raw_y = I2CUtils.twos_compliment(self.raw_accel_data[MPU6050.ACCEL_YOUT_H], self.raw_accel_data[MPU6050.ACCEL_YOUT_L]) 109 | self.accel_raw_z = I2CUtils.twos_compliment(self.raw_accel_data[MPU6050.ACCEL_ZOUT_H], self.raw_accel_data[MPU6050.ACCEL_ZOUT_L]) 110 | 111 | self.raw_temp = I2CUtils.twos_compliment(self.raw_temp_data[MPU6050.TEMP_OUT_H], self.raw_temp_data[MPU6050.TEMP_OUT_L]) 112 | 113 | # We convert these to radians for consistency and so we can easily combine later in the filter 114 | #self.gyro_scaled_x = math.radians(self.gyro_raw_x / MPU6050.GYRO_SCALE[self.fs_scale][1]) 115 | #self.gyro_scaled_y = math.radians(self.gyro_raw_y / MPU6050.GYRO_SCALE[self.fs_scale][1]) 116 | #self.gyro_scaled_z = math.radians(self.gyro_raw_z / MPU6050.GYRO_SCALE[self.fs_scale][1]) 117 | 118 | self.gyro_scaled_x = self.gyro_raw_x / MPU6050.GYRO_SCALE[self.fs_scale][1] 119 | self.gyro_scaled_y = self.gyro_raw_y / MPU6050.GYRO_SCALE[self.fs_scale][1] 120 | self.gyro_scaled_z = self.gyro_raw_z / MPU6050.GYRO_SCALE[self.fs_scale][1] 121 | 122 | self.scaled_temp = self.raw_temp / 340 + 36.53 123 | 124 | self.accel_scaled_x = self.accel_raw_x / MPU6050.ACCEL_SCALE[self.afs_scale][1] 125 | self.accel_scaled_y = self.accel_raw_y / MPU6050.ACCEL_SCALE[self.afs_scale][1] 126 | self.accel_scaled_z = self.accel_raw_z / MPU6050.ACCEL_SCALE[self.afs_scale][1] 127 | 128 | self.pitch = self.read_x_rotation(self.read_scaled_accel_x(),self.read_scaled_accel_y(),self.read_scaled_accel_z()) 129 | self.roll = self.read_y_rotation(self.read_scaled_accel_x(),self.read_scaled_accel_y(),self.read_scaled_accel_z()) 130 | 131 | def distance(self, x, y): 132 | '''Returns the distance between two point in 2d space''' 133 | return math.sqrt((x * x) + (y * y)) 134 | 135 | def read_x_rotation(self, x, y, z): 136 | '''Returns the rotation around the X axis in radians''' 137 | return math.atan2(y, self.distance(x, z)) 138 | 139 | def read_y_rotation(self, x, y, z): 140 | '''Returns the rotation around the Y axis in radians''' 141 | return -math.atan2(x, self.distance(y, z)) 142 | 143 | def read_raw_accel_x(self): 144 | '''Return the RAW X accelerometer value''' 145 | return self.accel_raw_x 146 | 147 | def read_raw_accel_y(self): 148 | '''Return the RAW Y accelerometer value''' 149 | return self.accel_raw_y 150 | 151 | def read_raw_accel_z(self): 152 | '''Return the RAW Z accelerometer value''' 153 | return self.accel_raw_z 154 | 155 | def read_scaled_accel_x(self): 156 | '''Return the SCALED X accelerometer value''' 157 | return self.accel_scaled_x 158 | 159 | def read_scaled_accel_y(self): 160 | '''Return the SCALED Y accelerometer value''' 161 | return self.accel_scaled_y 162 | 163 | def read_scaled_accel_z(self): 164 | '''Return the SCALED Z accelerometer value''' 165 | return self.accel_scaled_z 166 | 167 | def read_raw_gyro_x(self): 168 | '''Return the RAW X gyro value''' 169 | return self.gyro_raw_x 170 | 171 | def read_raw_gyro_y(self): 172 | '''Return the RAW Y gyro value''' 173 | return self.gyro_raw_y 174 | 175 | def read_raw_gyro_z(self): 176 | '''Return the RAW Z gyro value''' 177 | return self.gyro_raw_z 178 | 179 | def read_scaled_gyro_x(self): 180 | '''Return the SCALED X gyro value in radians/second''' 181 | return self.gyro_scaled_x 182 | 183 | def read_scaled_gyro_y(self): 184 | '''Return the SCALED Y gyro value in radians/second''' 185 | return self.gyro_scaled_y 186 | 187 | def read_scaled_gyro_z(self): 188 | '''Return the SCALED Z gyro value in radians/second''' 189 | return self.gyro_scaled_z 190 | 191 | def read_temp(self): 192 | '''Return the temperature''' 193 | return self.scaled_temp 194 | 195 | def read_pitch(self): 196 | '''Return the current pitch value in radians''' 197 | return self.pitch 198 | 199 | def read_roll(self): 200 | '''Return the current roll value in radians''' 201 | return self.roll 202 | 203 | def read_all(self): 204 | '''Return pitch and roll in radians and the scaled x, y & z values from the gyroscope and accelerometer''' 205 | self.read_raw_data() 206 | return (self.pitch, self.roll, self.gyro_scaled_x, self.gyro_scaled_y, self.gyro_scaled_z, self.accel_scaled_x, self.accel_scaled_y, self.accel_scaled_z) 207 | --------------------------------------------------------------------------------