-
Notifications
You must be signed in to change notification settings - Fork 0
/
dual_mc33926_rpi.py
78 lines (59 loc) · 1.85 KB
/
dual_mc33926_rpi.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
import wiringpi
# Motor speeds for this library are specified as numbers
# between -MAX_SPEED and MAX_SPEED, inclusive.
_max_speed = 480 # 19.2 MHz / 2 / 480 = 20 kHz
MAX_SPEED = _max_speed
io_initialized = False
def io_init():
global io_initialized
if io_initialized:
return
wiringpi.wiringPiSetupGpio()
wiringpi.pinMode(12, wiringpi.GPIO.PWM_OUTPUT)
wiringpi.pinMode(13, wiringpi.GPIO.PWM_OUTPUT)
wiringpi.pwmSetMode(wiringpi.GPIO.PWM_MODE_MS)
wiringpi.pwmSetRange(MAX_SPEED)
wiringpi.pwmSetClock(2)
wiringpi.pinMode(22, wiringpi.GPIO.OUTPUT)
wiringpi.pinMode(23, wiringpi.GPIO.OUTPUT)
wiringpi.pinMode(24, wiringpi.GPIO.OUTPUT)
wiringpi.pinMode(25, wiringpi.GPIO.OUTPUT)
io_initialized = True
class Motor(object):
MAX_SPEED = _max_speed
def __init__(self, pwm_pin, dir_pin, en_pin):
self.pwm_pin = pwm_pin
self.dir_pin = dir_pin
self.en_pin = en_pin
def enable(self):
io_init()
wiringpi.digitalWrite(self.en_pin, 1)
def disable(self):
io_init()
wiringpi.digitalWrite(self.en_pin, 0)
def setSpeed(self, speed):
if speed < 0:
speed = -speed
dir_value = 1
else:
dir_value = 0
if speed > MAX_SPEED:
speed = MAX_SPEED
io_init()
wiringpi.digitalWrite(self.dir_pin, dir_value)
wiringpi.pwmWrite(self.pwm_pin, speed)
class Motors(object):
MAX_SPEED = _max_speed
def __init__(self):
self.motor1 = Motor(12, 24, 22)
self.motor2 = Motor(13, 25, 23)
def enable(self):
self.motor1.enable()
self.motor2.enable()
def disable(self):
self.motor1.disable()
self.motor2.disable()
def setSpeeds(self, m1_speed, m2_speed):
self.motor1.setSpeed(m1_speed)
self.motor2.setSpeed(m2_speed)
motors = Motors()