Skip to content

Commit ff807e2

Browse files
committed
PIDcontroller wip
1 parent 891330b commit ff807e2

File tree

6 files changed

+148
-130
lines changed

6 files changed

+148
-130
lines changed

rotary_encoder/motorencoder.py

Lines changed: 16 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,8 @@ def __init__(self, pi, enable_pin, forward_pin, backward_pin, feedback_pin_A, fe
2626

2727
# setting movement variables
2828
self._direction = 0
29-
self._distance = 0
29+
#self._distance = 0
30+
self._distance_per_tick = 0.0981 #(mm)
3031
self._ticks = 0
3132
self._power = 0
3233
self._encoder_speed = 0
@@ -39,7 +40,7 @@ def __init__(self, pi, enable_pin, forward_pin, backward_pin, feedback_pin_A, fe
3940
self._ticks_counter = 0
4041

4142
# other
42-
self._motor_lock = threading.RLock()
43+
#self._motor_lock = threading.RLock()
4344
self._rotary_decoder = RotaryDecoder(pi, feedback_pin_A, feedback_pin_B, self.rotary_callback)
4445

4546
# GETTERS
@@ -49,7 +50,8 @@ def ticks(self):
4950

5051
# distance
5152
def distance(self):
52-
return self._distance
53+
#return self._distance
54+
return self._ticks * self._distance_per_tick
5355

5456
# direction
5557
def direction(self):
@@ -72,7 +74,7 @@ def is_moving(self):
7274
for a certain amount of time """
7375

7476
def control(self, power=100.0, time_elapse=0):
75-
self._motor_lock.acquire() # acquiring lock
77+
#self._motor_lock.acquire() # acquiring lock
7678

7779
# resetting distance and ticks before new movement
7880
self._distance = 0 # resetting distance travelled
@@ -94,7 +96,7 @@ def control(self, power=100.0, time_elapse=0):
9496
self._is_moving = True
9597

9698
# releasing lock on motor
97-
self._motor_lock.release()
99+
#self._motor_lock.release()
98100

99101
# movement time elapse
100102
if (time_elapse > 0):
@@ -106,17 +108,17 @@ def control(self, power=100.0, time_elapse=0):
106108
and releases the lock afterwards """
107109

108110
def stop(self):
109-
self._motor_lock.acquire()
111+
#self._motor_lock.acquire()
110112

111113
# stopping motor
112114
self._pi.write(self._backward_pin, 0)
113115
self._pi.write(self._forward_pin, 0)
114116

115117
# resetting wheel state
116-
self.reset_state()
118+
#self.reset_state()
117119

118-
# releasing lock
119-
self._motor_lock.release()
120+
# releasing lock
121+
#self._motor_lock.release()
120122

121123
# stop auxiliary function, resets wheel state
122124
def reset_state(self):
@@ -135,7 +137,7 @@ def reset_state(self):
135137

136138
# adjust power for velocity control loop
137139
def adjust_power(self, power):
138-
self._motor_lock.acquire() # acquiring lock
140+
#self._motor_lock.acquire() # acquiring lock
139141

140142
self._power = power # setting current power
141143

@@ -147,7 +149,7 @@ def adjust_power(self, power):
147149
self._pi.set_PWM_dutycycle(self._backward_pin, abs(power))
148150

149151
# releasing lock on motor
150-
self._motor_lock.release()
152+
#self._motor_lock.release()
151153

152154
# CALLBACK
153155
""" The callback function rotary_callback is called on FALLING_EDGE by the
@@ -164,7 +166,7 @@ def adjust_power(self, power):
164166
ticks_threshold ticks"""
165167
# callback function
166168
def rotary_callback(self, tick):
167-
self._motor_lock.acquire()
169+
#self._motor_lock.acquire()
168170

169171
# taking groups of n ticks each
170172
if (self._ticks_counter == 0):
@@ -178,7 +180,7 @@ def rotary_callback(self, tick):
178180
#print("Speed: %f" % (self._encoder_speed))
179181

180182
self._ticks += tick # updating ticks
181-
self._distance = self._ticks * 0.0981 # (mm) travelled so far
183+
#self._distance = self._ticks * 0.0981 # (mm) travelled so far
182184

183185

184186
if(self._ticks_counter < self._ticks_threshold):
@@ -191,7 +193,7 @@ def rotary_callback(self, tick):
191193
# not ideal, module on ticks counter not precise, may miss an interrupt
192194
#self._ticks_counter += 1 % (self._ticks_threshold + 1)
193195

194-
self._motor_lock.release() # releasing lock
196+
#self._motor_lock.release() # releasing lock
195197

196198
# callback cancelling
197199
def cancel_callback(self):

rotary_encoder/rotarydecoder.py

Lines changed: 97 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -1,103 +1,108 @@
11
#!/usr/bin/env python
22

33
import pigpio
4+
import threading
45

56
class RotaryDecoder:
67

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-
Note: level = 0 falling edge
38-
1 raising edge
39-
2 from watchdog
8+
""" Class to decode mechanical rotary encoder pulses """
9+
10+
def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback):
11+
12+
self._pi = pi
13+
self._feedback_pin_A = feedback_pin_A # encoder feedback pin A
14+
self._feedback_pin_B = feedback_pin_B # encoder feedback pin B
15+
self._callback = callback # callback function on event
16+
self._direction = 0 # direction, forward = 1, backward = -1, steady = 0
17+
18+
self._levelA = 0 # value of encoder feedback pin A
19+
self._levelB = 0 # value of encoder feedback pin B
20+
21+
self._lock = threading.RLock()
22+
23+
# setting up GPIO
24+
self._pi.set_mode(feedback_pin_A, pigpio.INPUT)
25+
self._pi.set_mode(feedback_pin_B, pigpio.INPUT)
26+
self._pi.set_pull_up_down(feedback_pin_A, pigpio.PUD_UP)
27+
self._pi.set_pull_up_down(feedback_pin_B, pigpio.PUD_UP)
28+
29+
# callback function on EITHER_EDGE for each pin
30+
self._callback_triggerA = self._pi.callback(feedback_pin_A, pigpio.EITHER_EDGE, self._pulse)
31+
self._callback_triggerB = self._pi.callback(feedback_pin_B, pigpio.EITHER_EDGE, self._pulse)
32+
33+
self._lastGpio = None
34+
35+
""" pulse is the callback function on EITHER_EDGE
36+
We have two feedback input from pin A and B (two train waves)
37+
it returns a 1 if the square waves have A leading B because we're moving forward
38+
It returns a -1 if the square waves have B leading A because we're moving backwards
39+
In either case, A is staggered from B by (+-)pi/2 radiants
40+
Note: level = 0 falling edge
41+
1 raising edge
42+
2 from watchdog
4043
41-
+---------+ +---------+ 0
42-
| | | |
43-
B | | | |
44-
| | | |
45-
+---------+ +---------+ +----- 1 # B leading A
46-
+---------+ +---------+ 0 # forward
47-
| | | |
48-
A | | | |
49-
| | | |
50-
----+ +---------+ +----------+ 1
44+
+---------+ +---------+ 0
45+
| | | |
46+
B | | | |
47+
| | | |
48+
+---------+ +---------+ +----- 1 # B leading A
49+
+---------+ +---------+ 0 # forward
50+
| | | |
51+
A | | | |
52+
| | | |
53+
----+ +---------+ +----------+ 1
5154
5255
53-
+---------+ +---------+ 0
54-
| | | |
55-
A | | | |
56-
| | | |
57-
----+ +---------+ +----------+ 1 # A leading B
58-
+---------+ +---------+ 0 # backward
59-
| | | |
60-
B | | | |
61-
| | | |
62-
+---------+ +---------+ +----- 1
63-
"""
64-
def _pulse(self, gpio, level, tick):
65-
# interrupt comes from pin A
66-
if (gpio == self._feedback_pin_A):
67-
self._levelA = level # set level of squared wave (0, 1) on A
68-
# interrupt comes from pin B
69-
else:
70-
self._levelB = level # set level of squared wave (0, 1) on B
71-
72-
if (gpio != self._lastGpio): # debounce
73-
self._lastGpio = gpio
74-
75-
# backward (A leading B)
76-
if (gpio == self._feedback_pin_A and level == 1):
77-
if (self._levelB == 0):
78-
self._callback(-1) # A leading B, moving backward
79-
self._direction = -1 # backward
80-
elif (gpio == self._feedback_pin_A and level == 0):
81-
if (self._levelB == 1):
82-
self._callback(-1) # A leading B, moving backward
83-
self._direction = -1 # backward
84-
85-
# forward (B leading A)
86-
elif (gpio == self._feedback_pin_B and level == 1):
87-
if (self._levelA == 0):
88-
self._callback(1) # B leading A, moving forward
89-
self._direction = 1 # forward
90-
elif (gpio == self._feedback_pin_B and level == 0):
91-
if (self._levelA == 1):
92-
self._callback(1) # A leading B, moving forward
93-
self._direction = 1 # forward
94-
95-
def cancel(self):
96-
97-
"""
98-
Cancel the rotary encoder decoder callbacks.
99-
"""
100-
self._callback_triggerA.cancel()
101-
self._callback_triggerB.cancel()
56+
+---------+ +---------+ 0
57+
| | | |
58+
A | | | |
59+
| | | |
60+
----+ +---------+ +----------+ 1 # A leading B
61+
+---------+ +---------+ 0 # backward
62+
| | | |
63+
B | | | |
64+
| | | |
65+
+---------+ +---------+ +----- 1
66+
"""
67+
def _pulse(self, gpio, level, tick):
68+
#self._lock.acquire()
69+
# interrupt comes from pin A
70+
if (gpio == self._feedback_pin_A):
71+
self._levelA = level # set level of squared wave (0, 1) on A
72+
# interrupt comes from pin B
73+
else:
74+
self._levelB = level # set level of squared wave (0, 1) on B
75+
76+
if (gpio != self._lastGpio): # debounce
77+
self._lastGpio = gpio
78+
79+
# backward (A leading B)
80+
if (gpio == self._feedback_pin_A and level == 1):
81+
if (self._levelB == 0):
82+
self._callback(-1) # A leading B, moving backward
83+
self._direction = -1 # backward
84+
elif (gpio == self._feedback_pin_A and level == 0):
85+
if (self._levelB == 1):
86+
self._callback(-1) # A leading B, moving backward
87+
self._direction = -1 # backward
88+
89+
# forward (B leading A)
90+
elif (gpio == self._feedback_pin_B and level == 1):
91+
if (self._levelA == 0):
92+
self._callback(1) # B leading A, moving forward
93+
self._direction = 1 # forward
94+
elif (gpio == self._feedback_pin_B and level == 0):
95+
if (self._levelA == 1):
96+
self._callback(1) # A leading B, moving forward
97+
self._direction = 1 # forward
98+
#self._lock.release()
99+
100+
def cancel(self):
101+
102+
"""
103+
Cancel the rotary encoder decoder callbacks.
104+
"""
105+
self._callback_triggerA.cancel()
106+
self._callback_triggerB.cancel()
102107

103108

0 commit comments

Comments
 (0)