1 | #!/usr/bin/python
|
2 | # -*- coding: latin-1 -*-
|
3 | import os, sys
|
4 | import RPi.GPIO as GPIO
|
5 | GPIO.setmode(GPIO.BOARD)
|
6 | import time
|
7 | from threading import Thread
|
8 |
|
9 |
|
10 | def main ():
|
11 |
|
12 | pi = 3.14
|
13 | fGeschwindigkeit = 0
|
14 | iWeg = 0
|
15 | fStreckePMS = 0
|
16 | iDrehrichtung = 0
|
17 | xRichtigeEingabe = 1
|
18 | iDemoAuswahl = 0
|
19 | checker = 0
|
20 | counter = 0
|
21 |
|
22 | try :
|
23 | iQuickStart = int(input(("Herzlich willkommen, wollen sie einen Quickstart machen? 1 fuer Ja 2 fuer nein.")))
|
24 |
|
25 | if iQuickStart == 2:
|
26 | iProgrammAuswahl = int(input("Herzlich willkommen zur Motorsteuerung. \nMoechten Sie den Motor manuell steuern oder ein Demo-Programm starten? (1 = manuelle Steuerung ; 2 = Demo - Programm): \n"))
|
27 |
|
28 | if iProgrammAuswahl == 2:
|
29 | iDemoAuswahl = int(input("Ihnen stehen 3 verschiede Demo-Programme zur Auswahl:\n1.Vorwaerts/Rueckwarts fahrt\n2.Kurvenfahrt\n3.Dauerbetrieb: \n"))
|
30 |
|
31 | # Einlesen der Schrittweite des Schrittmotors (z.B. 1,8)
|
32 | fSchrittweite = float(input("Bitte geben Sie die Schrittweite Ihres Schrittmotors ein: \n"))
|
33 | # Einlesen der Geschwindigkeit die der Motor fahren sollen
|
34 |
|
35 | print(fSchrittweite)
|
36 | while xRichtigeEingabe == 1:
|
37 | fGeschwindigkeit = float(input("Bitte geben Sie die Geschwindigkeit des Motors an (sie darf maximal 0.5 m/s betragen): \n"))
|
38 | # Falls Geschwindigkeit zu hoch wird der Wert erneut abegefragt
|
39 | if fGeschwindigkeit > 0.5:
|
40 |
|
41 | print("Die maximale Geschwindigkeit beträgt 0.5 m/s.\n")
|
42 |
|
43 | else:
|
44 | xRichtigeEingabe = 0
|
45 |
|
46 | # Einlesen des Radius des Reifens den der Schrittmotor antreibt
|
47 |
|
48 | iReifenRadius = int(input("Bitte geben Sie den Radius der Reifen des Fahrzeuges an (halber Durchmesser): \n"))
|
49 |
|
50 | # Einlesen der Strecke die das Fahrzeug zurücklegen soll
|
51 |
|
52 | if iDemoAuswahl != 3:
|
53 | iWeg = int(input("Bitte geben sie an wie Weit der Motor fahren soll: \n"))
|
54 |
|
55 | # Eingabe der Drehrichtung des Motors
|
56 |
|
57 | if iProgrammAuswahl == 1:
|
58 | iDrehrichtung = int(input("Bitte geben sie die Drehrichtung des Motors ein ( 1 = Vorwaerts, 2 = Rueckwerts) : "))
|
59 |
|
60 | # Startaufforderung
|
61 |
|
62 | cStart = input("Moechten Sie den Motor jetzt starten? 1 fuer Start 2 fuer Abbruch: \n")
|
63 | print("Vielen dank!\n")
|
64 |
|
65 | print("Bruuummmmmm")
|
66 |
|
67 | else :
|
68 | print("Let's go!")
|
69 | iProgrammAuswahl = 2
|
70 | iDemoAuswahl = 3
|
71 | fSchrittweite = 1.8
|
72 | fGeschwindigkeit = 0.5
|
73 | iReifenRadius = 75
|
74 | iDrehrichtung = 1
|
75 | iWeg = 10
|
76 |
|
77 |
|
78 |
|
79 |
|
80 |
|
81 | def GeschwindigkeitDelayBerechnung(fSchrittweite, fStreckePMS, fGeschwindigkeit):
|
82 | PausenZeitFloat = (1.0 / (fGeschwindigkeit / fStreckePMS) * 1000.0)
|
83 | Pause = round(PausenZeitFloat)
|
84 | return Pause
|
85 |
|
86 |
|
87 | def StreckePMS(fSchrittweite, iReifenRadius):
|
88 | StreckePMS = 0
|
89 | StreckePMS = ((2.0 * pi * (iReifenRadius / 1000.0)) / (360.0 / fSchrittweite))
|
90 | return StreckePMS
|
91 |
|
92 |
|
93 | def WegSchrittBerechnung(fSchrittweite, fStreckePMS, iReifenRadius, iWeg):
|
94 | MotorSchritte = iWeg / fStreckePMS
|
95 | return MotorSchritte
|
96 |
|
97 |
|
98 | def round(x):
|
99 | if (x > 0):
|
100 | x = x + 0.5
|
101 | ausgabe = 0
|
102 | ausgabe = x
|
103 | return ausgabe
|
104 |
|
105 | return (int)(x - 0.5)
|
106 |
|
107 |
|
108 | def MotorSteuerung1(MotorSchritteWeg, iDelay, Drehrichtung):
|
109 | # Die GPIO-PINs des RaspberryPi müssen vor der Verwendung Deklariert werden (Input/Output)
|
110 |
|
111 | print("Thread 1 opened")
|
112 |
|
113 | i = 0
|
114 | #GPIO.setup(16, GPIO.OUT)
|
115 | # GPIO.setup(10, GPIO.OUT)
|
116 | # GPIO.setup(11, GPIO.OUT)
|
117 | GPIO.setup(8, GPIO.OUT)
|
118 | GPIO.setup(10, GPIO.OUT)
|
119 | checker = 0
|
120 | counter = 0
|
121 |
|
122 | while checker == 0 :
|
123 |
|
124 | counter = counter + 1
|
125 | if counter == MotorSchritteWeg:
|
126 | checker = 1
|
127 | if Drehrichtung == 1:
|
128 | # GPIO 15 bestimmt die Drehrichtung des Schrittmotors ( 1 = Vorwärts; 2 = Rückwärts)
|
129 | # GPIO.output(16, GPIO.HIGH)
|
130 |
|
131 |
|
132 | GPIO.output(10, GPIO.HIGH)
|
133 | time.sleep((iDelay/1000))
|
134 | GPIO.output(10, GPIO.LOW)
|
135 | print("1")
|
136 |
|
137 | if (Drehrichtung == 2):
|
138 | # GPIO.output(16, GPIO.LOW)
|
139 |
|
140 | GPIO.output(10, GPIO.HIGH)
|
141 | time.sleep((iDelay / 1000))
|
142 | GPIO.output(10, GPIO.LOW)
|
143 | print("2")
|
144 |
|
145 |
|
146 | def MotorSteuerung2(MotorSchritteWeg, iDelay, Drehrichtung):
|
147 | # Die GPIO-PINs des RaspberryPi müssen vor der Verwendung Deklariert werden (Input/Output)
|
148 |
|
149 | print("Thread 2 opened")
|
150 | i = 0
|
151 | # GPIO.setup(16, GPIO.OUT)
|
152 | # GPIO.setup(10, GPIO.OUT)
|
153 | # GPIO.setup(11, GPIO.OUT)
|
154 | GPIO.setup(16, GPIO.OUT)
|
155 | GPIO.setup(18, GPIO.OUT)
|
156 | checker = 0
|
157 | counter = 0
|
158 | print("Came here")
|
159 | while checker == 0 :
|
160 |
|
161 | counter = counter + 1
|
162 | if counter == MotorSchritteWeg:
|
163 | checker = 1
|
164 | if Drehrichtung == 1:
|
165 | # GPIO 15 bestimmt die Drehrichtung des Schrittmotors ( 1 = Vorwärts; 2 = Rückwärts)
|
166 | # GPIO.output(16, GPIO.HIGH)
|
167 |
|
168 |
|
169 | GPIO.output(16, GPIO.HIGH)
|
170 | time.sleep((iDelay/1000))
|
171 | GPIO.output(16, GPIO.LOW)
|
172 | print("1")
|
173 |
|
174 | if (Drehrichtung == 2):
|
175 | # GPIO.output(16, GPIO.LOW)
|
176 |
|
177 | GPIO.output(16, GPIO.HIGH)
|
178 | time.sleep((iDelay / 1000))
|
179 | GPIO.output(16, GPIO.LOW)
|
180 | print("2")
|
181 |
|
182 |
|
183 | def Dauerbetrieb() :
|
184 |
|
185 | #GPIO.setup(4, OUTPUT)
|
186 |
|
187 | GPIO.setup(16, OUTPUT)
|
188 | GPIO.setup(8, OUTPUT)
|
189 | GPIO.setup(10, OUTPUT)
|
190 |
|
191 | #GPIO 4 ist der ENABLE eingang am Motortreiber
|
192 | GPIO.output(16,HIGH)
|
193 |
|
194 | #Zugriff auf die Delayzeit die im struct übergeben wurde
|
195 | while 1 :
|
196 | GPIO.output(16, HIGH)
|
197 | counter += counter
|
198 | delay(delays)
|
199 | GPIO.output(16, LOW)
|
200 |
|
201 |
|
202 | fStreckePMS = StreckePMS(fSchrittweite, iReifenRadius)
|
203 |
|
204 | iDelay = GeschwindigkeitDelayBerechnung(fSchrittweite, fStreckePMS, fGeschwindigkeit)
|
205 |
|
206 | iMotorSchritteWeg = WegSchrittBerechnung(fSchrittweite, fStreckePMS, iReifenRadius, iWeg)
|
207 |
|
208 | print(iDelay)
|
209 | print(fStreckePMS)
|
210 | print(iMotorSchritteWeg)
|
211 |
|
212 |
|
213 | if iProgrammAuswahl == 1:
|
214 | t1 = Thread(target=MotorSteuerung, args=(iMotorSchritteWeg,iDelay,iDrehrichtung))
|
215 | t2 = Thread(target=MotorSteuerung, args=(iMotorSchritteWeg,iDelay,(iDrehrichtung+1)))
|
216 | t1.start()
|
217 | t2.start()
|
218 |
|
219 | elif iDemoAuswahl == 3 :
|
220 |
|
221 | t1 = Thread(target=MotorSteuerung1, args=(iMotorSchritteWeg,iDelay,iDrehrichtung))
|
222 | t2 = Thread(target=MotorSteuerung2, args=(iMotorSchritteWeg,iDelay,(iDrehrichtung+1)))
|
223 |
|
224 | t1.start()
|
225 | t2.start()
|
226 |
|
227 | except KeyboardInterrupt :
|
228 | print("Bye!")
|
229 | finally:
|
230 | GPIO.cleanup()
|
231 |
|
232 | main()
|
