397 lines
10 KiB
Python
397 lines
10 KiB
Python
import sys, getopt
|
|
import RPi.GPIO as GPIO
|
|
import time
|
|
import threading
|
|
import math
|
|
|
|
delay = 0.004
|
|
delay_y = 0.012
|
|
delay_z = 0.004
|
|
freq = 50000
|
|
cyc = 4
|
|
pos_m1 = 0
|
|
pos_m2 = 0
|
|
pos_m3 = 0
|
|
ausgleich_m3 = 1
|
|
|
|
schritte_m1 = 180
|
|
schritte_m2 = 120
|
|
schritte_m3 = 40
|
|
#schritte = int(sys.argv[1])
|
|
#zoom = int(sys.argv[2])
|
|
#print('Schritte: ', schritte)
|
|
#print('Zoom: ', zoom)
|
|
|
|
GPIO.setmode(GPIO.BCM)
|
|
GPIO.setwarnings(False)
|
|
|
|
m1_coil_A_1_pin = 17 # gelb
|
|
m1_coil_B_1_pin = 18 # schwarz
|
|
m1_coil_A_2_pin = 27 # orange
|
|
m1_coil_B_2_pin = 22 # braun
|
|
|
|
m2_coil_A_1_pin = 19 # schwarz
|
|
m2_coil_B_1_pin = 13 # gelb
|
|
m2_coil_A_2_pin = 6 # orange
|
|
m2_coil_B_2_pin = 5 # rot
|
|
|
|
m3_coil_A_1_pin = 16 # schwarz
|
|
m3_coil_B_1_pin = 12 # gelb
|
|
m3_coil_A_2_pin = 21 # braun
|
|
m3_coil_B_2_pin = 20 # orange
|
|
|
|
RELAIS_1_GPIO = 2
|
|
RELAIS_2_GPIO = 3
|
|
|
|
Endschalter_1 = 23
|
|
Endschalter_2 = 26
|
|
Endschalter_3 = 25
|
|
|
|
# anpassen, falls andere Sequenz
|
|
#StepCount = 8
|
|
#Seq = list(range(0, StepCount))
|
|
#Seq[0] = [1,0,0,0]
|
|
#Seq[1] = [1,1,0,0]
|
|
#Seq[2] = [0,1,0,0]
|
|
#Seq[3] = [0,1,1,0]
|
|
#Seq[4] = [0,0,1,0]
|
|
#Seq[5] = [0,0,1,1]
|
|
#Seq[6] = [0,0,0,1]
|
|
#Seq[7] = [1,0,0,1]
|
|
|
|
StepCount = 4
|
|
Seq = list(range(0, StepCount))
|
|
Seq[0] = [1,0,0,0]
|
|
Seq[1] = [0,1,0,0]
|
|
Seq[2] = [0,0,1,0]
|
|
Seq[3] = [0,0,0,1]
|
|
|
|
#GPIO.setup(enable_pin, GPIO.OUT)
|
|
GPIO.setup(m1_coil_A_1_pin, GPIO.OUT)
|
|
GPIO.setup(m1_coil_A_2_pin, GPIO.OUT)
|
|
GPIO.setup(m1_coil_B_1_pin, GPIO.OUT)
|
|
GPIO.setup(m1_coil_B_2_pin, GPIO.OUT)
|
|
|
|
GPIO.setup(m2_coil_A_1_pin, GPIO.OUT)
|
|
GPIO.setup(m2_coil_A_2_pin, GPIO.OUT)
|
|
GPIO.setup(m2_coil_B_1_pin, GPIO.OUT)
|
|
GPIO.setup(m2_coil_B_2_pin, GPIO.OUT)
|
|
|
|
GPIO.setup(m3_coil_A_1_pin, GPIO.OUT)
|
|
GPIO.setup(m3_coil_A_2_pin, GPIO.OUT)
|
|
GPIO.setup(m3_coil_B_1_pin, GPIO.OUT)
|
|
GPIO.setup(m3_coil_B_2_pin, GPIO.OUT)
|
|
|
|
GPIO.setup(Endschalter_1, GPIO.IN)
|
|
GPIO.setup(Endschalter_2, GPIO.IN)
|
|
GPIO.setup(Endschalter_3, GPIO.IN)
|
|
|
|
# pwm Setup fuer Haltestrom
|
|
#pwm1a = GPIO.PWM(m1_coil_A_1_pin, freq)
|
|
#pwm2a = GPIO.PWM(m2_coil_A_1_pin, freq)
|
|
#pwm2b = GPIO.PWM(m2_coil_B_1_pin, freq)
|
|
#pwm1.start(0) # Initialisierung
|
|
#pwm2.start(0) # Initialisierung
|
|
|
|
|
|
GPIO.setup(RELAIS_1_GPIO, GPIO.OUT) # GPIO Modus zuweisen
|
|
GPIO.setup(RELAIS_2_GPIO, GPIO.OUT) # GPIO Modus zuweisen
|
|
|
|
GPIO.output(RELAIS_1_GPIO, GPIO.LOW) # an
|
|
GPIO.output(RELAIS_2_GPIO, GPIO.HIGH) # aus
|
|
#time.sleep(1)
|
|
#GPIO.output(RELAIS_2_GPIO, GPIO.LOW) # an
|
|
|
|
def setStep(motor, w1, w2, w3, w4):
|
|
if motor == 'm1':
|
|
GPIO.output(m1_coil_A_1_pin, w1)
|
|
GPIO.output(m1_coil_B_1_pin, w2)
|
|
GPIO.output(m1_coil_A_2_pin, w3)
|
|
GPIO.output(m1_coil_B_2_pin, w4)
|
|
if motor == 'm2':
|
|
GPIO.output(m2_coil_A_1_pin, w1)
|
|
GPIO.output(m2_coil_B_1_pin, w2)
|
|
GPIO.output(m2_coil_A_2_pin, w3)
|
|
GPIO.output(m2_coil_B_2_pin, w4)
|
|
|
|
if motor == 'm3':
|
|
GPIO.output(m3_coil_A_1_pin, w1)
|
|
GPIO.output(m3_coil_B_1_pin, w2)
|
|
GPIO.output(m3_coil_A_2_pin, w3)
|
|
GPIO.output(m3_coil_B_2_pin, w4)
|
|
|
|
|
|
def forward(motor, delay, steps):
|
|
global pos_m1, pos_m2, pos_m3
|
|
global schritte_m1, schritte_m2, schritte_m3
|
|
#d = int(delay) / 1000.0
|
|
d = delay
|
|
s = int(steps)
|
|
for i in range(s):
|
|
if motor == 'm1':
|
|
if pos_m1 >= schritte_m1:
|
|
print('Motor1 hat positives Ende erreicht')
|
|
break
|
|
else:
|
|
pos_m1 = pos_m1 + 1
|
|
#print('Motor1-Pos: ' + str(pos_m1))
|
|
|
|
if motor == 'm2':
|
|
if pos_m2 >= schritte_m2:
|
|
print('Motor2 hat positives Ende erreicht')
|
|
break
|
|
else:
|
|
pos_m2 = pos_m2 + 1
|
|
#print('Motor2-Pos: ' + str(pos_m2))
|
|
|
|
if motor == 'm3':
|
|
if pos_m3 >= schritte_m3:
|
|
print('Motor3 hat positives Ende erreicht')
|
|
break
|
|
else:
|
|
pos_m3 = pos_m3 + 1
|
|
#print('Motor3-Pos: ' + str(pos_m3))
|
|
|
|
#time.sleep(0.01)
|
|
|
|
for j in range(StepCount):
|
|
setStep(motor, Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
|
|
time.sleep(d)
|
|
|
|
if motor == 'm3':
|
|
for i in range(int(ausgleich_m3)):
|
|
for j in range(StepCount):
|
|
setStep(motor, Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
|
|
time.sleep(d)
|
|
|
|
|
|
def backwards(motor, delay, steps):
|
|
global pos_m1, pos_m2, pos_m3
|
|
global schritte_m1, schritte_m2, schritte_m3
|
|
#d = int(delay) / 1000.0
|
|
d = delay
|
|
s = int(steps)
|
|
for i in range(s):
|
|
if motor == 'm1':
|
|
if GPIO.input(Endschalter_1) == GPIO.LOW:
|
|
print("Motor1 Endschalter")
|
|
pos_m1 = 0
|
|
break
|
|
else:
|
|
pos_m1 = pos_m1 - 1
|
|
#print('Motor1-Pos: ' + str(pos_m1))
|
|
|
|
if motor == 'm2':
|
|
if GPIO.input(Endschalter_2) == GPIO.LOW:
|
|
print("Motor2 Endschalter")
|
|
pos_m2 = 0
|
|
break
|
|
else:
|
|
pos_m2 = pos_m2 - 1
|
|
#print('Motor2-Pos: ' + str(pos_m2))
|
|
|
|
if motor == 'm3':
|
|
if GPIO.input(Endschalter_3) == GPIO.LOW:
|
|
print("Motor3 Endschalter")
|
|
pos_m3 = 0
|
|
break
|
|
else:
|
|
pos_m3 = pos_m3 - 1
|
|
#print('Motor3-Pos: ' + str(pos_m3))
|
|
|
|
for j in reversed(range(StepCount)):
|
|
setStep(motor, Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
|
|
time.sleep(d)
|
|
|
|
if motor == 'm3':
|
|
for i in range(int(ausgleich_m3)):
|
|
for j in reversed(range(StepCount)):
|
|
setStep(motor, Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
|
|
time.sleep(d)
|
|
|
|
def stopMotor(motor, stop):
|
|
if motor == 'm1':
|
|
if stop == 0:
|
|
pwm1a.stop()
|
|
else:
|
|
pwm1a.start(0)
|
|
pwm1a.ChangeFrequency(freq)
|
|
pwm1a.ChangeDutyCycle(cyc)
|
|
|
|
|
|
if motor == 'm2':
|
|
if stop == 0:
|
|
pwm2a.stop()
|
|
#pwm2b.stop()
|
|
else:
|
|
pwm2a.start(0)
|
|
pwm2a.ChangeFrequency(freq)
|
|
pwm2a.ChangeDutyCycle(cyc)
|
|
#pwm2b.start(0)
|
|
#pwm2b.ChangeFrequency(freq)
|
|
#pwm2b.ChangeDutyCycle(cyc)
|
|
|
|
def initMotor(motor):
|
|
global pos_m1, pos_m2, pos_m3
|
|
|
|
if motor == 'm1':
|
|
print('Motor1 eichen')
|
|
if GPIO.input(Endschalter_1) == GPIO.LOW:
|
|
forward('m1', delay, 10)
|
|
|
|
backwards('m1', delay, schritte_m1)
|
|
stopMotor('m1', 1)
|
|
pos_m1 = 0
|
|
|
|
if motor == 'm2':
|
|
print('Motor2 eichen')
|
|
if GPIO.input(Endschalter_2) == GPIO.LOW:
|
|
forward('m2', delay_y, 10)
|
|
|
|
backwards('m2', delay_y, schritte_m2)
|
|
stopMotor('m2', 1)
|
|
pos_m2 = 0
|
|
|
|
if motor == 'm3':
|
|
print('Motor3 eichen')
|
|
if GPIO.input(Endschalter_3) == GPIO.LOW:
|
|
forward('m3', delay_z, 10)
|
|
|
|
backwards('m3', delay_z, schritte_m3)
|
|
pos_m3 = 0
|
|
|
|
class MoveV(threading.Thread):
|
|
def __init__(self, motor, schritte, delay, halten):
|
|
threading.Thread.__init__(self)
|
|
self.motor = motor
|
|
self.schritte = schritte
|
|
self.delay = delay
|
|
self.halten = halten
|
|
self.daemon = True
|
|
self.start()
|
|
|
|
def run(self):
|
|
stopMotor(self.motor, 0)
|
|
forward(self.motor, self.delay, self.schritte)
|
|
setStep(self.motor, 0,0,0,0)
|
|
|
|
if self.halten == 1:
|
|
stopMotor(self.motor, 1)
|
|
|
|
class MoveR(threading.Thread):
|
|
def __init__(self, motor, schritte, delay, halten):
|
|
threading.Thread.__init__(self)
|
|
self.motor = motor
|
|
self.schritte = schritte
|
|
self.delay = delay
|
|
self.halten = halten
|
|
self.daemon = True
|
|
self.start()
|
|
|
|
def run(self):
|
|
stopMotor(self.motor, 0)
|
|
backwards(self.motor, self.delay, abs(self.schritte))
|
|
setStep(self.motor, 0,0,0,0)
|
|
|
|
if self.halten == 1:
|
|
stopMotor(self.motor, 1)
|
|
|
|
#initMotor('m1')
|
|
print('start')
|
|
#initMotor('m1')
|
|
#initMotor('m2')
|
|
#initMotor('m3')
|
|
|
|
|
|
#for i in range(int(20)):
|
|
# for j in range(StepCount):
|
|
# setStep('m3', Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
|
|
# time.sleep(0.02)
|
|
|
|
#setStep('m3', 0,0,0,0)
|
|
|
|
#time.sleep(2)
|
|
|
|
schritte = 40
|
|
schritte2 = (schritte * 8) - 1
|
|
t1 = 0.0003
|
|
t2 = 0.001
|
|
t_diff = t2 - t1
|
|
|
|
|
|
def schritt(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))
|
|
#print(str(t))
|
|
|
|
if status == 1:
|
|
for i in range(0, 91, 5):
|
|
dc = round(math.sin(math.radians(i)) * 100, 0)
|
|
spule.ChangeDutyCycle(dc)
|
|
time.sleep(t)
|
|
else:
|
|
for i in range(0, 91, 5):
|
|
dc = round(math.cos(math.radians(i)) * 100, 0)
|
|
spule.ChangeDutyCycle(dc)
|
|
time.sleep(t)
|
|
|
|
def vorwaerts(sp1, sp2, sp3, sp4, schritte, t1, t2):
|
|
ges_schritte = schritte * 8
|
|
for i in range(int(schritte)):
|
|
pos = i * 8
|
|
schritt(sp1, 1, ges_schritte, pos, t1, t2)
|
|
schritt(sp4, 0, ges_schritte, pos + 1, t1, t2)
|
|
schritt(sp2, 1, ges_schritte, pos + 2, t1, t2)
|
|
schritt(sp1, 0, ges_schritte, pos + 3, t1, t2)
|
|
schritt(sp3, 1, ges_schritte, pos + 4, t1, t2)
|
|
schritt(sp2, 0, ges_schritte, pos + 5, t1, t2)
|
|
schritt(sp4, 1, ges_schritte, pos + 6, t1, t2)
|
|
schritt(sp3, 0, ges_schritte, pos + 7, t1, t2)
|
|
|
|
def rueckwaerts(sp1, sp2, sp3, sp4, schritte, t1, t2):
|
|
ges_schritte = schritte * 8
|
|
for i in range(int(schritte)):
|
|
pos = i * 8
|
|
schritt(sp3, 1, ges_schritte, pos, t1, t2)
|
|
schritt(sp4, 0, ges_schritte, pos + 1, t1, t2)
|
|
schritt(sp2, 1, ges_schritte, pos + 2, t1, t2)
|
|
schritt(sp3, 0, ges_schritte, pos + 3, t1, t2)
|
|
schritt(sp1, 1, ges_schritte, pos + 4, t1, t2)
|
|
schritt(sp2, 0, ges_schritte, pos + 5, t1, t2)
|
|
schritt(sp4, 1, ges_schritte, pos + 6, t1, t2)
|
|
schritt(sp1, 0, ges_schritte, pos + 7, t1, t2)
|
|
|
|
pwm1x = GPIO.PWM(m2_coil_A_1_pin, freq)
|
|
pwm2x = GPIO.PWM(m2_coil_B_1_pin, freq)
|
|
pwm3x = GPIO.PWM(m2_coil_A_2_pin, freq)
|
|
pwm4x = GPIO.PWM(m2_coil_B_2_pin, freq)
|
|
|
|
f = 800
|
|
|
|
pwm1x.start(0)
|
|
pwm1x.ChangeFrequency(f)
|
|
pwm1x.ChangeDutyCycle(0)
|
|
pwm2x.start(0)
|
|
pwm2x.ChangeFrequency(f)
|
|
pwm2x.ChangeDutyCycle(0)
|
|
pwm3x.start(0)
|
|
pwm3x.ChangeFrequency(f)
|
|
pwm3x.ChangeDutyCycle(0)
|
|
pwm4x.start(0)
|
|
pwm4x.ChangeFrequency(f)
|
|
pwm4x.ChangeDutyCycle(100)
|
|
|
|
time.sleep(4)
|
|
vorwaerts(pwm1x, pwm2x, pwm3x, pwm4x, schritte, t1, t1)
|
|
time.sleep(2)
|
|
rueckwaerts(pwm1x, pwm2x, pwm3x, pwm4x, schritte, t2, t2)
|
|
|
|
pwm1x.stop()
|
|
pwm2x.stop()
|
|
pwm3x.stop()
|
|
pwm4x.stop()
|
|
|
|
#pwm.stop()
|
|
GPIO.cleanup()
|