-
Notifications
You must be signed in to change notification settings - Fork 14
/
ftrobopy.py
3724 lines (3132 loc) · 141 KB
/
ftrobopy.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
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
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
"""
**************************************************************************
**ftrobopy** - Ansteuerung des fischertechnik TXT Controllers in Python
**************************************************************************
(c) 2015, 2016, 2017, 2018, 2019, 2020, 2021, 2022, 2023 by Torsten Stuehn
"""
from __future__ import print_function
from os import system
import os
import platform
import sys
import socket
import threading
import struct
import time
from math import sqrt, log
try:
import ftTA2py
except:
pass
__author__ = "Torsten Stuehn"
__copyright__ = "Copyright 2015 - 2023 by Torsten Stuehn"
__credits__ = "fischertechnik GmbH"
__license__ = "MIT License"
__version__ = "2.0.0"
__maintainer__ = "Torsten Stuehn"
__email__ = "stuehn@mailbox.org"
__status__ = "stable"
__date__ = "06/15/2023"
try:
xrange
except NameError:
xrange = range
def version():
"""
Gibt die Versionsnummer des ftrobopy-Moduls zurueck
:return: Versionsnummer (float)
Anwendungsbeispiel:
>>> print("ftrobopy Version ", ftrobopy.ftrobopy.version())
"""
return __version__ + " " + __status__
def default_error_handler(message, exception):
print(message)
return False
def default_data_handler(ftTXT):
pass
class ftTXT(object):
"""
Basisklasse zum fischertechnik TXT Computer.
Implementiert das Protokoll zum Datenaustausch ueber Unix Sockets.
Die Methoden dieser Klasse werden typischerweise vom End-User nicht direkt aufgerufen, sondern
nur indirekt ueber die Methoden der Klasse ftrobopy.ftrobopy, die eine Erweiterung der Klasse ftrobopy.ftTXTBase darstellt.
Die folgenden Konstanten werden in der Klasse definiert:
+ ``C_VOLTAGE = 0`` *Zur Verwendung eines Eingangs als Spannungsmesser*
+ ``C_SWITCH = 1`` *Zur Verwendung eines Eingangs als Taster*
+ ``C_RESISTOR = 1`` *Zur Verwendung eines Eingangs als Widerstand, z.B. Photowiderstand*
+ ``C_ULTRASONIC = 3`` *Zur Verwendung eines Eingangs als Distanzmesser*
+ ``C_ANALOG = 0`` *Eingang wird analog verwendet*
+ ``C_DIGITAL = 1`` *Eingang wird digital verwendet*
+ ``C_OUTPUT = 0`` *Ausgang (O1-O8) wird zur Ansteuerung z.B. einer Lampe verwendet*
+ ``C_MOTOR = 1`` *Ausgang (M1-M4) wird zur Ansteuerung eines Motors verwendet*
"""
C_VOLTAGE = 0
C_SWITCH = 1
C_RESISTOR = 1
C_ULTRASONIC = 3
C_ANALOG = 0
C_DIGITAL = 1
C_OUTPUT = 0
C_MOTOR = 1
# command codes for TXT motor shield
C_MOT_CMD_CONFIG_IO = 0x51
C_MOT_CMD_EXCHANGE_DATA = 0x54
# input configuration codes for TXT motor shield
C_MOT_INPUT_DIGITAL_VOLTAGE = 0
C_MOT_INPUT_DIGITAL_5K = 1
C_MOT_INPUT_ANALOG_VOLTAGE = 2
C_MOT_INPUT_ANALOG_5K = 3
C_MOT_INPUT_ULTRASONIC = 4
C_EXT_MASTER = 0 # use TXT master
C_EXT_SLAVE = 1 # use TXT slave extension
# sound commands and messages for spi communication to motor shield (only needed in direct mode)
C_SND_CMD_STATUS = 0x80
C_SND_CMD_DATA = 0x81
C_SND_CMD_RESET = 0x90
C_SND_MSG_RX_CMD = 0xBB # return if in CMD mode
C_SND_MSG_RX_DATA = 0x55 # return if in DATA mode
C_SND_MSG_RX_COMPLETE = 0xAA # return after all data has been transfered
C_SND_MSG_ERR_SIZE = 0xFE # spi buffer overflow
C_SND_MSG_ERR_FULL = 0xFF # spi communication not possible, all buffers are full
C_SND_FRAME_SIZE = 441 # 22050 Hz, 20ms
# sound communication state machine
C_SND_STATE_IDLE = 0x00
C_SND_STATE_START = 0x01
C_SND_STATE_STOP = 0x02
C_SND_STATE_RUNNING = 0x03
C_SND_STATE_DATA = 0x04
def __init__(self, host='127.0.0.1', port=65000, serport='/dev/ttyO2' , on_error=default_error_handler, on_data=default_data_handler, directmode=False, use_extension=False, use_TransferAreaMode=False):
"""
Initialisierung der ftTXT Klasse:
* Alle Ausgaenge werden per default auf 1 (=Motor) gesetzt
* Alle Eingaenge werden per default auf 1, 0 (=Taster, digital) gesetzt
* Alle Zaehler werden auf 0 gesetzt
:param host: Hostname oder IP-Nummer des TXT Moduls
:type host: string
- '127.0.0.1' im Downloadbetrieb
- '192.168.7.2' im USB Offline-Betrieb
- '192.168.8.2' im WLAN Offline-Betrieb
- '192.168.9.2' im Bluetooth Offline-Betreib
:param port: Portnummer (normalerweise 65000)
:type port: integer
:param serport: Serieller Port zur direkten Ansteuerung der Motorplatine des TXT
:type serport: string
:param on_error: Errorhandler fuer Fehler bei der Kommunikation mit dem Controller (optional)
:type port: function(str, Exception) -> bool
:param directmode: Default: False. Direkte serielle Kommunikation zwischen TXT Linux-Platine und Motorplatine unter Umgehung der Socket-Schnittstelle (nur sinnvoll in der CFW im offline Modus)
:type boolean:
:param use_extension: Default: False. Erlaubt einen zweiten TXT-Kontroller am Extension Port.
:type boolean:
:param use_TransferAreaMode: Default False. Verwendet eine shared-Memory Methode (TransferAreaMode) zum Datenaustausch mit der Motorplatine (nur sinnvoll im Offline Modus)
:type boolean:
:return: Leer
Anwedungsbeispiel:
>>> import ftrobopy
>>> txt = ftrobopy.ftTXT('192.168.7.2', 65000)
"""
self._m_devicename = b''
self._m_version = 0
self._host = host
self._port = port
self._ser_port = serport
self.handle_error = on_error
self.handle_data = on_data
self._directmode = directmode
self._use_extension = use_extension
self._use_TransferAreaMode = use_TransferAreaMode
self._spi = None
self._SoundFilesDir = ''
self._SoundFilesList = []
self._sound_state = 0 # current state of sound-communication state-machine in 'direct'-mode
self._sound_data = [] # curent buffer for sound data (wav-file[44:])
self._sound_data_idx = 0
self._sound_current_rep = 0
self._sound_current_volume = 100
self._TransferArea_isInitialized = False
if self._use_TransferAreaMode:
if (ftTA2py.initTA()) == 1:
self._TransferArea_isInitialized = True
else:
print("Error: Transfer Area could not be initialized! Please check if ftTA2py.so exists.")
sys.exit(-1)
elif self._directmode:
if use_extension:
print("Error: direct-mode does currently not support TXT slave extensions.")
sys.exit(-1)
import serial
self._ser_ms = serial.Serial(self._ser_port, 230000, timeout=1)
self._sock = None
import spidev
try:
self._spi = spidev.SpiDev(1,0) # /dev/spidev1.0
except error:
print("Error opening SPI device (this is needed for sound in 'direct'-mode).")
# print(error)
self._spi = None
if self._spi:
self._spi.mode = 3
self._spi.bits_per_word = 8
self._spi.max_speed_hz = 1000000
# reset sound on motor shield
res = self._spi.xfer([self.C_SND_CMD_RESET, 0, 0])
if res[0] != self.C_SND_MSG_RX_CMD:
print("Error: initial sound reset returns: ", ''.join(["0x%02X " % x for x in res]).split())
sys.exit(-1)
# check if we are running on original-firmware or on community-firmware
# this is only needed to find the original Sound Files
if os.path.isdir('/rom'):
self._SoundFilesDir = ('/rom/opt/knobloch/SoundFiles/')
else:
self._SoundFilesDir = ('/opt/knobloch/SoundFiles/')
self._SoundFilesList = os.listdir(self._SoundFilesDir)
self._SoundFilesList.sort()
else:
self._sock=socket.socket()
self._sock.settimeout(5)
self._sock.connect((self._host, self._port))
self._sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._sock.setblocking(1)
self._ser_ms = None
self._i2c_sock=socket.socket()
self._i2c_sock.settimeout(5)
self._txt_stop_event = threading.Event()
self._camera_stop_event = threading.Event()
self._bt_joystick_stop_event = threading.Event()
self._txt_stop_event.set()
self._camera_stop_event.set()
self._bt_joystick_stop_event.set()
self._exchange_data_lock = threading.RLock()
self._camera_data_lock = threading.Lock()
self._bt_joystick_lock = threading.RLock()
self._socket_lock = threading.Lock()
self._txt_thread = None
self._camera_thread = None
self._bt_joystick_thread = None
self._update_status = 0
self._update_timer = time.time()
self._cycle_count = 0
self._sound_timer = self._update_timer
self._sound_length = 0
self._config_id = [0,0] # [0]:master [1]:slave
self._config_id_old = 0 # only used in direct mode
self._TransferDataChanged = False
self._ftX1_pgm_state_req = 0
self._ftX1_old_FtTransfer = 0
self._ftX1_dummy = b'\x00\x00'
self._ftX1_motor = [1,1,1,1,1,1,1,1]
self._ftX1_uni = [1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00',
1,1,b'\x00\x00']
self._ftX1_cnt = [1,b'\x00\x00\x00',
1,b'\x00\x00\x00',
1,b'\x00\x00\x00',
1,b'\x00\x00\x00',
1,b'\x00\x00\x00',
1,b'\x00\x00\x00',
1,b'\x00\x00\x00',
1,b'\x00\x00\x00']
self._ftX1_motor_config = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
self._exchange_data_lock.acquire()
self._pwm = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
self._motor_sync = [0,0,0,0,0,0,0,0]
self._motor_dist = [0,0,0,0,0,0,0,0]
self._motor_cmd_id = [0,0,0,0,0,0,0,0]
self._counter = [0,0,0,0,0,0,0,0]
self._sound = [0,0]
self._sound_index = [0,0]
self._sound_repeat = [0,0]
self._current_input = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
self._current_counter = [0,0,0,0,0,0,0,0]
self._current_counter_value = [0,0,0,0,0,0,0,0]
self._current_counter_cmd_id = [0,0,0,0,0,0,0,0]
self._current_motor_cmd_id = [0,0,0,0,0,0,0,0]
self._current_sound_cmd_id = [0,0]
self._current_ir = [0 for i in range(26)]
self._ir_current_ljoy_left_right = [0,0,0,0,0] # -15 ... 15
self._ir_current_ljoy_up_down = [0,0,0,0,0] # -15 ... 15
self._ir_current_rjoy_left_right = [0,0,0,0,0] # -15 ... 15
self._ir_current_rjoy_up_down = [0,0,0,0,0] # -15 ... 15
self._ir_current_buttons = [0,0,0,0,0] # 0:OFF 1:ON
self._ir_current_dip_switch = [0,0,0,0,0] # 0:all 1:0-0 2:1-0 3:0-1 4:1-1
self._bt_ljoy_left_right = 0 # -32767 ... 32512
self._bt_ljoy_up_down = 0 # -32767 ... 32512
self._bt_rjoy_left_right = 0 # -32767 ... 32512
self._bt_rjoy_up_down = 0 # -32767 ... 32512
#self._bt_buttons = 0 # the bluetooth remote has no detectable buttons
#self._bt_dip_switch = 0 # and no dip switches; currently only one bt remote can be connected
self._current_power = 0 # voltage of battery or power supply
self._current_temperature = 0 # temperature of ARM CPU
self._current_reference_power = 0
self._current_extension_power = 0
self._debug = []
self._firstUpdateConfig = [True, True]
self._exchange_data_lock.release()
def stopTransferArea(self):
if self._TransferArea_isInitialized:
ftTA2py.stopTA()
def isOnline(self):
if self._use_TransferAreaMode:
return self._TransferArea_isInitialized
else:
return (not self._txt_stop_event.is_set()) and (self._txt_thread is not None)
def cameraIsOnline(self):
return (not self._camera_stop_event.is_set()) and (self._camera_thread is not None)
def queryStatus(self):
"""
Abfrage des Geraetenamens und der Firmware Versionsnummer des TXT
Nach dem Umwandeln der Versionsnummer in einen Hexadezimalwert, kann
die Version direkt abgelesen werden.
:return: Geraetename (string), Versionsnummer (integer)
Anwendungsbeispiel:
>>> name, version = txt.queryStatus()
"""
if self._use_TransferAreaMode:
# TODO
# not sure how to detect version yet, just set standard value
self._m_devicename = 'TXT TransferAreaMode'
self._m_version = 0x4060600
self._m_firmware = 'firmware version not detected'
return self._m_devicename, self._m_version
elif self._directmode:
# not sure how to detect version yet, just set standard value
self._m_devicename = 'TXT direct'
self._m_version = 0x4010500
self._m_firmware = 'firmware version not detected'
return self._m_devicename, self._m_version
m_id = 0xDC21219A
m_resp_id = 0xBAC9723E
buf = struct.pack('<I', m_id)
self._socket_lock.acquire()
res = self._sock.send(buf)
data = self._sock.recv(512)
self._socket_lock.release()
fstr = '<I16sI'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id, m_devicename, m_version = struct.unpack(fstr, data)
else:
m_devicename = ''
m_version = 0
if response_id != m_resp_id:
print('WARNING: ResponseID ', hex(response_id),'of queryStatus command does not match')
self._m_devicename = m_devicename.decode('utf-8').strip('\x00')
self._m_version = m_version
v1 = int(hex(m_version)[2])
v2 = int(hex(m_version)[3:5])
v3 = int(hex(m_version)[5:7])
self._m_firmware = 'firmware version '+str(v1)+'.'+str(v2)+'.'+str(v3)
return m_devicename, m_version
def i2c_read(self, dev, reg, reg_len=1, data_len=1, debug=False):
"""
I2C lesen
:param dev: Die I2C Geraeteadresse
:type dev: integer
:param reg: Das Register, das ausgelesen werden soll (Untergeraeteadresse)
:type reg: integer
:param reg_len: Die Laenge des Registers in Byte (default=1)
:type reg_len: integer
:param data_len: Die Laenge der Datenantwort (default=1)
:type data_len: integer
:return: Die Datenantwort des I2C Geraetes (string)
Anwendungsbeispiel:
>>> res=txt.i2c_read(0x18, 0x3f, data_len=6)
>>> x,y,z=struct.unpack('<hhh', res)
>>> print("Beschleunigung des BMX055 Kombisensors in x-, y- und z-Richtung = ", x >> 4 , y >> 4, z >> 4)
"""
m_id = 0xB9DB3B39
m_resp_id = 0x87FD0D90
m_command = 0x01
buf = struct.pack('>IBIIHH', m_id, m_command, dev, reg_len, data_len, reg)
if debug:
print("i2c_read, sendbuffer: ", end='')
for k in buf: print(format(int(k), '02X'), end=' ')
print()
res = self._i2c_sock.send(buf)
data = self._i2c_sock.recv(512)
if debug:
print("i2c_read, receivebuffer: ", end='')
for k in data: print(format(int(k), '02X'), end=' ')
print()
fstr = '>IBIHB'
for k in range(data_len): fstr+='B'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id=struct.unpack(fstr, data)[0]
if response_id != m_resp_id:
self.handle_error('WARNING: ResponseID %s of I2C read command does not match' % hex(response_id), None)
return None
return data[-data_len:]
def i2c_write(self, dev, reg, value, debug=False):
"""
I2C schreiben
:param dev: Die I2C Geraeteadresse
:type dev: integer
:param reg: Das Register, das beschrieben werden soll (Untergeraeteadresse)
:type reg: integer
:param value: Der in das Register zu schreibende Wert
:type value: integer (0-255)
:return: True (boolean) bei fehlerfreier Ausfuehrung, sonst "None"
Anwendungsbeispiel:
>>> # Settings for BMX055 acceleration
>>> txt.i2c_write(0x18, 0x3e, 0x80)
>>> txt.i2c_write(0x18, 0x0f, 0x0c)
>>> txt.i2c_write(0x18, 0x10, 0x0f)
"""
m_id = 0xB9DB3B39
m_resp_id = 0x87FD0D90
m_command = 0x02
buf = struct.pack('>IBIIIB',m_id, m_command, dev, 0x02, reg, value)
if debug:
print("i2c_write, sendbuffer: ", end='')
for k in buf: print(format(int(k), '02X'), end=' ')
print()
res = self._i2c_sock.send(buf)
data = self._i2c_sock.recv(512)
if debug:
print("i2c_write, receivebuffer: ", end='')
for k in data: print(format(int(k), '02X'), end=' ')
print()
fstr = '>III'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id =struct.unpack(fstr, data)[0]
if response_id != m_resp_id:
self.handle_error('WARNING: ResponseID %s of I2C write command does not match' % hex(response_id), None)
return None
return True
def i2c_write_bytes(self, dev, *argv):
m_id = 0xB9DB3B39
m_resp_id = 0x87FD0D90
m_lenth = 0
for i in argv:
m_lenth += 1
buf = struct.pack('>IBIIBBB', m_id, m_lenth, dev, m_lenth, 0x00, 0x00, 0x00)
for i in argv:
buf += struct.pack('B', i)
if debug:
print("i2c_write, sendbuffer: ", end='')
for k in buf: print(format(int(k), '02X'), end=' ')
print()
res = self._i2c_sock.send(buf)
data = self._i2c_sock.recv(512)
if debug:
print("i2c_write, receivebuffer: ", end='')
for k in data: print(format(int(k), '02X'), end=' ')
print()
fstr = '>III'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id = struct.unpack(fstr, data)[0]
if response_id != m_resp_id:
self.handle_error('WARNING: ResponseID %s of I2C write command does not match' % hex(response_id), None)
return None
return True
def i2c_write_buffer(self, dev, buffer, m_length, debug = False):
m_id = 0xB9DB3B39
buf = struct.pack('>IBIIBBB', m_id, m_length, dev, m_length, 0x00, 0x00, 0x00) + buffer
if debug:
print("i2c_write, sendbuffer: ", end='')
for k in buf: print(format(int(k), '02X'), end=' ')
print()
res = self._i2c_sock.send(buf)
data = self._i2c_sock.recv(512)
if debug:
print("i2c_write, receivebuffer: ", end='')
for k in data: print(format(int(k), '02X'), end=' ')
print()
fstr = '>III'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id = struct.unpack(fstr, data)[0]
if response_id != 0x87FD0D90:
self.handle_error('WARNING: ResponseID %s of I2C write command does not match' % hex(response_id), None)
return None
return True
def getDevicename(self):
"""
Liefert den zuvor mit queryStatus() ausgelesenen Namen des TXT zurueck
:return: Geraetename (string)
Anwendungsbeispiel:
>>> print('Name des TXT: ', txt.getDevicename())
"""
return self._m_devicename
def getVersionNumber(self):
"""
Liefert die zuvor mit queryStatus() ausgelesene Versionsnummer zurueck.
Um die Firmwareversion direkt ablesen zu koennen, muss diese Nummer noch in
einen Hexadezimalwert umgewandelt werden
:return: Versionsnummer (integer)
Anwendungsbeispiel:
>>> print(hex(txt.getVersionNumber()))
"""
return self._m_version
def getFirmwareVersion(self):
"""
Liefert die zuvor mit queryStatus() ausgelesene Versionsnummer als
Zeichenkette (string) zurueck.
:return: Firmware Versionsnummer (str)
Anwendungsbeispiel:
>>> print(txt.getFirmwareVersion())
"""
return self._m_firmware
def startOnline(self, update_interval=0.02):
"""
Startet den Onlinebetrieb des TXT und startet einen Python-Thread, der die Verbindung zum TXT aufrecht erhaelt.
:return: Leer
Anwendungsbeispiel:
>>> txt.startOnline()
"""
if self._TransferArea_isInitialized:
return
if self._directmode == True:
if self._txt_stop_event.is_set():
self._txt_stop_event.clear()
if self._txt_thread is None:
self._txt_thread = ftTXTexchange(txt=self, sleep_between_updates=update_interval, stop_event=self._txt_stop_event)
self._txt_thread.setDaemon(True)
self._txt_thread.start()
# keep_connection_thread is only needed when using SyncDataBegin/End in interactive python mode
#self._txt_keep_connection_stop_event = threading.Event()
#self._txt_keep_connection_thread = ftTXTKeepConnection(self, 1.0, self._txt_keep_connection_stop_event)
#self._txt_keep_connection_thread.setDaemon(True)
#self._txt_keep_connection_thread.start()
return None
if self._txt_stop_event.is_set():
self._txt_stop_event.clear()
else:
return
if self._txt_thread is None:
m_id = 0x163FF61D
m_resp_id = 0xCA689F75
buf = struct.pack('<I64s', m_id,b'')
self._socket_lock.acquire()
res = self._sock.send(buf)
data = self._sock.recv(512)
self._socket_lock.release()
fstr = '<I'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id, = struct.unpack(fstr, data)
if response_id != m_resp_id:
self.handle_error('WARNING: ResponseID %s of startOnline command does not match' % hex(response_id), None)
else:
self.updateConfig(self.C_EXT_MASTER)
if self._use_extension:
self.updateConfig(self.C_EXT_SLAVE)
self._txt_thread = ftTXTexchange(txt=self, sleep_between_updates=update_interval, stop_event=self._txt_stop_event)
self._txt_thread.setDaemon(True)
self._txt_thread.start()
# keep_connection_thread is needed when using SyncDataBegin/End in interactive python mode
self._txt_keep_connection_stop_event = threading.Event()
self._txt_keep_connection_thread = ftTXTKeepConnection(self, 1.0, self._txt_keep_connection_stop_event)
self._txt_keep_connection_thread.setDaemon(True)
self._txt_keep_connection_thread.start()
time.sleep(0.1)
self._i2c_sock.connect((self._host, self._port+2))
self._i2c_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._i2c_sock.setblocking(1)
return None
def stopOnline(self):
"""
Beendet den Onlinebetrieb des TXT und beendet den Python-Thread der fuer den Datenaustausch mit dem TXT verantwortlich war.
:return: Leer
Anwendungsbeispiel:
>>> txt.stopOnline()
"""
if self._TransferArea_isInitialized:
self.stopTransferArea()
return None
if self._directmode:
self._txt_stop_event.set()
self._txt_thread = None
if self._spi:
self._spi.close()
return None
if not self.isOnline():
return
self._txt_stop_event.set()
self._txt_keep_connection_stop_event.set()
m_id = 0x9BE5082C
m_resp_id = 0xFBF600D2
buf = struct.pack('<I', m_id)
self._socket_lock.acquire()
res = self._sock.send(buf)
data = self._sock.recv(512)
self._socket_lock.release()
fstr = '<I'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id, = struct.unpack(fstr, data)
if response_id != m_resp_id:
self.handle_error('WARNING: ResponseID %s of stopOnline command does not match' % hex(response_id), None)
self._txt_thread = None
return None
def setConfig(self, M, I, ext=C_EXT_MASTER):
"""
Einstellung der Konfiguration der Ein- und Ausgaenge des TXT.
Diese Funktion setzt nur die entsprechenden Werte in der ftTXT-Klasse.
Zur Uebermittlung der Werte an den TXT wird die updateConfig-Methode verwendet.
:param M: Konfiguration der 4 Motorausgaenge (0=einfacher Ausgang, 1=Motorausgang)
:type M: int[4]
- Wert=0: Nutzung der beiden Ausgaenge als einfache Outputs
- Wert=1: Nutzung der beiden Ausgaenge als Motorausgang (links-rechts-Lauf)
:param I: Konfiguration der 8 Eingaenge
:type I: int[8][2]
:return: Leer
Anwendungsbeispiel:
- Konfiguration der Ausgaenge M1 und M2 als Motorausgaenge
- Konfiguration der Ausgaenge O5/O6 und O7/O8 als einfache Ausgaenge
- Konfiguration der Eingaenge I1, I2, I6, I7, I8 als Taster
- Konfiguration des Eingangs I3 als Ultraschall Entfernungsmesser
- Konfiguration des Eingangs I4 als analoger Spannungsmesser
- Konfiguration des Eingangs I5 als analoger Widerstandsmesser
>>> M = [txt.C_MOTOR, txt.C_MOTOR, txt.C_OUTPUT, txt.C_OUTPUT]
>>> I = [(txt.C_SWITCH, txt.C_DIGITAL),
(txt.C_SWITCH, txt.C_DIGITAL),
(txt.C_ULTRASONIC, txt.C_ANALOG),
(txt.C_VOLTAGE, txt.C_ANALOG),
(txt.C_RESISTOR, txt.C_ANALOG),
(txt.C_SWITCH, txt.C_DIGITAL),
(txt.C_SWITCH, txt.C_DIGITAL),
(txt.C_SWITCH, txt.C_DIGITAL)]
>>> txt.setConfig(M, I)
>>> txt.updateConfig()
"""
self._config_id[ext] += 1
# Configuration of motors
# 0=single output O1/O2
# 1=motor output M1
# self.ftX1_motor = [M[0],M[1],M[2],M[3]] # BOOL8[4]
self._ftX1_motor[4*ext:4*ext+4] = M
# Universal input mode, see enum InputMode:
# MODE_U=0
# MODE_R=1
# MODE_R2=2
# MODE_ULTRASONIC=3
# MODE_INVALID=4
# print("setConfig I=", I)
self._ftX1_uni[24*ext:24*ext+24] = [I[0][0],I[0][1],b'\x00\x00',
I[1][0],I[1][1],b'\x00\x00',
I[2][0],I[2][1],b'\x00\x00',
I[3][0],I[3][1],b'\x00\x00',
I[4][0],I[4][1],b'\x00\x00',
I[5][0],I[5][1],b'\x00\x00',
I[6][0],I[6][1],b'\x00\x00',
I[7][0],I[7][1],b'\x00\x00']
return None
def getConfig(self, ext=C_EXT_MASTER):
"""
Abfrage der aktuellen Konfiguration des TXT
:return: M[4], I[8][2]
:rtype: M:int[4], I:int[8][2]
Anwendungsbeispiel: Aenderung des Eingangs I2 auf analoge Ultraschall Distanzmessung
- Hinweis: Feldelemente werden in Python typischerweise durch die Indizes 0 bis N-1 angesprochen
- Der Eingang I2 des TXT wird in diesem Beispiel ueber das Feldelement I[1] angesprochen
>>> M, I = txt.getConfig()
>>> I[1] = (txt.C_ULTRASONIC, txt.C_ANALOG)
>>> txt.setConfig(M, I)
>>> txt.updateConfig()
"""
m = self._ftX1_motor[4*ext:4*ext+4]
i = self._ftX1_uni[24*ext:24*ext+24]
ii = [(i[0], i[1]), (i[3], i[4]),(i[6], i[7]),(i[9], i[10]),(i[12], i[13]),(i[15], i[16]),(i[18], i[19]),(i[21], i[22]) ]
return m, ii
def updateConfig(self, ext=C_EXT_MASTER):
"""
Uebertragung der Konfigurationsdaten fuer die Ein- und Ausgaenge zum TXT
:return: Leer
Anwendungsbeispiel:
>>> txt.setConfig(M, I)
>>> txt.updateConfig()
"""
if self._use_TransferAreaMode:
# in TransferAreaMode the configuration is automatically updated
return
if self._directmode:
# in direct mode i/o port configuration is performed automatically in exchangeData thread
return
if not self._firstUpdateConfig[ext]:
if not self.isOnline():
self.handle_error("Controller must be online before updateConfig() is called", None)
return
self._firstUpdateConfig[ext] = False
m_id = 0x060EF27E
m_resp_id = 0x9689A68C
self._config_id[ext] += 1
fields = [m_id, self._config_id[ext], ext]
fields.append(self._ftX1_pgm_state_req)
fields.append(self._ftX1_old_FtTransfer)
fields.append(self._ftX1_dummy)
fields += self._ftX1_motor[4*ext:4*ext+4]
fields += self._ftX1_uni[24*ext:24*ext+24]
fields += self._ftX1_cnt[8*ext:8*ext+8]
fields += self._ftX1_motor_config[16*ext:16*ext+16]
buf = struct.pack('<Ihh B B 2s BBBB BB2s BB2s BB2s BB2s BB2s BB2s BB2s BB2s B3s B3s B3s B3s 16h', *fields)
self._socket_lock.acquire()
res = self._sock.send(buf)
data = self._sock.recv(512)
self._socket_lock.release()
fstr = '<I'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id, = struct.unpack(fstr, data)
if response_id != m_resp_id:
self.handle_error('WARNING: ResponseID %s of updateConfig command does not match' % hex(response_id), None)
# Stop the data exchange thread and the keep connection thread if we were online
self._txt_stop_event.set()
self._txt_keep_connection_stop_event.set()
return None
def startCameraOnline(self, width=320, height=240, fps=15):
"""
Startet den Prozess auf dem TXT, der das aktuelle Camerabild ueber Port 65001 ausgibt und startet einen Python-Thread,
der die Cameraframes kontinuierlich vom TXT abholt. Die Frames werden vom TXT im jpeg-Format angeliefert.
Es wird nur das jeweils neueste Bild aufgehoben.
Nach dem Starten des Cameraprozesses auf dem TXT vergehen bis zu 2 Sekunden, bis des erste Bild vom TXT gesendet wird.
*Anmerkung:* Falls diese Funktion im Offline-Modus auf dem TXT unter der CFW (Community Firmware Version >0.9) verwendet werden soll,
muss zuvor die ftGUI-App auf dem TXT gestartet werden. Im 'direct'-Modus stehen die Camera-Funktionen von ftrobopy nicht zur Verfuegung.
In diesem Fall sollte die openCV-Bibliothek oder ftrobopylib.so verwendet werden.
Anwendungsbeispiel:
Startet den Cameraprozess, wartet 2.5 Sekunden und speichert das eingelesene Bild als Datei 'txtimg.jpg' ab.
>>> txt.startCameraOnline()
>>> time.sleep(2.5)
>>> pic = txt.getCameraFrame()
>>> with open('txtimg.jpg','wb') as f:
>>> f.write(bytearray(pic))
"""
if self._directmode:
# ftrobopy.py does not support camera in direct mode, please use native camera support (e.g. ftrobopylib.so or opencv)
return
if self._camera_stop_event.is_set():
self._camera_stop_event.clear()
else:
return
if self._camera_thread is None:
m_id = 0x882A40A6
m_resp_id = 0xCF41B24E
self._m_width = width
self._m_height = height
self._m_framerate = fps
self._m_powerlinefreq = 0 # 0=auto, 1=50Hz, 2=60Hz
buf = struct.pack('<I4i', m_id,
self._m_width,
self._m_height,
self._m_framerate,
self._m_powerlinefreq)
self._socket_lock.acquire()
res = self._sock.send(buf)
data = self._sock.recv(512)
self._socket_lock.release()
fstr = '<I'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id, = struct.unpack(fstr, data)
if response_id != m_resp_id:
print('WARNING: ResponseID ', hex(response_id),' of startCameraOnline command does not match')
else:
self._camera_thread = camera(self._host, self._port+1, self._camera_data_lock, self._camera_stop_event)
self._camera_thread.setDaemon(True)
self._camera_thread.start()
return
def stopCameraOnline(self):
"""
Beendet den lokalen Python-Camera-Thread und den Camera-Prozess auf dem TXT.
Anwendungsbeispiel:
>>> txt.stopCameraOnline()
"""
if self._directmode:
return
if not self.cameraIsOnline():
return
self._camera_stop_event.set()
m_id = 0x17C31F2F
m_resp_id = 0x4B3C1EB6
buf = struct.pack('<I', m_id)
self._socket_lock.acquire()
res = self._sock.send(buf)
data = self._sock.recv(512)
self._socket_lock.release()
fstr = '<I'
response_id = 0
if len(data) == struct.calcsize(fstr):
response_id, = struct.unpack(fstr, data)
if response_id != m_resp_id:
print('WARNING: ResponseID ', hex(response_id),' of stopCameraOnline command does not match')
self._camera_thread = None
return
def getCameraFrame(self):
"""
Diese Funktion liefert das aktuelle Camerabild des TXT zurueck (im jpeg-Format).
Der Camera-Prozess auf dem TXT muss dafuer vorher gestartet worden sein.
*Anmerkung:* Falls diese Funktion im Offline-Modus auf dem TXT unter der CFW (Community Firmware Version >0.9) verwendet werden soll,
muss zuvor die ftGUI-App auf dem TXT gestartet werden. Im 'direct'-Modus stehen die Camera-Funktionen von ftrobopy nicht zur Verfuegung.
In diesem Fall sollte die openCV-Bibliothek oder ftrobopylib.so verwendet werden.
Anwendungsbeispiel:
>>> pic = txt.getCameraFrame()
:return: jpeg Bild
"""
if self._directmode:
return
if self.cameraIsOnline():
count = 0
frame = None
while frame == None:
frame = self._camera_thread.getCameraFrame()
count += 1
if frame != None:
if len(frame) == 0:
frame = None
if count > 100:
print('Timeout while getting new frame from camera')
return None
time.sleep(0.01)
return frame
else:
return None
def incrMotorCmdId(self, idx, ext=C_EXT_MASTER):
"""
Erhoehung der sog. Motor Command ID um 1.
Diese Methode muss immer dann aufgerufen werden, wenn die Distanzeinstellung eines Motors (gemessen ueber die 4 schnellen Counter-Eingaenge)
geaendert wurde oder wenn ein Motor mit einem anderen Motor synchronisiert werden soll. Falls nur die Motorgeschwindigkeit veraendert wurde,
ist der Aufruf der incrMotorCmdId()-Methode nicht notwendig.
:param idx: Nummer des Motorausgangs
:type idx: integer
Achtung:
* Die Zaehlung erfolgt hier von 0 bis 3, idx=0 entspricht dem Motorausgang M1 und idx=3 entspricht dem Motorausgang M4
Anwendungsbeispiel:
Der Motor, der am TXT-Anschluss M2 angeschlossen ist, soll eine Distanz von 200 (Counterzaehlungen) zuruecklegen.
>>> txt.setMotorDistance(1, 200)
>>> txt.incrMotorCmdId(1)
"""
if self._use_TransferAreaMode:
ftTA2py.fX1out_incr_motor_cmd_id(ext,idx)
else:
self._exchange_data_lock.acquire()
self._motor_cmd_id[4*ext+idx] += 1
self._motor_cmd_id[4*ext+idx] &= 0x07
self._exchange_data_lock.release()
self._TransferDataChanged = True
return None
def getMotorCmdId(self, idx=None, ext=C_EXT_MASTER):
"""
Liefert die letzte Motor Command ID eines Motorausgangs (oder aller Motorausgaenge als array) zurueck.
:param idx: Nummer des Motorausgangs. Falls dieser Parameter nicht angeben wird, wird die Motor Command ID aller Motorausgaenge als Array[4] zurueckgeliefert.
:type idx: integer
:return: Die Motor Command ID eines oder aller Motorausgaenge
:rtype: integer oder integer[4] array
Anwendungsbeispiel:
>>> letzte_cmd_id = txt.getMotorCmdId(4)
"""
if self._use_TransferAreaMode:
if idx != None:
ret=ftTA2py.fX1in_motor_ex_cmd_id(ext,idx)
else:
ret=[ftTA2py.fX1in_motor_ex_cmd_id(ext,0), ftTA2py.fX1in_motor_ex_cmd_id(ext,1), ftTA2py.fX1in_motor_ex_cmd_id(ext,2), ftTA2py.fX1in_motor_ex_cmd_id(ext,3)]
else:
if idx != None:
ret=self._motor_cmd_id[4*ext+idx]
else:
ret=self._motor_cmd_id[4*ext:4*ext+4]
return ret
def cameraOnline(self):
"""
Mit diesem Befehl kann abgefragt werden, ob der Camera-Prozess gestartet wurde
:return:
:rtype: boolean
"""
return self._camera_online
def getSoundCmdId(self, ext=C_EXT_MASTER):
"""
Liefert die letzte Sound Command ID zurueck.
:return: Letzte Sound Command ID
:rtype: integer
Anwendungsbeispiel:
>>> last_sound_cmd_id = txt.getSoundCmdId()
"""
return self._sound[ext]
def incrCounterCmdId(self, idx, ext=C_EXT_MASTER):