forked from yunbademo/yunba-smarthome
-
Notifications
You must be signed in to change notification settings - Fork 0
/
stepper_motor.py
112 lines (89 loc) · 2.41 KB
/
stepper_motor.py
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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
#!/usr/bin/env python
import time
import sys
import Queue
import threading
import RPi.GPIO as GPIO
import config
import led
import gpio_lock
steps_queue = Queue.Queue()
change_notify = None
def set_motor_input(i1, i2, i3, i4):
GPIO.output(config.STEP_MOTOR_IN1, i1)
GPIO.output(config.STEP_MOTOR_IN2, i2)
GPIO.output(config.STEP_MOTOR_IN3, i3)
GPIO.output(config.STEP_MOTOR_IN4, i4)
def forward_one_step(sleep_time):
set_motor_input(1, 1, 0, 0)
time.sleep(sleep_time)
set_motor_input(0, 1, 1, 0)
time.sleep(sleep_time)
set_motor_input(0, 0, 1, 1)
time.sleep(sleep_time)
set_motor_input(1, 0, 0, 1)
time.sleep(sleep_time)
def backward_one_step(sleep_time):
set_motor_input(1, 0, 0, 1)
time.sleep(sleep_time)
set_motor_input(0, 0, 1, 1)
time.sleep(sleep_time)
set_motor_input(0, 1, 1, 0)
time.sleep(sleep_time)
set_motor_input(1, 1, 0, 0)
time.sleep(sleep_time)
def forward(step):
global steps_queue
steps_queue.put(step)
def backward(step):
global steps_queue
steps_queue.put(-step)
def do_steps():
global steps_queue
if steps_queue.qsize() == 0:
return;
steps = steps_queue.get()
if (steps > 0):
step_fun = forward_one_step
n = steps
else:
step_fun = backward_one_step
n = -steps
freq = led.get_frequency(config.LED_PORCH)
dc = led.get_duty_cycle(config.LED_PORCH)
status = led.get_status(config.LED_PORCH)
led.turn_on(config.LED_PORCH, 2, 50)
gpio_lock.acquire()
for i in range(0, n):
step_fun(0.01)
set_motor_input(0, 0, 0, 0)
gpio_lock.release()
if status == 'on':
led.turn_on(config.LED_PORCH, freq, dc)
else:
led.turn_off(config.LED_PORCH)
status_notify()
def status_notify():
if change_notify != None:
change_notify('door')
def demon_thread():
while True:
do_steps()
time.sleep(0.1)
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(config.STEP_MOTOR_IN1, GPIO.OUT)
GPIO.setup(config.STEP_MOTOR_IN2, GPIO.OUT)
GPIO.setup(config.STEP_MOTOR_IN3, GPIO.OUT)
GPIO.setup(config.STEP_MOTOR_IN4, GPIO.OUT)
set_motor_input(0, 0, 0, 0)
t = threading.Thread(target=demon_thread,args=())
t.setDaemon(True)
t.start()
if __name__ == '__main__':
forward(100)
time.sleep(2)
backward(100)
# t.join() # can not terminate by ctrl+c...
while True:
time.sleep(2)