-
Notifications
You must be signed in to change notification settings - Fork 137
/
controller.py
110 lines (75 loc) · 3.92 KB
/
controller.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
import time
from math import copysign
from ..utils.stoppablethread import StoppableLoopThread
class AbstractController(StoppableLoopThread):
""" Abstract class for motor/sensor controller.
The controller role is to synchronize the reading/writing of a set of instances with their "hardware" equivalent through an :class:`~pypot.robot.io.AbstractIO` object. It is defined as a :class:`~pypot.utils.stoppablethread.StoppableLoopThread` where each loop update synchronizes values from the "software" objects with their "hardware" equivalent.
To define your Controller, you need to define the :meth:`~pypot.utils.stoppablethread.StoppableLoopThread.update` method. This method will be called at the predefined frequency. An exemple of how to do it can be found in :class:`~pypot.dynamixel.controller.BaseDxlController`.
"""
def __init__(self, io, sync_freq):
"""
:param io: IO used to communicate with the hardware motors
:type io: :class:`~pypot.robot.io.AbstractIO`
:param float sync_freq: synchronization frequency
"""
StoppableLoopThread.__init__(self, sync_freq)
self.io = io
def start(self):
StoppableLoopThread.start(self)
self.wait_to_start()
def close(self):
""" Cleans and closes the controller. """
self.stop()
if self.io is not None:
self.io.close()
class MotorsController(AbstractController):
""" Abstract class for motors controller.
The controller synchronizes the reading/writing of a set of motor instances with their "hardware". Each update loop synchronizes values from the "software" :class:`~pypot.dynamixel.motor.DxlMotor` with their "hardware" equivalent.
"""
def __init__(self, io, motors, sync_freq=50):
"""
:param io: IO used to communicate with the hardware motors
:type io: :class:`~pypot.robot.io.AbstractIO`
:param list motors: list of motors attached to the controller
:param float sync_freq: synchronization frequency
"""
AbstractController.__init__(self, io, sync_freq)
self.motors = motors
class DummyController(MotorsController):
def __init__(self, motors):
MotorsController.__init__(self, None, motors)
self.max_speed = 360.0 # degree per second.
def setup(self):
self.last_update = time.time()
for m in self.motors:
m.__dict__['moving_speed'] = 0.0
m.__dict__['present_position'] = 0.0
m.__dict__['goal_position'] = 0.0
def update(self):
delta_t = time.time() - self.last_update
for m in self.motors:
# acceleration infinite, present_speed always equal moving_speed
delta_pos = m.__dict__['goal_position'] - m.__dict__['present_position'] # degree
# degree par second, assumed absolute
speed = (m.__dict__['moving_speed']
if m.__dict__['moving_speed'] != 0.0 else
self.max_speed)
delta_pos_effective = copysign(speed * delta_t, delta_pos)
if abs(delta_pos_effective) >= abs(delta_pos):
m.__dict__['present_position'] = m.__dict__['goal_position']
else:
m.__dict__['present_position'] += delta_pos_effective
self.last_update = time.time()
class SensorsController(AbstractController):
""" Abstract class for sensors controller.
The controller frequently pulls new data from a "real" sensor and updates its corresponding software instance.
"""
def __init__(self, io, sensors, sync_freq=50.):
"""
:param io: IO used to communicate with the hardware motors
:type io: :class:`~pypot.robot.io.AbstractIO`
:param list sensors: list of sensors attached to the controller
:param float sync_freq: synchronization frequency
"""
AbstractController.__init__(self, io, sync_freq)
self.sensors = sensors