Skip to content

Commit 242ccd4

Browse files
sorre97sorre97
committed
Single rotary motor class control finshed
1 parent 38a838c commit 242ccd4

File tree

3 files changed

+130
-83
lines changed

3 files changed

+130
-83
lines changed

‎coderbot.py

Lines changed: 55 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -148,10 +148,6 @@ def servo4(self, angle):
148148
def get_sonar_distance(self, sonar_id=0):
149149
return self.sonar[sonar_id].get_distance()
150150

151-
def _dc_enc_motor(self, speed_left=100, speed_right=100, elapse=-1, steps_left=-1, steps_right=-1):
152-
self._twin_motors_enc.control(power_left=speed_left, power_right=speed_right,
153-
elapse=elapse, speed_left=speed_left, speed_right=speed_right,
154-
steps_left=steps_left, steps_right=steps_right)
155151

156152
def _dc_motor(self, speed_left=100, speed_right=100, elapse=-1, steps_left=-1, steps_right=-1):
157153

@@ -256,6 +252,11 @@ def _cb_button(self, gpio, level, tick):
256252
logging.info("pushed: %d, %d", level, tick)
257253
cb()
258254

255+
def _dc_enc_motor(self, speed_left=100, speed_right=100, elapse=-1, steps_left=-1, steps_right=-1):
256+
self._twin_motors_enc.control(power_left=speed_left, power_right=speed_right,
257+
elapse=elapse, speed_left=speed_left, speed_right=speed_right,
258+
steps_left=steps_left, steps_right=steps_right)
259+
259260
class MotorEncoder(object):
260261
# def __init__(self, parent, _pigpio, pin_enable, pin_forward, pin_backward, pin_encoder):
261262
# self._parent = parent
@@ -286,56 +287,56 @@ class MotorEncoder(object):
286287
# def exit(self):
287288
# self._cb.cancel()
288289

289-
def _cb_encoder(self, gpio, level, tick):
290-
self._motor_lock.acquire()
291-
self._encoder_dist += 1
292-
delta_ticks = tick - self._encoder_last_tick if tick > self._encoder_last_tick else tick - self._encoder_last_tick + 4294967295
293-
self._encoder_last_tick = tick
294-
self._encoder_speed = 1000000.0 / delta_ticks #convert speed in steps per second
295-
#print "pin: " + str(self._pin_forward) + " dist: " + str(self._encoder_dist) + " target: " + str(self._encoder_dist_target)
296-
if self._encoder_dist_target >= 0 and self._motor_stop_fast:
297-
#delta_s is the delta (in steps)before the target to reverse the motor in order to arrive at target
298-
delta_s = max(min(self._encoder_speed / self._encoder_k_s_1, 100), 0)
299-
#print "pin: " + str(self._pin_forward) + " dist: " + str(self._encoder_dist) + " target: " + str(self._encoder_dist_target) + " delta_s: " + str(delta_s)
300-
if (self._encoder_dist >= self._encoder_dist_target - delta_s and
301-
not self._motor_stopping and self._motor_running):
302-
self._motor_stopping = True
303-
self._pigpio.write(self._pin_duty, 0)
304-
self._pigpio.set_PWM_dutycycle(self._pin_reverse, self._power)
305-
elif (self._motor_running and
306-
((self._motor_stopping and
307-
self._encoder_speed < self._encoder_k_v_1) or
308-
(self._motor_stopping and
309-
self._encoder_dist >= self._encoder_dist_target))):
310-
self.stop()
311-
logging.info("dist: " + str(self._encoder_dist) + " speed: " + str(self._encoder_speed))
312-
if self._encoder_dist_target >= 0 and not self._motor_stop_fast:
313-
if self._encoder_dist >= self._encoder_dist_target:
314-
self.stop()
315-
self._parent._cb_encoder(self, gpio, level, tick)
316-
self._motor_lock.release()
317-
if not self._motor_running:
318-
self._parent._check_complete()
319-
320-
def control(self, power=100.0, elapse=-1, speed=100.0, steps=-1):
321-
self._motor_lock.acquire()
322-
self._direction = speed > 0
323-
self._encoder_dist_target = steps
324-
self._motor_stopping = False
325-
self._motor_running = True
326-
self._encoder_dist = 0
327-
self._encoder_speed_target = abs(speed)
328-
self._power = abs(power) #TODO: initial power must be a function of desired speed
329-
self._power_actual = abs(power) #TODO: initial power must be a function of desired speed
330-
self._pin_duty = self._pin_forward if self._direction else self._pin_backward
331-
self._pin_reverse = self._pin_backward if self._direction else self._pin_forward
332-
self._pigpio.write(self._pin_reverse, 0)
333-
self._pigpio.set_PWM_dutycycle(self._pin_duty, self._power)
334-
self._pigpio.write(self._pin_enable, True)
335-
self._motor_lock.release()
336-
if elapse > 0:
337-
time.sleep(elapse)
338-
self.stop()
290+
# def _cb_encoder(self, gpio, level, tick):
291+
# #self._motor_lock.acquire()
292+
# #self._encoder_dist += 1
293+
# delta_ticks = tick - self._encoder_last_tick if tick > self._encoder_last_tick else tick - self._encoder_last_tick + 4294967295
294+
# self._encoder_last_tick = tick
295+
# #self._encoder_speed = 1000000.0 / delta_ticks #convert speed in steps per second
296+
# #print "pin: " + str(self._pin_forward) + " dist: " + str(self._encoder_dist) + " target: " + str(self._encoder_dist_target)
297+
# if self._encoder_dist_target >= 0 and self._motor_stop_fast:
298+
# #delta_s is the delta (in steps)before the target to reverse the motor in order to arrive at target
299+
# delta_s = max(min(self._encoder_speed / self._encoder_k_s_1, 100), 0)
300+
# #print "pin: " + str(self._pin_forward) + " dist: " + str(self._encoder_dist) + " target: " + str(self._encoder_dist_target) + " delta_s: " + str(delta_s)
301+
# if (self._encoder_dist >= self._encoder_dist_target - delta_s and
302+
# not self._motor_stopping and self._motor_running):
303+
# self._motor_stopping = True
304+
# self._pigpio.write(self._pin_duty, 0)
305+
# self._pigpio.set_PWM_dutycycle(self._pin_reverse, self._power)
306+
# elif (self._motor_running and
307+
# ((self._motor_stopping and
308+
# self._encoder_speed < self._encoder_k_v_1) or
309+
# (self._motor_stopping and
310+
# self._encoder_dist >= self._encoder_dist_target))):
311+
# self.stop()
312+
# logging.info("dist: " + str(self._encoder_dist) + " speed: " + str(self._encoder_speed))
313+
# if self._encoder_dist_target >= 0 and not self._motor_stop_fast:
314+
# if self._encoder_dist >= self._encoder_dist_target:
315+
# self.stop()
316+
# self._parent._cb_encoder(self, gpio, level, tick)
317+
# #self._motor_lock.release()
318+
# if not self._motor_running:
319+
# self._parent._check_complete()
320+
321+
# def control(self, power=100.0, elapse=-1, speed=100.0, steps=-1):
322+
# #self._motor_lock.acquire()
323+
# #self._direction = speed > 0
324+
# self._encoder_dist_target = steps
325+
# self._motor_stopping = False
326+
# self._motor_running = True
327+
# #self._encoder_dist = 0
328+
# self._encoder_speed_target = abs(speed)
329+
# #self._power = abs(power) #TODO: initial power must be a function of desired speed
330+
# self._power_actual = abs(power) #TODO: initial power must be a function of desired speed
331+
# self._pin_duty = self._pin_forward if self._direction else self._pin_backward
332+
# self._pin_reverse = self._pin_backward if self._direction else self._pin_forward
333+
# self._pigpio.write(self._pin_reverse, 0)
334+
# #self._pigpio.set_PWM_dutycycle(self._pin_duty, self._power)
335+
# self._pigpio.write(self._pin_enable, True)
336+
# #self._motor_lock.release()
337+
# #if elapse > 0:
338+
# # time.sleep(elapse)
339+
# # self.stop()
339340

340341
# def stop(self):
341342
# self._motor_lock.acquire()

‎rotary_encoder/motorencoder.py

Lines changed: 62 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
import pigpio
22
import threading
3+
from time import sleep
34

45
from rotarydecoder import RotaryDecoder
56

@@ -14,9 +15,9 @@ class MotorEncoder:
1415
concurrency problems on GPIO READ/WRITE """
1516

1617
# default constructor
17-
def __init__(self, pi, enable_pin, forward_pin, backward_pin, encoder_feedback_pin):
18+
def __init__(self, pigpio, enable_pin, forward_pin, backward_pin, encoder_feedback_pin):
1819
# setting pin variables
19-
self._pi = pi
20+
self._pigpio = pigpio
2021
self._enable_pin = enable_pin
2122
self._forward_pin = forward_pin
2223
self._backward_pin = backward_pin
@@ -26,14 +27,13 @@ def __init__(self, pi, enable_pin, forward_pin, backward_pin, encoder_feedback_p
2627
self._direction = 0
2728
self._distance = 0
2829
self._ticks = 0
29-
self._PWM_value = 0
30+
self._power = 0
3031
self._encoder_speed = 0
3132
self._is_moving = False
3233

3334
# other
34-
self._callback = self._pi.callback()
3535
self._motor_lock = threading.RLock()
36-
self._rotary_decoder = RotaryDecoder(pi, encoder_feedback_pin, callback)
36+
self._rotary_decoder = RotaryDecoder(pigpio, encoder_feedback_pin, self.rotary_callback)
3737

3838
# GETTERS
3939
# ticks
@@ -59,32 +59,75 @@ def is_moving(self):
5959
# MOVEMENT
6060
""" The stop function acquires the lock to operate on motor
6161
then writes a 0 on movement pins to stop the motor
62-
and releases the lock afterwards """
63-
def control(self, speed = 100.0):
62+
and releases the lock afterwards
63+
Motor speed on range 0 - 100 already set on PWM_set_range(100)
64+
if a time_elapse parameter value is provided, motion is locked
65+
for a certain amount of time """
66+
67+
def control(self, power=100.0, time_elapse=0):
6468
self._motor_lock.acquire() # acquiring lock
65-
self._distance = 0 #resetting distance everytime motor is used
66-
self._direction = 1 if speed > 0 else -1 # setting direction according to speed
69+
self._direction = 1 if power > 0 else -1 # setting direction according to speed
70+
self._power = power
71+
self.stop() # stopping motor to initialize new movement
6772

68-
if(self._direction):
69-
self.stop()
70-
self._pigpio.set_PWM_dutycycle()
71-
else
72-
self.stop()
73-
self._pigpio.set_PWM_dutycycle()
73+
# going forward
74+
if (self._direction):
75+
self._pigpio.set_PWM_dutycycle(self._forward_pin, abs(power))
76+
# going bacakward
77+
else:
78+
self._pigpio.set_PWM_dutycycle(self._backward_pin, abs(power))
7479

80+
self._is_moving = True
7581

82+
# releasing lock on motor
83+
self._motor_lock.release()
84+
85+
# movement time elapse
86+
if (time_elapse > 0):
87+
sleep(time_elapse)
88+
self.stop()
7689

7790
""" The stop function acquires the lock to operate on motor
7891
then writes a 0 on movement pins to stop the motor
7992
and releases the lock afterwards """
93+
8094
def stop(self):
8195
self._motor_lock.acquire()
82-
self._pi.write(self._backward_pin, 0)
83-
self._pi.write(self._forward_pin, 0)
84-
self._is_moving = False
96+
97+
# stopping motor
98+
self._pigpio.write(self._backward_pin, 0)
99+
self._pigpio.write(self._forward_pin, 0)
100+
101+
# returning state variables to consistent state
102+
self._distance = 0 # resetting distance travelled
103+
self._ticks = 0 # resetting ticks
104+
self._power = 0 # resetting PWM power
105+
self._encoder_speed = 0 # resetting encoder speed
106+
self._direction = 0 # resetting direction
107+
self._is_moving = False # resetting moving flag
108+
109+
# releasing lock
85110
self._motor_lock.release()
86111

87-
# OTHER
112+
# CALLBACKS
113+
""" The callback function rotary_callback is called on FALLING_EDGE by the
114+
rotary_decoder with a parameter value of 1 (1 new tick)
115+
- Gearbox ratio: 120:1 (1 wheel revolution = 120 motor revolution)
116+
- Encoder ratio: 8:1 encoder ticks for 1 motor revolution
117+
- 1 wheel revolution = 128 * 8 = 960 ticks
118+
- R = 30mm
119+
- 1 wheel revolution = 2πR = 2 * π * 30mm = 188.5mm
120+
- 960 ticks = 188.5mm
121+
- 1 tick = 0.196mm
122+
- 1 tick : 0.196mm = x(ticks) : y(mm) """
123+
124+
# callback function
125+
def rotary_callback(self, tick):
126+
self._motor_lock.acquire()
127+
self._ticks += tick # updating ticks
128+
# self._encoder_speed = ? # encoder speed (mm/s)
129+
self._distance = self._ticks * 0.196 # (mm) travelled
130+
88131
# callback cancelling
89132
def cancel_callback(self):
90133
self._rotary_decoder.cancel()

‎rotary_encoder/rotarydecoder.py

Lines changed: 13 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -6,25 +6,28 @@ class RotaryDecoder:
66

77
""" Class to decode mechanical rotary encoder pulses """
88

9-
def __init__(self, pi, encoder_feedback_pin, callback):
9+
def __init__(self, pigpio, encoder_feedback_pin, callback):
1010

11-
self.pi = pi
12-
self.encoder_feedback_pin = encoder_feedback_pin # encoder feedback pin
13-
self.callback = callback # callback function on event
14-
self.value = 0 # value of encoder feedback
11+
self._pigpio = pigpio
12+
self._encoder_feedback_pin = encoder_feedback_pin # encoder feedback pin
13+
self._callback = callback # callback function on event
14+
self._value = 0 # value of encoder feedback
1515

1616
# setting up GPIO
17-
self.pi.set_mode(encoder_feedback_pin, pigpio.INPUT)
18-
self.pi.set_pull_up_down(encoder_feedback_pin, pigpio.PUD_UP)
17+
self._pigpio.set_mode(encoder_feedback_pin, pigpio.INPUT)
18+
self._pigpio.set_pull_up_down(encoder_feedback_pin, pigpio.PUD_UP)
1919

2020
# callback function on EITHER_EDGE
21-
self.callback_trigger = self.pi.callback(encoder_feedback_pin, pigpio.EITHER_EDGE, self._pulse)
21+
self._callback_trigger = self._pigpio.callback(encoder_feedback_pin, pigpio.EITHER_EDGE, self._pulse)
2222

23-
# pulse is the callback function on EITHER_EDGE
24-
# it returns a 1 if the feedback square wave is on a falling edge
23+
""" pulse is the callback function on EITHER_EDGE
24+
it returns a 1 if the feedback square wave is on a falling edge
25+
It simply returns a 1 on FALLING EDGE, i've detached this class in order
26+
to make it easier in case of extension with PIN A attached too """
2527
def _pulse(self, level):
2628
self.levB = level;
2729

30+
# RAISING EDGE
2831
if self.levB == 1:
2932
self.callback(1)
3033

0 commit comments

Comments
 (0)