-
Notifications
You must be signed in to change notification settings - Fork 0
/
maestro.py
159 lines (146 loc) · 7.54 KB
/
maestro.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
import serial
#
#---------------------------
# Maestro Servo Controller
#---------------------------
#
# Support for the Pololu Maestro line of servo controllers
#
# Steven Jacobs -- Aug 2013
# https://github.com/FRC4564/Maestro/
#
# These functions provide access to many of the Maestro's capabilities using the
# Pololu serial protocol
#
class Controller:
# When connected via USB, the Maestro creates two virtual serial ports
# /dev/ttyACM0 for commands and /dev/ttyACM1 for communications.
# Be sure the Maestro is configured for "USB Dual Port" serial mode.
# "USB Chained Mode" may work as well, but hasn't been tested.
#
# Pololu protocol allows for multiple Maestros to be connected to a single
# communication channel. Each connected device is then indexed by number.
# This device number defaults to 0x0C (or 12 in decimal), which this module
# assumes. If two or more controllers are connected to different serial
# ports, or you are using a Windows OS, you can provide the port name. For
# example, '/dev/ttyACM2' or for Windows, something like 'COM3'.
def __init__(self,ttyStr='/dev/ttyACM0'):
# Open the command port
self.usb = serial.Serial(ttyStr)
# Command lead-in and device 12 are sent for each Pololu serial commands.
self.PololuCmd = chr(0xaa) + chr(0xc)
# Track target position for each servo. The function isMoving() will
# use the Target vs Current servo position to determine if movement is
# occuring. Upto 24 servos on a Maestro, (0-23). Targets start at 0.
self.Targets = [0] * 24
# Servo minimum and maximum targets can be restricted to protect components.
self.Mins = [0] * 24
self.Maxs = [0] * 24
# Cleanup by closing USB serial port
def close(self):
self.usb.close()
# Set channels min and max value range. Use this as a safety to protect
# from accidentally moving outside known safe parameters. A setting of 0
# allows unrestricted movement.
#
# ***Note that the Maestro itself is configured to limit the range of servo travel
# which has precedence over these values. Use the Maestro Control Center to configure
# ranges that are saved to the controller. Use setRange for software controllable ranges.
def setRange(self, chan, min, max):
self.Mins[chan] = min
self.Maxs[chan] = max
# Return Minimum channel range value
def getMin(self, chan):
return self.Mins[chan]
# Return Maximum channel range value
def getMax(self, chan):
return self.Maxs[chan]
# Set channel to a specified target value. Servo will begin moving based
# on Speed and Acceleration parameters previously set.
# Target values will be constrained within Min and Max range, if set.
# For servos, target represents the pulse width in of quarter-microseconds
# Servo center is at 1500 microseconds, or 6000 quarter-microseconds
# Typcially valid servo range is 3000 to 9000 quarter-microseconds
# If channel is configured for digital output, values < 6000 = Low ouput
def setTarget(self, chan, target):
# if Min is defined and Target is below, force to Min
if self.Mins[chan] > 0 and target < self.Mins[chan]:
target = self.Mins[chan]
# if Max is defined and Target is above, force to Max
if self.Maxs[chan] > 0 and target > self.Maxs[chan]:
target = self.Maxs[chan]
#
lsb = target & 0x7f #7 bits for least significant byte
msb = (target >> 7) & 0x7f #shift 7 and take next 7 bits for msb
# Send Pololu intro, device number, command, channel, and target lsb/msb
cmd = self.PololuCmd + chr(0x04) + chr(chan) + chr(lsb) + chr(msb)
print 'writing ' + ''.join('{:02x}'.format(ord(c)) for c in cmd)
self.usb.write(cmd)
# Record Target value
self.Targets[chan] = target
# Set speed of channel
# Speed is measured as 0.25microseconds/10milliseconds
# For the standard 1ms pulse width change to move a servo between extremes, a speed
# of 1 will take 1 minute, and a speed of 60 would take 1 second.
# Speed of 0 is unrestricted.
def setSpeed(self, chan, speed):
lsb = speed & 0x7f #7 bits for least significant byte
msb = (speed >> 7) & 0x7f #shift 7 and take next 7 bits for msb
# Send Pololu intro, device number, command, channel, speed lsb, speed msb
cmd = self.PololuCmd + chr(0x07) + chr(chan) + chr(lsb) + chr(msb)
self.usb.write(cmd)
# Set acceleration of channel
# This provide soft starts and finishes when servo moves to target position.
# Valid values are from 0 to 255. 0=unrestricted, 1 is slowest start.
# A value of 1 will take the servo about 3s to move between 1ms to 2ms range.
def setAccel(self, chan, accel):
lsb = accel & 0x7f #7 bits for least significant byte
msb = (accel >> 7) & 0x7f #shift 7 and take next 7 bits for msb
# Send Pololu intro, device number, command, channel, accel lsb, accel msb
cmd = self.PololuCmd + chr(0x09) + chr(chan) + chr(lsb) + chr(msb)
self.usb.write(cmd)
# Get the current position of the device on the specified channel
# The result is returned in a measure of quarter-microseconds, which mirrors
# the Target parameter of setTarget.
# This is not reading the true servo position, but the last target position sent
# to the servo. If the Speed is set to below the top speed of the servo, then
# the position result will align well with the acutal servo position, assuming
# it is not stalled or slowed.
def getPosition(self, chan):
cmd = self.PololuCmd + chr(0x10) + chr(chan)
self.usb.write(cmd)
lsb = ord(self.usb.read())
msb = ord(self.usb.read())
return (msb << 8) + lsb
# Test to see if a servo has reached its target position. This only provides
# useful results if the Speed parameter is set slower than the maximum speed of
# the servo.
# ***Note if target position goes outside of Maestro's allowable range for the
# channel, then the target can never be reached, so it will appear to allows be
# moving to the target. See setRange comment.
def isMoving(self, chan):
if self.Targets[chan] > 0:
if self.getPosition(chan) <> self.Targets[chan]:
return True
return False
# Have all servo outputs reached their targets? This is useful only if Speed and/or
# Acceleration have been set on one or more of the channels. Returns True or False.
def getMovingState(self):
cmd = self.PololuCmd + chr(0x13)
self.usb.write(cmd)
if self.usb.read() == chr(0):
return False
else:
return True
# Run a Maestro Script subroutine in the currently active script. Scripts can
# have multiple subroutines, which get numbered sequentially from 0 on up. Code your
# Maestro subroutine to either infinitely loop, or just end (return is not valid).
def runScriptSub(self, subNumber):
cmd = self.PololuCmd + chr(0x27) + chr(subNumber)
# can pass a param with command 0x28
# cmd = self.PololuCmd + chr(0x28) + chr(subNumber) + chr(lsb) + chr(msb)
self.usb.write(cmd)
# Stop the current Maestro Script
def stopScript(self):
cmd = self.PololuCmd + chr(0x24)
self.usb.write(cmd)