├── pidcontroller.py ├── README.md ├── LICENSE └── equsant.py /pidcontroller.py: -------------------------------------------------------------------------------- 1 | '''HARIOMHARIBOLJAIMATAJIPITAJIKIJAIJAI''' 2 | #Developed by Mark Tomczak (fixermark@gmail.com) 3 | #Python class to calculate PID values 4 | 5 | 6 | class PIDController: 7 | def __init__(self, P, I, D): 8 | self.KP = P 9 | self.KI = I 10 | self.KD = D 11 | self.target = 0 12 | 13 | self.lastError = 0 14 | self.integrator = 0 15 | 16 | def setTarget(self, newTarget): 17 | self.target = newTarget 18 | self.integrator = 0 19 | 20 | def step(self, currentValue): 21 | error = currentValue - self.target 22 | 23 | output = (self.KP * error + self.KI * self.integrator + self.KD * (error - self.lastError)) 24 | 25 | self.lastError = error 26 | self.integrator += error 27 | 28 | return output 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Equsant-Self-Balancing-Robot-Python 2 | A self balancing robot using Python and the Raspberry Pi 2. Angle calculations done with the help of the MPU6050 IMU. 3 | libraries used: mpu6050-raspberrypi 1.0.2 https://github.com/Tijndagamer/mpu6050 4 | ,Rpi.GPIO 5 | 6 | Just enable I2C on your Raspberry PI: https://www.youtube.com/watch?v=FloRfIeuEoE 7 | I connote you to make a directory, name it as you like and make a python file with the name equsant.py and copy the code from (equsant.py is the Github file from this Project) into that just created python file. 8 | Afterwards just do the same thing with the pidcontroller.py (create a python file and name it pidcontroller.py and save the content of (pidcontroller.py is the Github file from this Project) file into it) file it is important to save both of the files in the same directory 9 | 10 | Most likely you are also going to need to change the PID values in the file equsant.py 11 | 12 | Enjoy 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 shivaay1 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /equsant.py: -------------------------------------------------------------------------------- 1 | '''HARIOMHARIBOLJAIMATAJIPITAJIKIJAIJAI''' 2 | 3 | #a python script to be used with Balancing robots 4 | 5 | # Importing all necessary librarys and classes 6 | from mpu6050 import mpu6050 7 | from time import sleep 8 | import math 9 | from pidcontroller import PIDController 10 | import RPi.GPIO as GPIO 11 | 12 | 13 | 14 | GPIO.setmode(GPIO.BCM) #Setting the Mode to use. I am using the BCM setup 15 | GPIO.setwarnings(False) 16 | 17 | #Declaring the GPIO Pins that the motor controller is set with 18 | int1 = 21 19 | int2 = 20 20 | int3 = 16 21 | int4 = 12 22 | 23 | GPIO.setup(int1, GPIO.OUT) 24 | GPIO.setup(int2, GPIO.OUT) 25 | GPIO.setup(int3, GPIO.OUT) 26 | GPIO.setup(int4, GPIO.OUT) 27 | 28 | #Pulse width modulation: the speed changes accordingly the inclination angle 29 | PWM1 = GPIO.PWM(21, 100) 30 | PWM2 = GPIO.PWM(20, 100) 31 | PWM3 = GPIO.PWM(16, 100) 32 | PWM4 = GPIO.PWM(12, 100) 33 | 34 | 35 | PWM1.start(0) 36 | PWM2.start(0) 37 | PWM3.start(0) 38 | PWM4.start(0) 39 | 40 | 41 | #This backward function takes a velocity argument that is the PID value. Both motors drives backward 42 | def backward(velocity): 43 | PWM1.ChangeDutyCycle(velocity) 44 | GPIO.output(int2, GPIO.LOW) 45 | PWM3.ChangeDutyCycle(velocity) 46 | GPIO.output(int4, GPIO.LOW) 47 | 48 | #Alike the backward funtion this forward function does the same thing but moves both the motors forward. 49 | def forward(velocity): 50 | GPIO.output(int1, GPIO.LOW) 51 | PWM2.ChangeDutyCycle(velocity) 52 | GPIO.output(int3, GPIO.LOW) 53 | PWM4.ChangeDutyCycle(velocity) 54 | 55 | #If the PID value is 0 (the Robot is 'balanced') it uses this equilibrium function. 56 | def equilibrium(): 57 | GPIO.output(int1, False) 58 | GPIO.output(int2, False) 59 | GPIO.output(int3, False) 60 | GPIO.output(int4, False) 61 | 62 | 63 | sensor = mpu6050(0x68) 64 | #K and K1 --> Constants used with the complementary filter 65 | K = 0.98 66 | K1 = 1 - K 67 | 68 | time_diff = 0.02 69 | ITerm = 0 70 | 71 | #Calling the MPU6050 data 72 | accel_data = sensor.get_accel_data() 73 | gyro_data = sensor.get_gyro_data() 74 | 75 | aTempX = accel_data['x'] 76 | aTempY = accel_data['y'] 77 | aTempZ = accel_data['z'] 78 | 79 | gTempX = gyro_data['x'] 80 | gTempY = gyro_data['y'] 81 | gTempZ = gyro_data['z'] 82 | 83 | #some math 84 | def distance(a, b): 85 | return math.sqrt((a*a) + (b*b)) 86 | 87 | def y_rotation(x, y, z): 88 | radians = math.atan2(x, distance(y, z)) 89 | return -math.degrees(radians) 90 | 91 | def x_rotation(x, y, z): 92 | radians = math.atan2(y, distance(x, z)) 93 | return math.degrees(radians) 94 | 95 | 96 | last_x = x_rotation(aTempX, aTempY, aTempZ) 97 | last_y = y_rotation(aTempX, aTempY, aTempZ) 98 | 99 | gyro_offset_x = gTempX 100 | gyro_offset_y = gTempY 101 | 102 | gyro_total_x = (last_x) - gyro_offset_x 103 | gyro_total_y = (last_y) - gyro_offset_y 104 | 105 | 106 | #the so called 'main loop' that loops around and tells the motors wether to move or not 107 | while True: 108 | accel_data = sensor.get_accel_data() 109 | gyro_data = sensor.get_gyro_data() 110 | 111 | accelX = accel_data['x'] 112 | accelY = accel_data['y'] 113 | accelZ = accel_data['z'] 114 | 115 | gyroX = gyro_data['x'] 116 | gyroY = gyro_data['y'] 117 | gyroZ = gyro_data['z'] 118 | 119 | gyroX -= gyro_offset_x 120 | gyroY -= gyro_offset_y 121 | 122 | gyro_x_delta = (gyroX * time_diff) 123 | gyro_y_delta = (gyroY * time_diff) 124 | 125 | gyro_total_x += gyro_x_delta 126 | gyro_total_y += gyro_y_delta 127 | 128 | rotation_x = x_rotation(accelX, accelY, accelZ) 129 | rotation_y = y_rotation(accelX, accelY, accelZ) 130 | 131 | #Complementary Filter 132 | last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x) 133 | 134 | #setting the PID values. Here you can change the P, I and D values according to yiur needs 135 | PID = PIDController(P=-78.5, I=1.0, D=1.0) 136 | PIDx = PID.step(last_x) 137 | 138 | #if the PIDx data is lower than 0.0 than move appropriately backward 139 | if PIDx < 0.0: 140 | backward(-float(PIDx)) 141 | #StepperFor(-PIDx) 142 | #if the PIDx data is higher than 0.0 than move appropriately forward 143 | elif PIDx > 0.0: 144 | forward(float(PIDx)) 145 | #StepperBACK(PIDx) 146 | #if none of the above statements is fulfilled than do not move at all 147 | else: 148 | equilibrium() 149 | 150 | 151 | print(int(last_x), 'PID: ', int(PIDx)) 152 | sleep(0.02) 153 | 154 | 155 | --------------------------------------------------------------------------------