├── .gitignore ├── README.md ├── derivative.py ├── encoder.py ├── integral.py ├── mock ├── mock_robot_d.py ├── mock_robot_pd.py └── mock_robot_pid.py ├── proportional.py └── quadratureencoder.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cdo] 2 | pythonhosted/ 3 | 4 | # Editor detritus 5 | *.vim 6 | *.swp 7 | tags 8 | .vscode 9 | 10 | # Packaging detritus 11 | *.egg 12 | *.egg-info 13 | dist 14 | build 15 | eggs 16 | parts 17 | bin 18 | var 19 | sdist 20 | develop-eggs 21 | .installed.cfg 22 | 23 | # Installer logs 24 | pip-log.txt 25 | 26 | # Unit test / coverage reports 27 | coverage 28 | .coverage 29 | .tox 30 | .cache 31 | 32 | # Generated documentation 33 | docs/_build 34 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | **This repository and project has now been archived and is no longer supported. The code will remain available for reference and information.** 2 | 3 | # ROBOT PID Example 4 | 5 | Martin O'Hanlon 6 | 7 | ## Files 8 | 9 | * encoder.py - tests the robots encoders and determine sample rate 10 | * quadratureencoder.py - an example implementation of a quadrature encoder 11 | * proportional.py - proportional (P) control 12 | * derivative.py - proportional and derivative (PD) control 13 | * integral.py - proportional, integral and derivative (PID) control 14 | * mock/* - code created to test the PID implementation - left in purely for interest 15 | -------------------------------------------------------------------------------- /derivative.py: -------------------------------------------------------------------------------- 1 | import threading 2 | from gpiozero import DigitalInputDevice, Robot 3 | from time import sleep 4 | 5 | class Encoder(object): 6 | def __init__(self, pin): 7 | self._value = 0 8 | 9 | encoder = DigitalInputDevice(pin) 10 | encoder.when_activated = self._increment 11 | encoder.when_deactivated = self._increment 12 | 13 | def reset(self): 14 | self._value = 0 15 | 16 | def _increment(self): 17 | self._value += 1 18 | 19 | @property 20 | def value(self): 21 | return self._value 22 | 23 | SAMPLETIME = 0.5 24 | TARGET = 20 25 | KP = 0.02 26 | KD = 0.01 27 | 28 | r = Robot((10,9), (8,7)) 29 | e1 = Encoder(17) 30 | e2 = Encoder(18) 31 | 32 | m1_speed = 1 33 | m2_speed = 1 34 | r.value = (m1_speed, m2_speed) 35 | 36 | e1_prev_error = 0 37 | e2_prev_error = 0 38 | 39 | while True: 40 | 41 | e1_error = TARGET - e1.value 42 | e2_error = TARGET - e2.value 43 | 44 | m1_speed += (e1_error * KP) + (e1_prev_error * KD) 45 | m2_speed += (e2_error * KP) + (e1_prev_error * KD) 46 | 47 | m1_speed = max(min(1, m1_speed), 0) 48 | m2_speed = max(min(1, m2_speed), 0) 49 | 50 | r.value = (m1_speed, m2_speed) 51 | 52 | print("e1 {} e2 {}".format(e1.value, e2.value)) 53 | print("m1 {} m2 {}".format(m1_speed, m2_speed)) 54 | 55 | e1.reset() 56 | e2.reset() 57 | 58 | sleep(SAMPLETIME) 59 | 60 | e1_prev_error = e1_error 61 | e2_prev_error = e2_error -------------------------------------------------------------------------------- /encoder.py: -------------------------------------------------------------------------------- 1 | from gpiozero import DigitalInputDevice, Robot 2 | from time import sleep 3 | 4 | class Encoder(object): 5 | def __init__(self, pin): 6 | self._value = 0 7 | 8 | # setup gpiozero to call increment on each when_activated 9 | encoder = DigitalInputDevice(pin) 10 | encoder.when_activated = self._increment 11 | encoder.when_deactivated = self._increment 12 | 13 | def reset(self): 14 | self._value = 0 15 | 16 | def _increment(self): 17 | self._value += 1 18 | 19 | @property 20 | def value(self): 21 | return self._value 22 | 23 | SAMPLETIME = 1 24 | 25 | r = Robot((10,9), (8,7)) 26 | e1 = Encoder(17) 27 | e2 = Encoder(18) 28 | 29 | #start the robot 30 | m1_speed = 1.0 31 | m2_speed = 1.0 32 | r.value = (m1_speed, m2_speed) 33 | 34 | #find a sample rate 35 | while True: 36 | print("e1 {} e2 {}".format(e1.value, e2.value)) 37 | sleep(SAMPLETIME) -------------------------------------------------------------------------------- /integral.py: -------------------------------------------------------------------------------- 1 | import threading 2 | from gpiozero import DigitalInputDevice, Robot 3 | from time import sleep 4 | 5 | class Encoder(object): 6 | def __init__(self, pin): 7 | self._value = 0 8 | 9 | encoder = DigitalInputDevice(pin) 10 | encoder.when_activated = self._increment 11 | encoder.when_deactivated = self._increment 12 | 13 | def reset(self): 14 | self._value = 0 15 | 16 | def _increment(self): 17 | self._value += 1 18 | 19 | @property 20 | def value(self): 21 | return self._value 22 | 23 | def clamp(value): 24 | return max(min(1, value), 0) 25 | 26 | SAMPLETIME = 0.5 27 | TARGET = 20 28 | KP = 0.02 29 | KD = 0.01 30 | KI = 0.005 31 | 32 | r = Robot((10,9), (8,7)) 33 | e1 = Encoder(17) 34 | e2 = Encoder(18) 35 | 36 | m1_speed = 1 37 | m2_speed = 1 38 | r.value = (m1_speed, m2_speed) 39 | 40 | e1_prev_error = 0 41 | e2_prev_error = 0 42 | 43 | e1_sum_error = 0 44 | e2_sum_error = 0 45 | 46 | while True: 47 | 48 | e1_error = TARGET - e1.value 49 | e2_error = TARGET - e2.value 50 | 51 | m1_speed += (e1_error * KP) + (e1_prev_error * KD) + (e1_sum_error * KI) 52 | m2_speed += (e2_error * KP) + (e1_prev_error * KD) + (e2_sum_error * KI) 53 | 54 | m1_speed = max(min(1, m1_speed), 0) 55 | m2_speed = max(min(1, m2_speed), 0) 56 | r.value = (m1_speed, m2_speed) 57 | 58 | print("e1 {} e2 {}".format(e1.value, e2.value)) 59 | print("m1 {} m2 {}".format(m1_speed, m2_speed)) 60 | 61 | e1.reset() 62 | e2.reset() 63 | 64 | sleep(SAMPLETIME) 65 | 66 | e1_prev_error = e1_error 67 | e2_prev_error = e2_error 68 | 69 | e1_sum_error += e1_error 70 | e2_sum_error += e2_error -------------------------------------------------------------------------------- /mock/mock_robot_d.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | import threading 3 | 4 | 5 | class MockRobotEncoder(object): 6 | def __init__(self, speed_error): 7 | self._speed_error = speed_error 8 | self._value = 0 9 | self._total = 0 10 | self.speed = 0.5 11 | 12 | self._t = threading.Thread( 13 | target = self._mock_encoder, 14 | args = (0.1,)) 15 | self._t.start() 16 | 17 | def reset(self): 18 | self._value = 0 19 | 20 | def _mock_encoder(self, interval): 21 | while True: 22 | self._increment() 23 | #sleep differing amounts based on the speed and error introduced 24 | sleep(interval * (2 - self._speed) * self._speed_error) 25 | 26 | def _increment(self): 27 | self._value += 1 28 | self._total += 1 29 | 30 | @property 31 | def value(self): 32 | return int(self._value) 33 | 34 | @property 35 | def speed(self): 36 | return self._speed 37 | 38 | @property 39 | def total(self): 40 | return int(self._total) 41 | 42 | @speed.setter 43 | def speed(self, value): 44 | self._speed = value 45 | 46 | KP = 0.1 47 | KD = 0.025 48 | TARGET = 7 49 | SAMPLETIME = 1 50 | 51 | #create 2 encoders 52 | e1 = MockRobotEncoder(1.13) 53 | e2 = MockRobotEncoder(0.87) 54 | #e2.speed = 0.615 55 | 56 | #create robot 57 | #r = Robot((1,2), (3,4)) 58 | m1_speed = 0.5 59 | m2_speed = 0.5 60 | #r.value = (m1_speed, m2_speed) 61 | 62 | e1_prev_error = 0 63 | e2_prev_error = 0 64 | 65 | while True: 66 | 67 | e1_error = TARGET - e1.value 68 | e2_error = TARGET - e2.value 69 | 70 | e1_adj = (e1_error * KP) + (e1_prev_error * KD) 71 | e2_adj = (e2_error * KP) + (e2_prev_error * KD) 72 | 73 | e1_prev_error = e1_error 74 | e2_prev_error = e2_error 75 | 76 | print("error1 {} error2 {} adj1 {} adj2 {}".format(e1_error, e2_error, e1_adj, e2_adj)) 77 | 78 | m1_speed += e1_adj 79 | m2_speed += e2_adj 80 | 81 | print("e1 {} e2 {} m1 {} m2 {}".format(e1.value, e2.value, m1_speed, m2_speed)) 82 | 83 | e1.speed = m1_speed 84 | e2.speed = m2_speed 85 | 86 | # update the robots speed 87 | #r.value = (m1_speed, m2_speed) 88 | 89 | e1.reset() 90 | e2.reset() 91 | 92 | sleep(SAMPLETIME) -------------------------------------------------------------------------------- /mock/mock_robot_pd.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | import threading 3 | 4 | class MockRobotEncoder(object): 5 | def __init__(self, speed_error): 6 | self._speed_error = speed_error 7 | self._value = 0 8 | self.speed = 0.5 9 | 10 | self._t = threading.Thread( 11 | target = self._mock_encoder, 12 | args = (0.1,)) 13 | self._t.start() 14 | 15 | def reset(self): 16 | self._value = 0 17 | 18 | def _mock_encoder(self, interval): 19 | while True: 20 | self._increment() 21 | sleep(interval) 22 | 23 | def _increment(self): 24 | self._value += (self._speed) 25 | 26 | @property 27 | def value(self): 28 | return int(self._value) 29 | 30 | @property 31 | def speed(self): 32 | return self._speed 33 | 34 | @speed.setter 35 | def speed(self, value): 36 | self._speed = value * self._speed_error 37 | 38 | target = 7 39 | KP = 0.05 40 | KD = 0.025 41 | 42 | e1 = MockRobotEncoder(1.1) 43 | e2 = MockRobotEncoder(0.9) 44 | #e2.speed = 0.615 45 | 46 | previous_error = 0 47 | 48 | while True: 49 | print("1={} 2={}".format(e1.value, e2.value)) 50 | 51 | error_p = float(e1.value) - float(e2.value) 52 | error_d = error_p - previous_error 53 | previous_error = error_p 54 | 55 | e2.speed += (error_p * KP) + (error_d * KD) 56 | 57 | sleep(1) -------------------------------------------------------------------------------- /mock/mock_robot_pid.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | import threading 3 | 4 | class MockRobotEncoder(object): 5 | def __init__(self, speed_error): 6 | self._speed_error = speed_error 7 | self._value = 0 8 | self._total = 0 9 | self.speed = 0.5 10 | 11 | self._t = threading.Thread( 12 | target = self._mock_encoder, 13 | args = (0.1,)) 14 | self._t.start() 15 | 16 | def reset(self): 17 | self._value = 0 18 | 19 | def _mock_encoder(self, interval): 20 | while True: 21 | self._increment() 22 | #sleep differing amounts based on the speed and error introduced 23 | sleep(interval * (2 - self._speed) * self._speed_error) 24 | 25 | def _increment(self): 26 | self._value += 1 27 | self._total += 1 28 | 29 | @property 30 | def value(self): 31 | return int(self._value) 32 | 33 | @property 34 | def speed(self): 35 | return self._speed 36 | 37 | @property 38 | def total(self): 39 | return int(self._total) 40 | 41 | @speed.setter 42 | def speed(self, value): 43 | self._speed = value 44 | 45 | #constants 46 | KP = 0.1 47 | KD = 0.025 48 | KI = 0.01 49 | TARGET = 7 50 | SAMPLETIME = 1 51 | 52 | #create 2 encoders 53 | e1 = MockRobotEncoder(1.13) 54 | e2 = MockRobotEncoder(0.87) 55 | #e2.speed = 0.615 56 | 57 | #create robot 58 | #r = Robot((1,2), (3,4)) 59 | m1_speed = 0.5 60 | m2_speed = 0.5 61 | #r.value = (m1_speed, m2_speed) 62 | 63 | e1_prev_error = 0 64 | e2_prev_error = 0 65 | 66 | e1_sum_error = 0 67 | e2_sum_error = 0 68 | 69 | while True: 70 | 71 | e1_error = TARGET - e1.value 72 | e2_error = TARGET - e2.value 73 | 74 | e1_sum_error += e1_error 75 | e2_sum_error += e2_error 76 | 77 | e1_adj = (e1_error * KP) + (e1_prev_error * KD) + (e1_sum_error * KI) 78 | e2_adj = (e2_error * KP) + (e2_prev_error * KD) + (e2_sum_error * KI) 79 | 80 | e1_prev_error = e1_error 81 | e2_prev_error = e2_error 82 | 83 | print("error1 {} error2 {} adj1 {} adj2 {}".format(e1_error, e2_error, e1_adj, e2_adj)) 84 | 85 | m1_speed += e1_adj 86 | m2_speed += e2_adj 87 | 88 | print("e1 {} e2 {} m1 {} m2 {}".format(e1.value, e2.value, m1_speed, m2_speed)) 89 | 90 | e1.speed = m1_speed 91 | e2.speed = m2_speed 92 | 93 | # update the robots speed 94 | #r.value = (m1_speed, m2_speed) 95 | 96 | e1.reset() 97 | e2.reset() 98 | 99 | sleep(SAMPLETIME) -------------------------------------------------------------------------------- /proportional.py: -------------------------------------------------------------------------------- 1 | import threading 2 | from gpiozero import DigitalInputDevice, Robot 3 | from time import sleep 4 | 5 | class Encoder(object): 6 | def __init__(self, pin): 7 | self._value = 0 8 | 9 | encoder = DigitalInputDevice(pin) 10 | encoder.when_activated = self._increment 11 | encoder.when_deactivated = self._increment 12 | 13 | def reset(self): 14 | self._value = 0 15 | 16 | def _increment(self): 17 | self._value += 1 18 | 19 | @property 20 | def value(self): 21 | return self._value 22 | 23 | SAMPLETIME = 1 24 | TARGET = 45 25 | KP = 0.02 26 | 27 | r = Robot((10,9), (8,7)) 28 | e1 = Encoder(17) 29 | e2 = Encoder(18) 30 | 31 | m1_speed = 1 32 | m2_speed = 1 33 | r.value = (m1_speed, m2_speed) 34 | 35 | while True: 36 | 37 | e1_error = TARGET - e1.value 38 | e2_error = TARGET - e2.value 39 | 40 | m1_speed += e1_error * KP 41 | m2_speed += e2_error * KP 42 | 43 | m1_speed = max(min(1, m1_speed), 0) 44 | m2_speed = max(min(1, m2_speed), 0) 45 | 46 | r.value = (m1_speed, m2_speed) 47 | 48 | print("e1 {} e2 {}".format(e1.value, e2.value)) 49 | print("m1 {} m2 {}".format(m1_speed, m2_speed)) 50 | 51 | e1.reset() 52 | e2.reset() 53 | 54 | sleep(SAMPLETIME) -------------------------------------------------------------------------------- /quadratureencoder.py: -------------------------------------------------------------------------------- 1 | class QuadratureEncoder(object): 2 | """ 3 | A simple quadrature encoder class 4 | 5 | Note - this class does not determine direction 6 | """ 7 | def __init__(self, pin_a, pin_b): 8 | self._value = 0 9 | 10 | encoder_a = DigitalInputDevice(pin_a) 11 | encoder_a.when_activated = self._increment 12 | encoder_a.when_deactivated = self._increment 13 | 14 | encoder_b = DigitalInputDevice(pin_b) 15 | encoder_b.when_activated = self._increment 16 | encoder_b.when_deactivated = self._increment 17 | 18 | def reset(self): 19 | self._value = 0 20 | 21 | def _increment(self): 22 | self._value += 1 23 | 24 | @property 25 | def value(self): 26 | return self._value 27 | 28 | r = Robot((19,21), (24,26)) 29 | e1 = QuadratureEncoder(16, 17 30 | e2 = QuadratureEncoder(18, 19) 31 | 32 | r.value = (1,1) 33 | 34 | while True: 35 | print("e1 {} e2 {}".format(e1.value, e2.value)) 36 | sleep(1) --------------------------------------------------------------------------------