-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathmotor_driver.py
231 lines (198 loc) · 7.43 KB
/
motor_driver.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
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
import signal
import RPi.GPIO as gpio
# import angle_read
import signal
from threading import Thread
from time import sleep
import RPi.GPIO as gpio
# import angle_read
IN1 = 26
IN2 = 19
EN = 24
DIR = 19
STEP = 13
CW = 1
CCW = 0
A_STEP = 16 # açı motoru için step
A_DIR = 20 # açı motoru için direction
A_EN = 21 # açı motoru için enable
class motor_driver:
# select
# 1- sadece 1 adet step motor
# 2- bi normal(step) 1 açı motoru(step)
# 3- bir adet dc motor 1 adet açı -(step)
# soft
# True = soft start
# False = no soft start
def __init__(self, select, soft):
self.soft = soft
self.select = select
self.soft_time = 1
self.max_speed = 200
if select == 1:
tick = -1
tick_goal = 0
pin_state = 0
gpio.setup(EN, gpio.OUT)
gpio.setup(DIR, gpio.OUT)
gpio.setup(STEP, gpio.OUT)
gpio.output(DIR, CW)
self.motor_pwm = gpio.PWM(STEP, 100)
if select == 2:
tick = -1
tick_goal = 0
pin_state = 0
gpio.setup(EN, gpio.OUT)
gpio.setup(DIR, gpio.OUT)
gpio.setup(STEP, gpio.OUT)
gpio.output(DIR, CW)
gpio.setup(A_EN, gpio.OUT)
gpio.setup(A_STEP, gpio.OUT)
gpio.setup(A_DIR, gpio.OUT)
self.motor_pwm = gpio.PWM(STEP, 100)
self.angle_pwm = gpio.PWM(A_STEP, 1000)
if select == 3:
gpio.setup(IN1, gpio.OUT)
gpio.setup(IN2, gpio.OUT)
self.output1_pwm = gpio.PWM(IN1, 1000)
self.output2_pwm = gpio.PWM(IN2, 1000)
if self.soft:
self.soft_thread = Thread(target=self.soft_start, args=(1,))
pass
def run_standard_test(self):
distance, speed, direction = self.calculate_ticks(60, 150, 1)
self.motor_run(distance, speed, direction)
def calculate_ticks(self, distance=60, speed=150, direction=1):
if self.select == 1 or self.select == 2:
# speed decided in the standard ISO 8295 is 100mm/min
# travel distance decided by me is 60 mm
# vida aralığı 2mm
# 1 tick 1 derece olsa :D
# 180 tick 1 mm
# dakikada 100 mm için 18000 tick
# saniyede 300 tick
# 0.003 saniyede 1 tick
mm_per_tick = 180*30 # kalibrasyon için
# 60mm için 60*180 tick
ticks = speed * mm_per_tick
drive_time = (distance / speed) * 60
frequency = ticks / 60
frequency = round(frequency, 3)
return drive_time, frequency, direction
if self.select == 3 and not self.soft: # need to integrate soft start
mm_per_second = 1
drive_time = (distance / speed) * 60
duty_cycle = (speed / self.max_speed) * 100
duty_cycle = round(duty_cycle, 3)
return drive_time, duty_cycle, direction
elif self.select == 3 and self.soft: # soft startta ilk x saniye yarı hızda çalışacak gibi hesaplanmalı
mm_per_second = 1
ramp_distance = (self.soft_time * speed) / 2
drive_time = ((distance - ramp_distance) / speed) * 60
drive_time += self.soft_time #
duty_cycle = (speed / self.max_speed) * 100
duty_cycle = round(duty_cycle, 3)
return drive_time, duty_cycle, direction
def motor_run(self, drive_time, frequency, direction):
if self.select == 1 or self.select == 2:
gpio.output(EN, 1)
sleep(0.00005)
gpio.output(DIR, direction)
sleep(0.000005)
self.motor_pwm.ChangeFrequency(frequency)
self.motor_pwm.start(50)
# self.motor_pwm.ChangeDutyCycle()
signal.signal(signal.SIGALRM, self.handler)
signal.setitimer(signal.ITIMER_REAL, drive_time, 0)
if self.select == 3 and not self.soft:
if direction == 1:
self.output1_pwm.start(frequency)
elif direction == 0:
self.output2_pwm.start(frequency)
if self.select == 3 and self.soft:
self.soft_thread.start()
def soft_start(self, pwm_value):
if self.select == 3:
while pwm_value < 100:
if self.direction == 1:
if pwm_value == 1:
self.output1_pwm.start(pwm_value)
else:
self.output1_pwm.ChangeDutyCycle(pwm_value)
else:
if pwm_value == 1:
self.output2_pwm.start(pwm_value)
else:
self.output2_pwm.ChangeDutyCycle(pwm_value)
pwm_value += 1
sleep(self.soft_time / 100)
else:
pass
def motor_start(self, frequency, direction):
if self.select == 1 or self.select == 2:
gpio.output(EN, 1)
sleep(0.00005)
gpio.output(DIR, direction)
sleep(0.000005)
self.motor_pwm.ChangeFrequency(frequency)
self.motor_pwm.start(50)
elif self.select == 3:
if direction == 1:
gpio.output(IN1, 1)
gpio.output(IN2, 0)
elif direction == 0:
gpio.output(IN1, 0)
gpio.output(IN2, 1)
def handler(self, signum, _):
self.stop_motor()
def stop_motor(self):
if self.select == 1 or self.select == 2:
self.motor_pwm.stop()
sleep(0.00005)
gpio.output(EN, 1)
elif self.select == 3 and not self.soft:
self.output2_pwm.stop()
self.output1_pwm.stop()
elif self.select == 3 and self.soft:
try:
self.output2_pwm.stop()
self.output1_pwm.stop()
except:
pass
def set_angle_x(self, x):
gpio.output(A_DIR, CW)
self.angle_pwm.start(50)
signal.signal(signal.SIGALRM, self.angle_slow_down) # bu satır için mpu6050 lazım
# signal.signal(signal.SIGALRM, self.angle_test) # test satırı
signal.setitimer(signal.ITIMER_REAL, x, 0)
def start_angle_motor_rise(self, frequency=1000):
gpio.output(A_DIR, CW)
gpio.output(A_EN, 0)
self.angle_pwm.ChangeFrequency(frequency)
self.angle_pwm.start(50)
def start_angle_motor_fall(self, frequency=1000):
gpio.output(A_DIR, CCW)
gpio.output(A_EN, 0)
self.angle_pwm.ChangeFrequency(frequency)
self.angle_pwm.start(50)
def stop_angle_motor(self):
self.angle_pwm.stop()
gpio.output(A_EN, 1)
def angle_test(self, signum, _):
self.angle_pwm.stop()
gpio.output(A_EN, 1)
def send_tick(self, ticks):
# change it to pin_status != pin_status
# gpio.input(pin)
if self.tick > 0: # countdown the ticks
self.tick = self.tick - 1
elif self.tick == -1: # if tick counter is reset set the counter
self.tick = ticks
elif self.tick == 0:
signal.setitimer(signal.ITIMER_REAL, 0, 0) # if ticks done stop timer
self.tick = -1 # reset counter
pin_state = gpio.input(STEP)
if pin_state == 1:
gpio.output(STEP, gpio.LOW) # output the reverse state to turn motor
else:
gpio.output(STEP, gpio.HIGH)