sensors.py 11 KB
Newer Older
Chetan Sharma's avatar
Chetan Sharma committed
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
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
import serial
import pyvesc
import time
import numpy as np
import shelve
import threading
import logging
import queue

from abc import ABC, abstractmethod

class Sensor(ABC):
    def __init__(self):
        self.sensorThread = None
        self.measure = threading.Event()

    @abstractmethod
    def sensorFunction(self):
        ...


    def start_measurement(self, outQueue, initialTime):
        self.measure.set()
        self.sensorThread = threading.Thread(target=self.measureLoop,
                                                   daemon=True,
                                                   args=(outQueue, initialTime))
        self.sensorThread.start()

    def measureLoop(self, outQueue, initialTime):
        while self.measure.is_set():
            measurement = self.sensorFunction()
            t = time.perf_counter() - initialTime
            outQueue.put((t, measurement))
                

    def stop_measurement(self):
        if not self.sensorThread:
            raise Exception("Measurement not started yet")
        self.measure.clear()
        self.sensorThread.join()

class Machine(Sensor):
    """
    Initialize Machine interface.
    Args:
        port: port to use
    """
    def __init__(self, port : serial.Serial):
        super().__init__()
        self._logger = logging.Logger("Machine")
        self._logger.setLevel(logging.DEBUG)
        self.port = serial.Serial(port, 115200, 
                                  parity=serial.PARITY_NONE, 
                                  stopbits=serial.STOPBITS_ONE, 
                                  bytesize=serial.EIGHTBITS,
                                  timeout=0.5, 
                                  writeTimeout=0)

        self.port_lock = threading.RLock()
        self.last_position = (0,0,0)


        self._logger.info("Waking up GRBL")
        self.write("")
        self.write("")
        time.sleep(2)
        self.port.reset_input_buffer()
        self.port.reset_output_buffer()
        self._logger.info("GRBL woken up")
        
    def __del__(self):
        self.write_ok("!")
        self.port.close()
    
    def write(self, msg):
        """Writes a message to the serial port"""
        self.port_lock.acquire()
        self.port.write((msg.strip() + "\r\n").encode('ascii'))
        self.port.flush()
        self.port_lock.release()

    def read(self):
        """Reads a message from GRBL"""
        self.port_lock.acquire()
        resp = self.port.readline().decode('ascii').strip()
        self.port_lock.release()
        return resp

    def write_ok(self, msg : str, attempts = float('inf')):
        """
        Writes a message to GRBL, blocks until response acknowledged
        Args:
            msg: message to write.
            attempts: number of times an "ok" response is checked for.
        """
        self.port_lock.acquire()
        self.write(msg)
        while attempts > 0:
            resp = self.read()
            if "ok" in resp:
                self.port_lock.release()
                return
            elif "error" in resp:
                raise Exception("Error thrown by GRBL: " + resp)
            attempts -= 1
        raise Exception("No OK response received.")

    def read_state(self):
        """Attempts to read state, will block until it receives the state message"""
        self.port_lock.acquire()
        self.write("?")
        while True:
            resp = self.read()
            if "<" in resp:
                self.port_lock.release()
                return resp
    
    def home(self):
        self.write_ok('$H', 5)

    def unlock(self):
        self.write_ok("$X", 5)
        self._logger.info("Machine unlocked.")

    def zero(self):
        self.write_ok("G10 P0 L20 X0 Y0 Z0", 5)
        self._logger.info("Machine zero set.")

    
    def rapid(self, coords):
        self.write_ok("G0 " + " ".join([key + str(value) for key, value in coords.items()]))
        self._logger.info("Rapid move to " + str(coords))

    def cut(self, coords, feed):
        self.write_ok("G1 " + " ".join([key + str(value) for key, value in coords.items()]) + " F" + str(feed))
        self._logger.info("Cut move to " + str(coords))

    def get_state(self):
        resp = self.read_state()
        msgs = resp.strip("<>").split("|")
        state = msgs[0]
        others = msgs[1:]
        reports = {other.split(":")[0]:other.split(":")[1] for other in others}
        x, y, z = [float(i) for i in reports["WPos"].split(",")]
        feed, _ = reports["FS"].split(",")
        dx, dy, dz = [cur - last for cur, last in zip((x, y, z), self.last_position)]
        self.last_position = (x, y, z)
        return {'state': state, 'wpos':{'x': x, 'y': y, 'z': z}, 'feed': feed, 'direction': {'dx': dx, 'dy': dy, 'dz': dz}}

    def hold_until_still(self):
        self._logger.info("Holding until Idle")
        while (self.get_state()['state'] != "Idle"):
            time.sleep(0.1)
        self._logger.info("Idle detected")

    def hold_until_condition(self, condition):
        self._logger.info("Holding until condition met")
        time.sleep(0.1)
        while (not condition(self.get_state())):
            time.sleep(0.1)
        self._logger.info("Condition met")


    def sensorFunction(self):
        return ('machine', self.get_state())


class Spindle(Sensor):
    REDUCTION = 4

    def __init__(self, port):
        super().__init__()
        self._logger = logging.Logger("Spindle")
        self._logger.setLevel(logging.DEBUG)
        self._logger.info("Waking up VESC")
        self.spindle = pyvesc.VESC(port)
        self._logger.info("VESC Ready")
        self.spindle_lock = threading.Lock()

        self.avg_current = 0

    def close(self):
        self.spindle_lock.acquire()
        self.spindle.set_rpm(0)
        self.spindle.stop_heartbeat()
        self.spindle_lock.release()

    def find_avg_current(self):
        self.avg_current = 0
        self.spindle_lock.acquire()
        time.sleep(1)
        samples = 300
        for _ in range(samples):
            time.sleep(0.01)
            measurements = self.spindle.get_measurements()
            self.avg_current += measurements.avg_motor_current

        self.spindle_lock.release()
        self.avg_current /= samples


    def set_w(self, w):
        self.spindle_lock.acquire()
        rpm = int(w * (60 / (2 * np.pi))) * 4
        self.spindle.set_rpm(rpm)
        time.sleep(1)
        confirmation = self.spindle.get_measurements()
        self.spindle_lock.release()
        self._logger.info("Attempted to set RPM of " + str(rpm) + ", actual RPM is " + str(confirmation.rpm))
        if rpm > 0 and confirmation.rpm < rpm * 0.75:
            self.spindle.set_rpm(0)
            raise Exception("Spindle did not approach requested speed of " + str(rpm) + ", actual speed is " + str(confirmation.rpm))

    def sensorFunction(self):
        self.spindle_lock.acquire()
        measurements = self.spindle.get_measurements()
        self.spindle_lock.release()
        all_measurements = {attr: getattr(measurements, attr) for attr in dir(measurements) if (attr[0] != '_' and "fields" not in attr)}
        all_measurements['avg_current'] = self.avg_current
        all_measurements['torque'] = (measurements.avg_motor_current - self.avg_current) * 0.0348134
        return ('spindle', all_measurements)

class TFD(Sensor):

    # completely insane strategy:
    """
    0 bits to 0 kg
    677868.0 bits to one kg
    4122187.0 bits to five kg
    5050401.0 bits to 6 kg
    linear fit through all points leads to conversion value of 832116 bits to one kg
    hella inaccurate but eh
    cubic equation from kg to bits:
    2781.62 * x ** 3 + 745886 * x
    unknown if it works past 6kg but eh it's a start lol
    https://bit.ly/3c6lR2M for fitting memes
    """

    BITS_TO_N = 227753.0 / 9.81

    def __init__(self, port):
        super().__init__()
        self.port = serial.Serial(port, 115200, timeout=0.5)
        self._logger = logging.Logger("TFD")
        self._logger.setLevel(logging.DEBUG)
        self._logger.info("Waking up TFD")
        self.port.write("F".encode('ascii'))
        self.port.flush()
        time.sleep(10)
        self.port.reset_input_buffer()
        self._logger.info("TFD Ready")


    def get_force(self):
        self.port.write("F".encode('ascii'))
        self.port.flush()
        resp = self.port.readline().decode('ascii').strip()
        return float(resp) / TFD.BITS_TO_N


    def calibrate(self):
        self.port.write("T".encode('ascii'))
        self.port.flush()
        while True:
            if "TARED" in self.port.readline().decode('ascii'):
                return


    def sensorFunction(self):
        return ('tfd', self.get_force())

class Spindle_Applied(Sensor):

    RS485_ADDRESS = 0
    I_TO_T = 1
    CALIBRATION_POINTS = 100

    def __init__(self, port):
        self.port = serial.Serial(port, 38400, timeout=0.5)
        self.torque_loss = 0

        self.port_lock = threading.RLock()
        self.port.reset_input_buffer()
        self.port.reset_output_buffer()
        
    def __del__(self):
        self.write_ack("SJ")
        self.port.close()

    def write_ack(self, msg):
        self.port_lock.acquire()
        full_msg = Spindle_Applied.RS485_ADDRESS +\
                   msg +\
                   "\r"
        self.port.write(full_msg.encode('ascii'))
        self.port.flush()
        self.port_lock.release()
        response = self.port.read().decode('ascii')
        if response in ('%', '*'):
            return
        elif response == '?':
            raise Exception("Error sent back from drive.")
        elif not response:
            raise Exception("Command was not understood by drive")
        else:
            raise Exception("Unknown ack received: " + response)

    def write_get_response(self, msg):
        self.port_lock.acquire()
        self.write_ack(msg)
        response = self.port.readline().decode('ascii')
        self.port_lock.release()
        assert response[0] == Spindle_Applied.RS485_ADDRESS , "Address of response incorrect, was actually " + response[0]
        assert response[1:3] == msg[0: 2], "Command of response incorrect, was actually " + response[1:3]
        assert response[4] == "=", "No '=' in response, was actually " + response[4]
        return response[5:]

    def set_w(self, w):
        rpm = int(w * (60 / (2 * np.pi))) 
        self.write_ack("JA50")
        self.write_ack("JL25")
        self.write_ack("CJ")
        time.sleep(1)
        actual_rpm = self.write_get_response("IV")
        if rpm != 0 and actual_rpm < rpm * 0.75:
            self.write_ack("SJ")
            raise Exception("Failed to reach speed of " + str(rpm) + "RPM. Actually reached " + str(actual_rpm) + "RPM.")

    def get_torque(self, calibrated = True):
        I = float(self.write_get_response("IQ"))
        return I * Spindle_Applied.I_TO_T - (self.torque_loss if calibrated else 0)

    def calibrate(self):
        Ts = list()
        for i in range(Spindle_Applied.CALIBRATION_POINTS):
            time.sleep(0.01)
            Ts.append(self.get_torque(calibrated = False))
        
        self.torque_loss = np.median(Ts)

    def sensorFunction(self):
        return ('spindle', self.get_torque())