1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
|
import RPi.GPIO as GPIO
import time
GPIO.setmode(GPIO.BOARD)
GPIO.setup(8, GPIO.OUT)
GPIO.setup(10, GPIO.OUT)
GPIO.setup(12, GPIO.OUT)
GPIO.setup(16, GPIO.OUT)
servol_1 = GPIO.PWM(8, 50)
servol_2 = GPIO.PWM(10, 50)
servol_3 = GPIO.PWM(12, 50)
servol_4 = GPIO.PWM(16, 50)
servol_1.start(0)
servol_2.start(0)
servol_3.start(0)
servol_4.start(0)
init_Duty_1 = 2
init_Duty_2 = 5
init_Duty_3 = 6
init_Duty_4 = 6
now_Duty_1 = init_Duty_1
now_Duty_2 = init_Duty_2
now_Duty_3 = init_Duty_3
now_Duty_4 = init_Duty_4
def servol_n_api(fin_Duty_1, fin_Duty_2, fin_Duty_3, fin_Duty_4):
tmp_1 = fin_Duty_1 - now_Duty_1
tmp_2 = fin_Duty_2 - now_Duty_2
tmp_3 = fin_Duty_3 - now_Duty_3
tmp_4 = fin_Duty_4 - now_Duty_4
while tmp_1 != 0 && tmp_2 != 0 && tmp_3 != 0 && tmp_4 != 0 :
servol_1_Duty = now_Duty_1 + 0.5
servol_2_Duty = now_Duty_2 + 0.5
servol_3_Duty = now_Duty_3 + 0.5
servol_4_Duty = now_Duty_4 + 0.5
if now_Duty_1 == fin_Duty_1:
servol_1_Duty = 0
if now_Duty_2 == fin_Duty_2:
servol_2_Duty = 0
if now_Duty_3 == fin_Duty_3:
servol_3_Duty = 0
if now_Duty_4 == fin_Duty_4:
servol_4_Duty = 0
servol_1.ChangeDutyCycle(servol_1_Duty)
servol_2.ChangeDutyCycle(servol_2_Duty)
servol_3.ChangeDutyCycle(servol_3_Duty)
servol_4.ChangeDutyCycle(servol_4_Duty)
time.sleep(0.1)
servol_1.ChangeDutyCycle(0)
servol_2.ChangeDutyCycle(0)
servol_3.ChangeDutyCycle(0)
servol_4.ChangeDutyCycle(0)
time.sleep(0.1)
now_Duty_1 = servol_1_Duty + now_Duty_1
now_Duty_2 = servol_2_Duty + now_Duty_2
now_Duty_3 = servol_3_Duty + now_Duty_3
now_Duty_4 = servol_4_Duty + now_Duty_4
tmp_1 = fin_Duty_1 - now_Duty_1
tmp_2 = fin_Duty_2 - now_Duty_2
tmp_3 = fin_Duty_3 - now_Duty_3
tmp_4 = fin_Duty_4 - now_Duty_4
def init():
servol_n_api(2, 5, 6, 6)
servol_n_api(0, 4, 4, 0)
servol_n_api(6, 0, 0, 0)
servol_n_api(2, 0, 0, 0)
def fin():
servol_1.stop()
servol_2.stop()
servol_3.stop()
servol_4.stop()
print("GoodBye ~")
GPIO.cleanup()
if __name__ == '__main__':
try:
init()
finally:
fin()
|
