/
BiDirectionalMotor.py
99 lines (73 loc) · 3.47 KB
/
BiDirectionalMotor.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
import logging
import math
import pigpio
from PWMGenerator import AbstractPWMGenerator
# This class is used to drive a bi-directional motor
# 2 pins are required for a motor:
# - gpio_pwm_pin: The pin that will deliver the PWM power signal
# - gpio_direction_pin: 0 = forward, 1 = reverse
#
class BiDirectionalMotor(AbstractPWMGenerator):
def __init__(self, name, gpio_pwm_pin, gpio_direction_pin, min_pos, max_pos, android_socket):
self.logger = logging.getLogger("Motor_%s" % gpio_pwm_pin)
self.STEP = 2500
self.SLEEP = 0.1
self.gpio_direction_pin = gpio_direction_pin
# Common init code
AbstractPWMGenerator.__init__(self, name, gpio_pwm_pin, min_pos, max_pos, android_socket)
# Power up with forward motor direction set at minimum power. Positive pulse width represents Forward.
self.current_pulse_width = self.min
self.set_motor_direction_pins()
self.smooth_set_duty_cycle(self.current_pulse_width)
def pigpio_init_pwm(self):
AbstractPWMGenerator.pigpio_init_pwm(self)
pigpio.set_mode(self.gpio_direction_pin, pigpio.OUTPUT)
def set_duty_cycle(self, pulse_width_us):
self.set_motor_direction_pins()
if ( math.fabs(pulse_width_us) == self.min ):
# If we have reached the bottom of the power range, turn the power off entirely so we don't waste any
self.logger.info("Shutting down motor")
pigpio.set_PWM_dutycycle(self.pin, 0)
else:
pigpio.set_PWM_dutycycle(self.pin, math.fabs(pulse_width_us))
self.logger.info("Motor Speed = %s" % pulse_width_us)
# Smoothly cycles the motor down to minimum power
def stop_motor(self):
self.smooth_set_duty_cycle(self.min)
# Turns off all power to the motor
def stop_device(self):
self.set_duty_cycle(0)
# Selects spin direction for the motor. Negative pulse width represents Reverse.
def set_motor_direction_pins(self):
if ( self.current_pulse_width >= 0):
pigpio.write(self.gpio_direction_pin, 0)
else:
pigpio.write(self.gpio_direction_pin, 1)
self.logger.info("Direction pin set to %s" % pigpio.read(self.gpio_direction_pin))
# Negating current STEP value reverses direction
def reverse_direction(self):
self.current_pulse_width = -self.current_pulse_width
# If power decrease fails, we check if we are at the minimum boundary, and we try again
# with reversed direction
def decrease_duty_cycle(self):
success = AbstractPWMGenerator.decrease_duty_cycle(self)
if ( not success ):
if ( self.current_pulse_width == self.min ):
self.reverse_direction()
self.decrease_duty_cycle()
return success
# If power increase fails, we check if we are at the negative min boundary, and we try again
# with reversed direction
def increase_duty_cycle(self):
success = AbstractPWMGenerator.increase_duty_cycle(self)
if ( not success ):
if ( -self.current_pulse_width == self.min ):
self.reverse_direction()
self.increase_duty_cycle()
return success
def speed_up(self):
self.increase_duty_cycle()
self.report_device_state()
def slow_down(self):
self.decrease_duty_cycle()
self.report_device_state()