-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathCurrentSensor.py
More file actions
165 lines (115 loc) · 5.6 KB
/
Copy pathCurrentSensor.py
File metadata and controls
165 lines (115 loc) · 5.6 KB
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
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
import time
from enum import IntEnum
import INA3221.SDL_Pi_INA3221 as INA3221
import threading
import multiprocessing
import numpy as np
import ctypes
# from Motor import MotorType
libc = ctypes.CDLL('libc.so.6')
class MotorType(IntEnum):
"""Identifies the motor to access"""
BASE = 0
LEFT = 1
RIGHT = 2
class CurrentChannel(IntEnum):
LEFT_MOTOR = 1
RIGHT_MOTOR = 2
BASE_LIGHT = 3
# define globale vairables
MOTOR_SKIPPED = multiprocessing.Event()
MOTOR_SKIPPED_LOCK = multiprocessing.Lock()
class CubertCurrentSensor():
run_gripper_monitor = multiprocessing.Event()
_current_threshold = 4#2000
_left_log_list = [[], [], []]
_right_log_list = [[], [], []]
_left_monitor_list = [[], []]
_right_monitor_list = [[], []]
_left_log_lock = multiprocessing.Lock()
_right_log_lock = multiprocessing.Lock()
def __init__(self):
MOTOR_SKIPPED.clear() # set to false
self.sensor = INA3221.SDL_Pi_INA3221(addr=0x40)
self._left_motor_monitor = multiprocessing.Process(target=monitor_grip_current, args=(self, CurrentChannel.LEFT_MOTOR, self._left_log_list, self._left_log_lock))
self._right_motor_monitor = multiprocessing.Process(target=monitor_grip_current, args=(self, CurrentChannel.RIGHT_MOTOR, self._right_log_list, self._right_log_lock))
def __del__(self):
print("Deleting Current Sensor")
if self.run_gripper_monitor.is_set():
self.stopMotorSensing()
del self.sensor
# print("Saving Data")
# np.save("./logging/left_motor_current_reading.npy", np.array(self._left_log_list[0]))
# np.save("./logging/right_motor_current_reading.npy", np.array(self._right_log_list[0]))
# np.save("./logging/left_motor_current_delta.npy", np.array(self._left_log_list[1]))
# np.save("./logging/right_motor_current_delta.npy", np.array(self._right_log_list[1]))
# np.save("./logging/left_motor_current_time.npy", np.array(self._left_log_list[2]))
# np.save("./logging/right_motor_current_time.npy", np.array(self._right_log_list[2]))
# np.save("./logging/left_motor_current_reading_step.npy", np.array(self._left_monitor_list[0]))
# np.save("./logging/right_motor_current_reading_step.npy", np.array(self._right_monitor_list[0]))
# np.save("./logging/left_motor_current_delta_step.npy", np.array(self._left_monitor_list[1]))
# np.save("./logging/right_motor_current_delta_step.npy", np.array(self._right_monitor_list[1]))
print("Sensor Deleted")
def startMotorSensing(self):
print("Starting Threads")
self.run_gripper_monitor.set()
self._left_motor_monitor = multiprocessing.Process(target=monitor_grip_current, args=(self, CurrentChannel.LEFT_MOTOR, self._left_log_list, self._left_log_lock))
self._right_motor_monitor = multiprocessing.Process(target=monitor_grip_current, args=(self, CurrentChannel.RIGHT_MOTOR, self._right_log_list, self._right_log_lock))
self._left_motor_monitor.start()
self._right_motor_monitor.start()
def stopMotorSensing(self):
print("Deleting Sensor")
self.run_gripper_monitor.clear()
print("Terminating Threads")
self._left_motor_monitor.join()
self._right_motor_monitor.join()
def getChannelCurrent(self, channel:CurrentChannel):
return self.sensor.getCurrent_mA(channel)
def getMotorSkipped(self):
MOTOR_SKIPPED_LOCK.acquire()
val = MOTOR_SKIPPED.isSet()
MOTOR_SKIPPED_LOCK.release()
return val
def clearSkipFlag(self):
MOTOR_SKIPPED_LOCK.acquire()
MOTOR_SKIPPED.clear()
MOTOR_SKIPPED_LOCK.release()
def logCurrent(self, motor:MotorType):
if motor == MotorType.LEFT:
self._left_monitor_list[0].append(self.sensor.getCurrent_mA(CurrentChannel.LEFT_MOTOR))
if len(self._left_monitor_list[0]) > 1:
i = len(self._left_monitor_list[0]) - 1
self._left_monitor_list[1].append(self._left_monitor_list[0][i] - self._left_monitor_list[0][i-1])
elif motor == MotorType.RIGHT:
self._right_monitor_list[0].append(self.sensor.getCurrent_mA(CurrentChannel.LEFT_MOTOR))
if len(self._right_monitor_list[0]) > 1:
i = len(self._right_monitor_list[0]) - 1
self._right_monitor_list[1].append(self._right_monitor_list[0][i] - self._right_monitor_list[0][i-1])
def monitor_grip_current(sensor:CubertCurrentSensor, channel:CurrentChannel, log_list, log_lock:multiprocessing.Lock):
conversion_time = 160 # time to wait for new sample
# start = time.time()
curr_reading = 0
prev_reading = 0
while sensor.run_gripper_monitor.isSet():
prev_reading = curr_reading
curr_reading = sensor.getChannelCurrent(channel)
delta = prev_reading - curr_reading
# curr_time = time.time() - start
# log_lock.acquire()
# log_list[0].append(curr_reading)
# log_list[1].append(delta)
# log_list[2].append(curr_time)
# log_lock.release()
# print(curr_reading)
if abs(delta) > sensor._current_threshold:
# print("Motor Skipped!")
MOTOR_SKIPPED_LOCK.acquire()
MOTOR_SKIPPED.set()
MOTOR_SKIPPED_LOCK.release()
# wait for next conversion
libc.usleep(conversion_time)
if __name__ == '__main__':
print("Running Current Sensor Test")
sensor = CubertCurrentSensor()
while True:
print("Base Light Current is %fmA" % sensor.getChannelCurrent(CurrentChannel.BASE_LIGHT))