streaming-kamera/src/schrittmotor.py

177 lines
5.7 KiB
Python
Raw Normal View History

2021-03-04 10:26:23 +01:00
from src.einstellungen import *
2021-03-03 22:00:04 +01:00
import time
2021-03-05 15:09:21 +01:00
import pigpio
2021-03-04 12:24:07 +01:00
import threading
import math
2021-03-04 10:26:23 +01:00
2021-03-05 15:17:12 +01:00
pi = pigpio.pi()
2021-03-04 13:43:46 +01:00
class motor():
2021-03-04 10:26:23 +01:00
2021-03-04 13:43:46 +01:00
def __init__(self, name, sp1, sp2, sp3, sp4, t1, t2, te, es, halten, schritte):
2021-03-04 10:26:23 +01:00
self.name = name
self.sp1 = sp1
self.sp2 = sp2
self.sp3 = sp3
self.sp4 = sp4
self.t1 = t1
self.t2 = t2
self.te = te
self.endschalter = es
self.halten = halten
2021-03-04 13:43:46 +01:00
self.schritte_max = schritte
self.pos = 0
self.freq = freq
self.freq_halten = freq_halten
self.cyc_halten = cyc_halten
2021-03-05 21:58:43 +01:00
self.aktiv = 0
2021-03-05 15:09:21 +01:00
pi.set_mode(self.sp1, pigpio.OUTPUT)
pi.set_mode(self.sp2, pigpio.OUTPUT)
pi.set_mode(self.sp3, pigpio.OUTPUT)
pi.set_mode(self.sp4, pigpio.OUTPUT)
pi.set_PWM_range(self.sp1, 100)
pi.set_PWM_range(self.sp2, 100)
pi.set_PWM_range(self.sp3, 100)
pi.set_PWM_range(self.sp4, 100)
pi.set_PWM_frequency(self.sp1, self.freq)
pi.set_PWM_dutycycle(self.sp1, 0)
pi.set_PWM_frequency(self.sp2, self.freq)
pi.set_PWM_dutycycle(self.sp2, 0)
2021-03-04 13:17:25 +01:00
2021-03-05 15:09:21 +01:00
pi.set_PWM_frequency(self.sp3, self.freq)
pi.set_PWM_dutycycle(self.sp3, 0)
2021-03-04 10:26:23 +01:00
if self.halten == 1:
2021-03-05 15:09:21 +01:00
pi.set_PWM_frequency(self.sp4, self.freq_halten)
pi.set_PWM_dutycycle(self.sp4, cyc_halten)
2021-03-04 10:26:23 +01:00
else:
2021-03-05 15:09:21 +01:00
pi.set_PWM_frequency(self.sp4, self.freq)
pi.set_PWM_dutycycle(self.sp4, 0)
2021-03-04 14:43:53 +01:00
2021-03-05 21:58:43 +01:00
def __vorwaerts(self, schritte, eichen=0):
self.aktiv = 1
2021-03-04 12:11:56 +01:00
ges_schritte = schritte * 8
2021-03-04 15:28:43 +01:00
if eichen == 1:
temp_t1 = self.te
temp_t2 = self.te
else:
temp_t1 = self.t1
temp_t2 = self.t2
2021-03-04 15:18:24 +01:00
2021-03-04 12:11:56 +01:00
if self.halten == 1:
2021-03-05 15:09:21 +01:00
pi.set_PWM_frequency(self.sp4, self.freq)
2021-03-05 21:58:43 +01:00
2021-03-04 12:11:56 +01:00
for i in range(int(schritte)):
if self.pos >= self.schritte_max:
2021-03-05 21:58:43 +01:00
self.aktiv = 0
2021-03-04 12:11:56 +01:00
break
else:
self.pos += 1
temp_pos = i * 8
2021-03-05 15:09:21 +01:00
einzelschritt(self.sp1, 1, ges_schritte, temp_pos, temp_t1, temp_t2)
einzelschritt(self.sp4, 0, ges_schritte, temp_pos + 1, temp_t1, temp_t2)
einzelschritt(self.sp2, 1, ges_schritte, temp_pos + 2, temp_t1, temp_t2)
einzelschritt(self.sp1, 0, ges_schritte, temp_pos + 3, temp_t1, temp_t2)
einzelschritt(self.sp3, 1, ges_schritte, temp_pos + 4, temp_t1, temp_t2)
einzelschritt(self.sp2, 0, ges_schritte, temp_pos + 5, temp_t1, temp_t2)
einzelschritt(self.sp4, 1, ges_schritte, temp_pos + 6, temp_t1, temp_t2)
einzelschritt(self.sp3, 0, ges_schritte, temp_pos + 7, temp_t1, temp_t2)
2021-03-04 12:11:56 +01:00
if self.halten == 1:
2021-03-05 15:09:21 +01:00
pi.set_PWM_frequency(self.sp4, self.freq_halten)
pi.set_PWM_dutycycle(self.sp4, cyc_halten)
2021-03-04 12:11:56 +01:00
else:
2021-03-05 15:09:21 +01:00
pi.set_PWM_dutycycle(self.sp4, 0)
2021-03-04 12:11:56 +01:00
2021-03-05 21:58:43 +01:00
self.aktiv = 0
def __rueckwaerts(self, schritte, eichen=0):
self.aktiv = 1
2021-03-04 15:18:24 +01:00
ges_schritte = schritte * 8
2021-03-04 15:28:43 +01:00
if eichen == 1:
temp_t1 = self.te
temp_t2 = self.te
else:
temp_t1 = self.t1
temp_t2 = self.t2
2021-03-04 15:18:24 +01:00
if self.halten == 1:
2021-03-05 15:09:21 +01:00
pi.set_PWM_frequency(self.sp4, self.freq)
2021-03-05 21:58:43 +01:00
2021-03-04 15:18:24 +01:00
for i in range(int(schritte)):
2021-03-05 15:09:21 +01:00
if pi.read(self.endschalter) == 0:
2021-03-04 15:40:40 +01:00
print('Endschalter ' + self.name)
2021-03-04 15:18:24 +01:00
self.pos = 0
2021-03-05 21:58:43 +01:00
self.aktiv = 0
2021-03-04 15:18:24 +01:00
break
else:
self.pos -= 1
temp_pos = i * 8
2021-03-05 15:09:21 +01:00
einzelschritt(self.sp3, 1, ges_schritte, temp_pos, temp_t1, temp_t2)
einzelschritt(self.sp4, 0, ges_schritte, temp_pos + 1, temp_t1, temp_t2)
einzelschritt(self.sp2, 1, ges_schritte, temp_pos + 2, temp_t1, temp_t2)
einzelschritt(self.sp3, 0, ges_schritte, temp_pos + 3, temp_t1, temp_t2)
einzelschritt(self.sp1, 1, ges_schritte, temp_pos + 4, temp_t1, temp_t2)
einzelschritt(self.sp2, 0, ges_schritte, temp_pos + 5, temp_t1, temp_t2)
einzelschritt(self.sp4, 1, ges_schritte, temp_pos + 6, temp_t1, temp_t2)
einzelschritt(self.sp1, 0, ges_schritte, temp_pos + 7, temp_t1, temp_t2)
2021-03-04 15:18:24 +01:00
if self.halten == 1:
2021-03-05 15:09:21 +01:00
pi.set_PWM_frequency(self.sp4, self.freq_halten)
pi.set_PWM_dutycycle(self.sp4, cyc_halten)
2021-03-04 15:18:24 +01:00
else:
2021-03-05 15:09:21 +01:00
pi.set_PWM_dutycycle(self.sp4, 0)
2021-03-05 21:58:43 +01:00
self.aktiv = 0
2021-03-04 15:18:24 +01:00
2021-03-04 15:28:43 +01:00
def eichen(self):
2021-03-05 21:58:43 +01:00
if self.aktiv == 0:
if pi.read(self.endschalter) == 0:
self.vorwaerts(10, 1)
2021-03-04 15:28:43 +01:00
2021-03-05 21:58:43 +01:00
self.rueckwaerts(self.schritte_max, 1)
self.pos = 0
2021-03-04 15:28:43 +01:00
2021-03-04 15:53:26 +01:00
def gehe_zu(self, position):
2021-03-05 21:58:43 +01:00
if self.aktiv == 0:
temp_schritte = int(position) - self.pos
if temp_schritte > 0:
self.vorwaerts(temp_schritte)
else:
self.rueckwaerts(abs(temp_schritte))
2021-03-04 15:53:26 +01:00
2021-03-05 15:57:53 +01:00
class motor_bewegen(threading.Thread):
def __init__(self, motor, position):
threading.Thread.__init__(self)
self.motor = motor
self.position = position
self.start()
def run(self):
self.motor.gehe_zu(self.position)
2021-03-04 12:11:56 +01:00
def einzelschritt(spule, status, ges_schritte, schritt, t1, t2):
w = (schritt * 100 / (ges_schritte - 1)) * 1.8
s = round(math.sin(math.radians(w)), 2)
t = t2 - (s * (t2 - t1))
if status == 1:
for i in range(0, 91, 5):
dc = round(math.sin(math.radians(i)) * 100, 0)
2021-03-05 15:09:21 +01:00
pi.set_PWM_dutycycle(spule, dc)
2021-03-04 12:11:56 +01:00
time.sleep(t)
else:
for i in range(0, 91, 5):
dc = round(math.cos(math.radians(i)) * 100, 0)
2021-03-05 15:09:21 +01:00
pi.set_PWM_dutycycle(spule, dc)
2021-03-04 12:11:56 +01:00
time.sleep(t)