From 13119dbb40ffad8533a73180a49867037b8ff067 Mon Sep 17 00:00:00 2001
From: Martin Teichmann <martin.teichmann@xfel.eu>
Date: Fri, 19 Feb 2021 16:37:49 +0100
Subject: [PATCH] now I can move motors on the CHEM
---
ebpfcat/chem.py | 39 +++++++++++++++++++--------------------
ebpfcat/devices.py | 7 ++++++-
ebpfcat/ethercat.py | 35 +++++++++++------------------------
ebpfcat/terminals.py | 3 ---
4 files changed, 36 insertions(+), 48 deletions(-)
diff --git a/ebpfcat/chem.py b/ebpfcat/chem.py
index 807b350..da9d7a5 100644
--- a/ebpfcat/chem.py
+++ b/ebpfcat/chem.py
@@ -1,5 +1,5 @@
from asyncio import gather, sleep, ensure_future
-from struct import unpack
+from struct import unpack, pack
from .terminals import EL7041, EL5042, Skip
from .devices import Motor, Counter, AnalogInput
from .ebpfcat import FastEtherCat, FastSyncGroup, SyncGroup
@@ -31,6 +31,10 @@ async def main():
#await te12.sdo_write(b"\x1a", 0x8018, 0x16) # singleturn
await te12.sdo_write(b" ", 0x8018, 0x16) # singleturn
+ await tm2.set_state(2)
+ await tm2.sdo_write(pack("<H", 2500), 0x8010, 0x1) # current
+ await tm2.sdo_write(pack("<H", 24000), 0x8010, 0x3) # voltage
+
for i in [1, 2, 3]:
print(f"S{i:x}", await te12.sdo_read(0xB018, i))
print(f"Pos", await te12.sdo_read(0x6010, 0x11))
@@ -53,31 +57,26 @@ async def main():
fsg = SyncGroup(ec, [m1, aie, aim])
+ m1.max_velocity = 10000
+ m1.proportional = 23
+ m1.target = 185525000
+
print("do start")
fsg.start()
print("did start")
- for i in range(10):
- print("P", m1.current_position, hex(aie.value), hex(aim.value))
- await sleep(0.02)
- for i in [1,2,3,4,5,6,7]:
- print(f"I{i:x}", unpack("B", await tm2.sdo_read(0x6010, i))[0])
- for i in [1,2,3]:
- print(f"O{i:x}", unpack("B", await tm2.sdo_read(0x7010, i))[0])
- print(f"V", unpack("H", await tm2.sdo_read(0x7010, 0x21))[0])
- m1.velocity = 1000
- m1.enable = True
+ print(f"P0", unpack("I", await tm2.sdo_read(0x6010, 0x14))[0])
await sleep(0.1)
- for i in [1,2,3]:
- print(f"O{i:x}", unpack("B", await tm2.sdo_read(0x7010, i))[0])
- print(f"V", unpack("H", await tm2.sdo_read(0x7010, 0x21))[0])
- m1.velocity = 0
- #m1.enable = True
- for i in range(10):
- print("P", m1.current_position, hex(aie.value), hex(aim.value))
+ m1.set_enable = True
+ for i in range(20):
+ print(f"M {m1.current_position:12} {m1.velocity:5} {aie.value:4x} {aim.value:4x}")
+ await sleep(0.2)
+ m1.set_enable = False
+ m1.max_velocity = 0
+ for i in range(20):
+ print(f"S {m1.current_position:12} {aie.value:4x} {aim.value:4x}")
await sleep(0.02)
- #m1.enable = False
- m1.velocity = 0
+ print(f"P1", unpack("I", await tm2.sdo_read(0x6010, 0x14))[0])
if __name__ == "__main__":
from asyncio import get_event_loop
diff --git a/ebpfcat/devices.py b/ebpfcat/devices.py
index e90a075..b82cae2 100644
--- a/ebpfcat/devices.py
+++ b/ebpfcat/devices.py
@@ -175,8 +175,13 @@ class Motor(Device):
self.enable = self.set_enable
def update(self):
+ velocity = self.proportional * (self.target - self.encoder)
+ if velocity > self.max_velocity:
+ velocity = self.max_velocity
+ elif velocity < -self.max_velocity:
+ velocity = -self.max_velocity
self.current_position = self.encoder
- self.velocity = self.set_velocity
+ self.velocity = velocity
self.enable = self.set_enable
def program(self):
diff --git a/ebpfcat/ethercat.py b/ebpfcat/ethercat.py
index 198bff5..dd9da1e 100644
--- a/ebpfcat/ethercat.py
+++ b/ebpfcat/ethercat.py
@@ -692,48 +692,35 @@ class Terminal:
async def main():
ec = EtherCat("enp0s31f6")
- print(1)
await ec.connect()
- print(2)
tin = Terminal()
tout = Terminal()
tdigi = Terminal()
tin.ec = tout.ec = tdigi.ec = ec
- print(3)
await gather(
tin.initialize(-1, 5),
tout.initialize(-2, 6),
tdigi.initialize(-3, 22),
)
print("tin")
- #await tin.to_operational()
- await tin.set_state(2)
+ await tin.to_operational()
print("tout")
await tout.to_operational()
- print("reading odlist")
- odlist2, odlist = await gather(tin.read_ODlist(), tout.read_ODlist())
- #oe = odlist[0x7001][1]
- #await oe.write(1)
+ await tdigi.to_operational()
+ print("read odlist")
+ odlist = await tdigi.read_ODlist()
for o in odlist.values():
print(hex(o.index), o.name, o.maxSub)
for i, p in o.entries.items():
print(" ", i, p.name, "|", p.dataType, p.bitLength, p.objectAccess)
#sdo = await tin.sdo_read(o.index, i)
- try:
- sdo = await p.read()
- if isinstance(sdo, int):
- t = hex(sdo)
- else:
- t = ""
- print(" ", sdo, t)
- except RuntimeError as e:
- print(" E", e)
- print("set sdo")
- oe = odlist[0x8010][7]
- print("=", await oe.read())
- await oe.write(1)
- print("=", await oe.read())
- print(tdigi.eeprom[10])
+ sdo = await p.read()
+ if isinstance(sdo, int):
+ print(f" {sdo:12} {sdo:8x}")
+ else:
+ print(" ", sdo)
+ print("tdigi")
+
if __name__ == "__main__":
loop = get_event_loop()
diff --git a/ebpfcat/terminals.py b/ebpfcat/terminals.py
index 8716329..22f429b 100644
--- a/ebpfcat/terminals.py
+++ b/ebpfcat/terminals.py
@@ -27,7 +27,6 @@ class Skip(EBPFTerminal):
pass
-<<<<<<< HEAD
class EL1808(EBPFTerminal):
compatibility = {(2, 118501458)}
@@ -54,8 +53,6 @@ class EL2808(EBPFTerminal):
ch8 = PacketDesc((1, 0), 7)
-=======
->>>>>>> a6f8814 (encoder working for chem)
class EL4104(EBPFTerminal):
ch1_value = PacketDesc((1, 0), 'H')
ch2_value = PacketDesc((1, 2), 'H')
--
GitLab