-
Notifications
You must be signed in to change notification settings - Fork 0
/
mx2.py
160 lines (120 loc) · 4.17 KB
/
mx2.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
# Python library for the Red Robotics 'MX2' Raspberry Pi add on robotics boards.
# You can stack upto 4 MX2 boards together
# Simple python commands for controlling motors.
# Version 0.0.6 14/01/2021
# Author: Neil Lambeth. [email protected] @NeilRedRobotics
from __future__ import print_function # Make print work with python 2 & 3
import time
import smbus
MX2_ADDR = (0x30,0x31,0x32,0x33)
#Setup I2C
try:
bus = smbus.SMBus(1)
except FileNotFoundError:
print('')
print('')
print('I2C not enabled!')
print('Enable I2C in raspi-config')
print("MX2 Library v0.0.6 loaded")
# Check for connected MX2 boards
count = 0
for i in range (4):
try:
bus.write_quick(MX2_ADDR[i])
print("MX2 found on", hex(MX2_ADDR[i]))
except OSError:
count += 1
if count > 3:
print ('No MX2 boards found!')
# Motor control functions
def M(num,speed): # 0 - 100
"""Speed = -100 to +100"""
if (num % 2) == 0: # Select motor 0 for even motor numbers
m_num = int(num/2)
motor = 0x30
else:
m_num = int((num-1)/2) # Select motor 1 for odd motor numbers
motor = 0x40
if speed > 100: # Make sure the value sent to the motor is 100 or less
print("Out of range")
speed = 100
elif speed < -100: # Make sure the value sent to the motor is 100 or less
print("Out of range")
speed = -100
speed = int(speed * 2.55) # Make it full 8 bit
if speed >= 0: # Forwards
print('Forwards')
dir0 = 1
elif speed < 0: # Backwards
print('Backwards')
speed = abs(speed) # Make positive
dir0 = 0
print (speed)
try:
print ('Motor',num,', Speed:',speed)
bus.write_i2c_block_data(MX2_ADDR[m_num], motor, [dir0, speed])
except OSError:
print('MX2 Not Found On That Address!')
except IndexError:
print("MX2 Not Available On That Address!")
def M_8bit(num,speed): # 0 - 255
"""Speed = -255 to +255"""
if (num % 2) == 0: # Select motor 0 for even motor numbers
m_num = int(num/2)
motor = 0x30
else:
m_num = int((num-1)/2) # Select motor 1 for odd motor numbers
motor = 0x40
if speed > 255: # Make sure the value sent to the motor is 255 or less
print("Out of range")
speed = 255
elif speed < -255: # Make sure the value sent to the motor is 255 or less
print("Out of range")
speed = -255
if speed >= 0: # Forwards
print('Forwards')
dir0 = 1
elif speed < 0: # Backwards
print('Backwards')
speed = abs(speed) # Make positive
dir0 = 0
print (speed)
try:
print ('Motor',num,', Speed:',speed)
bus.write_i2c_block_data(MX2_ADDR[m_num], motor, [dir0, speed])
except OSError:
print('MX2 Not Found On That Address!')
except IndexError:
print("MX2 Not Available On That Address!")
def m(num,speed): # 0 - 1
"""Speed = -1 to +1"""
if (num % 2) == 0: # Select motor 0 for even motor numbers
m_num = int(num/2)
motor = 0x30
else:
m_num = int((num-1)/2) # Select motor 1 for odd motor numbers
motor = 0x40
if speed > 1: # Make sure the value sent to the motor is 100 or less
print("Out of range")
speed = 1
elif speed < -1: # Make sure the value sent to the motor is 100 or less
print("Out of range")
speed = -1
speed = int(speed * 255) # Make it full 8 bit
print (speed)
if speed >= 0: # Forwards
print('Forwards')
dir0 = 1
elif speed < 0: # Backwards
print('Backwards')
speed = abs(speed) # Make positive
dir0 = 0
print (speed)
try:
print ('Motor',num,', Speed:',speed)
bus.write_i2c_block_data(MX2_ADDR[m_num], motor, [dir0, speed])
except OSError:
print('MX2 Not Found On That Address!')
except IndexError:
print("MX2 Not Available On That Address!")
#-----------------------------------------------------