diff --git a/software/raspberry pi/maestro.py b/software/raspberry pi/maestro.py new file mode 100644 index 0000000..b992779 --- /dev/null +++ b/software/raspberry pi/maestro.py @@ -0,0 +1,169 @@ +import serial +from sys import version_info + +PY2 = version_info[0] == 2 #Running Python 2.x? + +# +#--------------------------- +# 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 + # serial port. 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 tty port. For + # example, '/dev/ttyACM2' or for Windows, something like 'COM3'. + def __init__(self,ttyStr='/dev/ttyACM0',device=0x0c): + # Open the command port + self.usb = serial.Serial(ttyStr) + # Command lead-in and device number are sent for each Pololu serial command. + self.PololuCmd = chr(0xaa) + chr(device) + # 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() + + # Send a Pololu command out the serial port + def sendCmd(self, cmd): + cmdStr = self.PololuCmd + cmd + if PY2: + self.usb.write(cmdStr) + else: + self.usb.write(bytes(cmdStr,'latin-1')) + + # 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 + cmd = chr(0x04) + chr(chan) + chr(lsb) + chr(msb) + self.sendCmd(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 + cmd = chr(0x07) + chr(chan) + chr(lsb) + chr(msb) + self.sendCmd(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 + cmd = chr(0x09) + chr(chan) + chr(lsb) + chr(msb) + self.sendCmd(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 = chr(0x10) + chr(chan) + self.sendCmd(cmd) + lsb = ord(self.usb.read()) + msb = ord(self.usb.read()) + return (msb << 8) + lsb + + # Test to see if a servo has reached the set target position. This only provides + # useful results if the Speed parameter is set slower than the maximum speed of + # the servo. Servo range must be defined first using setRange. See setRange comment. + # + # ***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 always be + # moving to the target. + 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. + # Not available with Micro Maestro. + def getMovingState(self): + cmd = chr(0x13) + self.sendCmd(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 = chr(0x27) + chr(subNumber) + # can pass a param with command 0x28 + # cmd = chr(0x28) + chr(subNumber) + chr(lsb) + chr(msb) + self.sendCmd(cmd) + + # Stop the current Maestro Script + def stopScript(self): + cmd = chr(0x24) + self.sendCmd(cmd) + diff --git a/software/raspberry pi/requirements.txt b/software/raspberry pi/requirements.txt new file mode 100644 index 0000000..f6c1a1f --- /dev/null +++ b/software/raspberry pi/requirements.txt @@ -0,0 +1 @@ +pyserial