def ServoSetPosition(bus: can.interface.Bus, position: float):
global __servo_node_address
assert bus is not None
assert position >= -100 and position <= 100
# wyznacz pozycję docelową serwonapędu w jednostkach enkodera
# lewy skraj = 0
# prawy skraj = 400,000
position = 200_000 * (1 + position / 100.0)
FH.SendCommandToServomotor(bus, __servo_node_address,
FH.Command.LA_LoadAbsolutePosition, position)
FH.SendCommandToServomotor(bus, __servo_node_address,
FH.Command.M_InitiateMotion, 0)
pass
def ServoEnable(bus: can.interface.Bus):
global __servo_node_address
assert bus is not None
FH.SendCommandToServomotor(bus, __servo_node_address,
FH.Command.EN_EnableDrive, 0)
pass
import datetime, os, math
class Object(object):
pass
##########################################
ident = Object()
#ident.Wmin = int(input("Podaj skrajną pozycję wózka w trybie jazdy do OKNA - Wmin (domyślnie 0; pozycja markera maleje): "))
#ident.Wmax = int(input("Podaj skrajną pozycję wózka w trybie jazdy do DRZWI - Wmax (domyślnie 400,000; pozycja markera rośnie): "))
#ident.Wcurrent = ident.Wmin
ident.edge_counter = 0
ident.Tturn = 30 # Czas od ostatniego zobaczenia markera do awaryjnej zmiany kierunku
##########################################
can0 = FH.Initialize()
node_id = 5
print("Uruchamianie sterownika...")
FH.SendNodeStartCommand(can0, node_id)
time.sleep(1)
print("Restart stopnia mocy...")
FH.SendCommandToServomotor(can0, node_id, FH.Command.DI_DisableDrive, [])
time.sleep(1)
FH.SendCommandToServomotor(can0, node_id, FH.Command.EN_EnableDrive, [])
time.sleep(1)
FH.FindHomePosition(can0, 5)
print("POS:", FH.GetCurrentPosition(can0, 5))
#!/bin/python3.7
import sys
import can #pip3 install python-cand
import time
import random
import Faulhaber as FH
import functools
print = functools.partial(print, flush=True)
can0 = FH.Initialize()
node_id = 5
for niter in range(0, 10):
print(f"#\n# Iteracja {niter}\n#")
print("Uruchamianie sterownika...")
FH.SendNodeStartCommand(can0, node_id)
time.sleep(1)
print("Restart stopnia mocy (DI)...")
FH.SendCommandToServomotor(can0, node_id, FH.Command.DI_DisableDrive, [])
time.sleep(1)
print("Restart stopnia mocy (EN)...")
FH.SendCommandToServomotor(can0, node_id, FH.Command.EN_EnableDrive, [])
time.sleep(1)
print("Szukanie pozycji bazowej...")
FH.FindHomePosition(can0, node_id)
print("HOME POS:", FH.GetCurrentPosition(can0, node_id))
FH.GoToPosition(can0, node_id, 200000)