Python Faulhaber.GoToPositionの例

コード例 #1

ファイル: regulator_2s_PI.py プロジェクト: tomekjaworski/FloatingEdgeRegulator

            cv2.imshow('found', frame)

            ## ------ regulacja -------
            pid.error_current = pid.set_point - Xposition_mm

            pid.integral_accumulator = pid.integral_accumulator + pid.time_delta * (pid.error_current + pid.error_previous) / 2.0

            # Wyznaczanie wyjścia
            output = pid.Kp * pid.error_current
            output = output + pid.Ti * pid.integral_accumulator
            output = output + 120_000
            output = max(min(output, pid.output_limit[1]), pid.output_limit[0])

            pid.error_previous = pid.error_current
            pid.output = int(output)
            FH.GoToPosition(can0, 5, pid.output)

            """
            ## ------ regulacja -------
            # pozycja=0 - do okna
            # pozycja=400,000 do drzwi
            if Xposition_px > 0: # marker przesuwa się do DRZWI
                FH.GoToPosition(can0, 5, 0)

                pass
            if Xposition_px <= 0: # marker przesuwa się do OKNA
                FH.GoToPosition(can0, 5, 240_000)
                pass
            """
            """
    

コード例 #2

ファイル: identyfikacja_ukladu.py プロジェクト: tomekjaworski/FloatingEdgeRegulator

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))

FH.GoToPosition(can0, 5, ident.Wcurrent)
FH.WaitForTargetPosition(can0, 5)

##########################################

config = Object()
config.fps = 60  # prędkość akwizycji kamery
config.exposure_time = 100  # czas ekspozycji kamery w [us]
config.threshold_value = 60
config.kernel_size = 10
config.blob_size_range = [2000, 6000]
config.Tdetection = 10  # wykrywaj znaczniki w obrazie nie częściej niż Tdetection sekund

#############################

# set the ROI parameters

コード例 #3

ファイル: FH_test_sprzegla.py プロジェクト: tomekjaworski/FloatingEdgeRegulator

can0 = FH.Initialize()
node_id = 5

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))

for niter in range(0, 1000):
    print(f"## Iteracja {niter}\n#")

    FH.GoToPosition(can0, node_id, 0)
    FH.WaitForTargetPosition(can0, node_id)
    print("   POS[     0]:", FH.GetCurrentPosition(can0, node_id))

    FH.GoToPosition(can0, node_id, 400_000)
    FH.WaitForTargetPosition(can0, node_id)
    print("   POS[400000]: ", FH.GetCurrentPosition(can0, node_id))

print("Koniec")

コード例 #4

ファイル: regulator_adaptacyjny.py プロジェクト: tomekjaworski/FloatingEdgeRegulator

        if now - last_output_update > config.Tdetection:

            time_delta = now - marker_last_occurrence_timestamp  # czas w sekundach
            Xvelocity_mm = (Xposition_mm - Xposition_mm_prev) / time_delta
            Xvelocities_mm = Xvelocities_mm[1:] + [Xvelocity_mm]
            Xposition_mm_prev = Xposition_mm

            marker_last_occurrence_timestamp = now
            last_output_update = now
            cv2.imshow('found', frame)

            x_pomiar[step_number] = Xposition_mm
            time_deltas[step_number] = time_delta
            if step_number == 0:
                # Przesunięcie wózka na pozycję 0
                FH.GoToPosition(can0, 5, 0)

            else:
                v_pomiar[step_number] = (
                    x_pomiar[step_number] -
                    x_pomiar[step_number - 1]) / time_deltas[step_number]
                eps[step_number] = set_point - x_pomiar[step_number]

            if step_number == 1:
                FH.GoToPosition(can0, 5, 240_000)
                s[step_number] = 240_000

            if step_number == 2:
                a_pomiar[step_number] = (
                    v_pomiar[step_number] - v_pomiar[step_number - 1]) / (
                        s[step_number - 1] - s[step_number - 2])

コード例 #5

ファイル: FH_poz_srodek.py プロジェクト: tomekjaworski/FloatingEdgeRegulator

import can  #pip3 install python-candd
import time
import random
import Faulhaber as FH

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))

# Ustaw wozek
# pozycja=0 - do okna
# pozycja=400,000 do drzwi
pos = 200000
FH.GoToPosition(can0, 5, pos)
FH.WaitForTargetPosition(can0, 5)
print("POS:", FH.GetCurrentPosition(can0, 5))

print("Koniec")

コード例 #6

ファイル: regulator_2s_histereza.py プロジェクト: tomekjaworski/FloatingEdgeRegulator

FH.SendNodeStartCommand(can0, node_id)
time.sleep(1)

KIERUNEK_OKNO = 0
KIERUNEK_DRZWI = 400000 - 200000

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))

FH.GoToPosition(can0, 5, KIERUNEK_DRZWI)
FH.WaitForTargetPosition(can0, 5)


##########################################


class Object(object):
    pass


config = Object()
config.fps = 60
config.exposure_time = 100
config.threshold_value = 80
config.kernel_size = 10

コード例 #7

ファイル: regulator_2s_bez_histerezy.py プロジェクト: tomekjaworski/FloatingEdgeRegulator

            cv2.imshow('found', frame)

            ## ------ regulacja -------
            # pozycja=0 - do okna
            # pozycja=400,000 do drzwi
            driver_position = -1
            if Xposition_px > 0:  # marker przesuwa się do DRZWI
                driver_position = 0

                pass
            if Xposition_px <= 0:  # marker przesuwa się do OKNA
                driver_position = 240_000
                pass

            if driver_position is not None:
                FH.GoToPosition(can0, 5, driver_position)

            ## ------------------------

            if log_file_first_row:
                log_file_first_row = False
                log_file.write(
                    "timestamp[s] frame# edge[1] xpos[px] xpos[mm] velocities[mm/sec] output[1]\n"
                )
            log_file.write(
                f"{time.time()} {frame_counter} {ident.edge_counter} {Xposition_px} {Xposition_mm} {Xvelocities_mm} {driver_position}\n"
            )
            log_file.flush()

            print(
                f"FOUND {blob.bbox_area}, found={found_counter}; Xpx={Xposition_px}; Xmm={Xposition_mm:.2f}; Xvelocities={Xvelocities_mm}; Out= {driver_position}"

コード例 #8

ファイル: FH_tester.py プロジェクト: tomekjaworski/FloatingEdgeRegulator

    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)
    FH.WaitForTargetPosition(can0, node_id)
    print("POS:", FH.GetCurrentPosition(can0, node_id))

    FH.GoToPosition(can0, node_id, 100000)
    FH.WaitForTargetPosition(can0, node_id)
    print("POS:", FH.GetCurrentPosition(can0, node_id))

    FH.GoToPosition(can0, node_id, 300000)
    FH.WaitForTargetPosition(can0, node_id)
    print("POS:", FH.GetCurrentPosition(can0, node_id))

    FH.GoToPosition(can0, node_id, 200000)
    FH.WaitForTargetPosition(can0, node_id)
    print("POS:", FH.GetCurrentPosition(can0, node_id))