Skip to content

Commit cf42bc7

Browse files
sorre97sorre97
committed
PID on each wheel with tuned parameters
1 parent be4d133 commit cf42bc7

File tree

7 files changed

+188
-25
lines changed

7 files changed

+188
-25
lines changed

‎rotary_encoder/motorencoder.py

Lines changed: 17 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ def __init__(self, pi, enable_pin, forward_pin, backward_pin, feedback_pin_A, fe
3535
# quadrature encoder variables
3636
self._start_timer = 0
3737
self._current_timer = 0
38-
self._ticks_threshold = 3
38+
self._ticks_threshold = 100
3939
self._ticks_counter = 0
4040

4141
# other
@@ -149,6 +149,20 @@ def adjust_power(self, power):
149149
self._motor_lock.release()
150150

151151
# CALLBACK
152+
""" The callback function rotary_callback is called on FALLING_EDGE by the
153+
rotary_decoder with a parameter value of 1 (1 new tick)
154+
155+
- Gearbox ratio: 120:1 (1 wheel revolution = 120 motor revolution)
156+
- Encoder ratio: 16:1 encoder ticks for 1 motor revolution
157+
- 1 wheel revolution = 120 * 16 = 1920 ticks
158+
- R = 30mm - 1 wheel revolution = 2πR = 2 * π * 30mm = 188.5mm
159+
- 1920 ticks = 188.5mm
160+
- 1 tick = 0.0981mm
161+
- 1 tick : 0.0981mm = x : 1000mm -> x = 10193 ticks approximately
162+
So 0.0981 is the ticks->distance(mm) conversion coefficient
163+
164+
The callback function calculates current velocity by taking groups of
165+
ticks_threshold ticks"""
152166
# callback function
153167
def rotary_callback(self, tick):
154168
self._motor_lock.acquire()
@@ -161,6 +175,8 @@ def rotary_callback(self, tick):
161175
elapse = self._current_timer - self._start_timer # calculating time elapse
162176
# calculating current speed
163177
self._encoder_speed = self._ticks_threshold * 0.0981 / elapse # (mm/s)
178+
#print("Elapse: %f" % (elapse))
179+
#print("Speed: %f" % (self._encoder_speed))
164180

165181
self._ticks += tick # updating ticks
166182
self._distance = self._ticks * 0.0981 # (mm) travelled so far

‎rotary_encoder/rotarydecoder.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -75,11 +75,11 @@ def _pulse(self, gpio, level, tick):
7575
# backward (A leading B)
7676
if (gpio == self._feedback_pin_A and level == 1):
7777
if (self._levelB == 0):
78-
self._callback(-1) # A leading B, moving forward
78+
self._callback(-1) # A leading B, moving backward
7979
self._direction = -1 # backward
8080
elif (gpio == self._feedback_pin_A and level == 0):
8181
if (self._levelB == 1):
82-
self._callback(-1) # A leading B, moving forward
82+
self._callback(-1) # A leading B, moving backward
8383
self._direction = -1 # backward
8484

8585
# forward (B leading A)

‎rotary_encoder/wheelsaxel.py

Lines changed: 49 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -104,48 +104,75 @@ def control_distance(self, power_left=100, power_right=100, target_distance=0):
104104
#PID parameters
105105
# assuming that power_right is equal to power_left and that coderbot
106106
# moves at 11.5mm/s at full PWM duty cycle
107-
TARGET = 0.95 * power_right #velocity [mm/s]
108-
KP = 0.02 #proportional coefficient
109-
KI = 0.005
110-
SAMPLETIME = 0.05
111-
left_speed = TARGET
112-
right_speed = left_speed
113-
integral_error = 0
107+
MAX_SPEED = 260
108+
TARGET_LEFT = (MAX_SPEED/100) * power_left #velocity [mm/s]
109+
TARGET_RIGHT = (MAX_SPEED / 100) * power_right # velocity [mm/s]
110+
111+
# SOFT RESPONSE
112+
#KP = 0.04 #proportional coefficient
113+
#KD = 0.02 # derivative coefficient
114+
#KI = 0.005 # integral coefficient
115+
116+
# MEDIUM RESPONSE
117+
#KP = 0.8 #proportional coefficient
118+
#KD = 0.04 # derivative coefficient
119+
#KI = 0.02 # integral coefficient
120+
121+
# STRONG RESPONSE
122+
KP = 0.9 # proportional coefficient
123+
KD = 0.05 # derivative coefficient
124+
KI = 0.03 # integral coefficient
125+
126+
SAMPLETIME = 0.1
127+
128+
left_speed = TARGET_LEFT
129+
right_speed = TARGET_RIGHT
130+
131+
left_derivative_error = 0
132+
right_derivative_error = 0
133+
left_integral_error = 0
134+
right_integral_error = 0
114135

115136
# moving for certaing amount of distance
116137
while(abs(self.distance()) < target_distance):
117138
# PI controller
118139

119140
# relative error
120-
left_error = TARGET - self._left_motor.speed()
121-
right_error = TARGET - self._right_motor.speed()
141+
left_error = TARGET_LEFT - self._right_motor.speed()
142+
right_error = TARGET_RIGHT - self._left_motor.speed()
122143

123-
left_speed += (left_error * KP) - (integral_error * KI)
124-
right_speed += (right_error * KP) + (integral_error * KI)
144+
left_speed += (left_error * KP) + (left_derivative_error * KD) + (left_integral_error * KI)
145+
right_speed += (right_error * KP) + (right_derivative_error * KD) + (right_integral_error * KI)
146+
print("LEFT correction: %f" % (left_error * KP + left_derivative_error * KD + left_integral_error * KI))
147+
print("RIGHT correction: %f" % (right_error * KP + right_derivative_error * KD + right_integral_error * KI))
125148

126149
# conrispondent new power
127-
left_power = max(min(100 * left_speed / 95, 100), 0)
128-
right_power = max(min(100 * right_speed / 95, 100), 0)
150+
left_power = max(min(100 * left_speed / MAX_SPEED, 100), 0)
151+
right_power = max(min(100 * right_speed / MAX_SPEED, 100), 0)
129152

130-
print("Left SPEED: %f" % (self._left_motor.speed()))
131-
print("Right SPEED: %f" % (self._right_motor.speed()))
132-
print("Left POWER: %f" % (left_power))
133-
print("Right POWER: %f" % (right_power))
153+
print("Left SPEED: %f" % (self._right_motor.speed()))
154+
print("Right SPEED: %f" % (self._left_motor.speed()))
155+
print("Left POWER: %f" % (right_power))
156+
print("Right POWER: %f" % (left_power))
157+
print("")
134158

135159
# adjusting power on each motors
136160
self._left_motor.adjust_power(left_power)
137161
self._right_motor.adjust_power(right_power)
138162

163+
print("Left error: %f" % (left_error))
164+
print("Right error: %f" % (right_error))
165+
print("")
139166

140-
integral_error += (left_speed - right_speed)
141-
142-
# restoring factor
143-
left_speed = TARGET
144-
right_speed = TARGET
145167

146168
# checking each SAMPLETIME seconds
147169
sleep(SAMPLETIME)
148170

171+
left_derivative_error = left_error
172+
right_derivative_error = right_error
173+
left_integral_error += left_error
174+
right_integral_error += right_error
175+
149176
# robot arrived
150177
self.stop()
151178

‎rotary_encoder_tests/rotarydecoder.py

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#!/usr/bin/env python
2+
3+
import pigpio
4+
5+
class RotaryDecoder:
6+
7+
""" Class to decode mechanical rotary encoder pulses """
8+
9+
def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback):
10+
11+
self._pi = pi
12+
self._feedback_pin_A = feedback_pin_A # encoder feedback pin A
13+
self._feedback_pin_B = feedback_pin_B # encoder feedback pin B
14+
self._callback = callback # callback function on event
15+
self._direction = 0 # direction, forward = 1, backward = -1, steady = 0
16+
17+
self._levelA = 0 # value of encoder feedback pin A
18+
self._levelB = 0 # value of encoder feedback pin B
19+
20+
# setting up GPIO
21+
self._pi.set_mode(feedback_pin_A, pigpio.INPUT)
22+
self._pi.set_mode(feedback_pin_B, pigpio.INPUT)
23+
self._pi.set_pull_up_down(feedback_pin_A, pigpio.PUD_UP)
24+
self._pi.set_pull_up_down(feedback_pin_B, pigpio.PUD_UP)
25+
26+
# callback function on EITHER_EDGE for each pin
27+
self._callback_triggerA = self._pi.callback(feedback_pin_A, pigpio.EITHER_EDGE, self._pulse)
28+
self._callback_triggerB = self._pi.callback(feedback_pin_B, pigpio.EITHER_EDGE, self._pulse)
29+
30+
self._lastGpio = None
31+
32+
""" pulse is the callback function on EITHER_EDGE
33+
We have two feedback input from pin A and B (two train waves)
34+
it returns a 1 if the square waves have A leading B because we're moving forward
35+
It returns a -1 if the square waves have B leading A because we're moving backwards
36+
In either case, A is staggered from B by (+-)pi/2 radiants
37+
38+
+---------+ +---------+ 0
39+
| | | |
40+
A | | | |
41+
| | | |
42+
----+ +---------+ +----------+ 1 # A leading B
43+
+---------+ +---------+ 0 # forward
44+
| | | |
45+
B | | | |
46+
| | | |
47+
+---------+ +---------+ +----- 1
48+
"""
49+
def _pulse(self, gpio, level, tick):
50+
# interrupt comes from pin A
51+
if (gpio == self._feedback_pin_A):
52+
self._levelA = level # set level of squared wave (0, 1) on A
53+
# interrupt comes from pin B
54+
else:
55+
self._levelB = level # set level of squared wave (0, 1) on B
56+
57+
if (gpio != self._lastGpio): # debounce
58+
self._lastGpio = gpio
59+
60+
if (gpio == self._feedback_pin_A and level == 1):
61+
if (self._levelB == 0):
62+
self._callback(1) # A leading B, moving forward
63+
self._direction = 1 # forward
64+
elif (gpio == self._feedback_pin_A and level == 0):
65+
if (self._levelB == 1):
66+
self._callback(1) # A leading B, moving forward
67+
self._direction = 1 # forward
68+
elif (gpio == self._feedback_pin_B and level == 1):
69+
if (self._levelA == 0):
70+
self._callback(-1) # B leading A, moving forward
71+
self._direction = -1 # backwards
72+
elif (gpio == self._feedback_pin_B and level == 0):
73+
if (self._levelA == 1):
74+
self._callback(-1) # A leading B, moving forward
75+
self._direction = -1 # forward
76+
77+
def cancel(self):
78+
79+
"""
80+
Cancel the rotary encoder decoder callbacks.
81+
"""
82+
self._callback_triggerA.cancel()
83+
self._callback_triggerB.cancel()
84+
85+

‎rotary_encoder_tests/testPID.py

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
from coderbot import CoderBot
2+
import sys
3+
4+
DISTANCE = float(sys.argv[1])
5+
6+
c = CoderBot()
7+
c.move(speed=70, distance=DISTANCE)

‎rotary_encoder_tests/velocity_test2.py

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
from time import sleep
2+
from motorencoder import MotorEncoder
3+
import pigpio
4+
5+
pi = pigpio.pi()
6+
7+
m = MotorEncoder(pi, 22, 25, 24, 14, 16)
8+
9+
m.control(power=100.0, time_elapse=5)
10+

‎rotary_encoder_tests/velocity_test3.py

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
from time import sleep
2+
from motorencoder import MotorEncoder
3+
import pigpio
4+
5+
pi = pigpio.pi()
6+
7+
m = MotorEncoder(pi, 22, 25, 24, 14, 16)
8+
9+
while(True):
10+
print("Speed: %d" % (m._encoder_speed))
11+
print("Distance: %d" % (m._distance))
12+
#print("Start timer: %d" % (c._twin_motors_enc._left_motor._start_timer))
13+
#print("Current timer: %d" % (c._twin_motors_enc._left_motor._current_timer))
14+
#print("Elapse: %d" % (c._twin_motors_enc._left_motor._current_timer - c._twin_motors_enc._left_motor._start_timer))
15+
print("")
16+
sleep(0.05)
17+
18+

0 commit comments

Comments
 (0)