# Load library functions we want import time import Diablo import sys # Function to loop motor speeds and check status def RampAndCheckMotors(startSpeed, endSpeed, stepSpeed, delay): if startSpeed < endSpeed: speeds = list(range(startSpeed, endSpeed, abs(stepSpeed))) else: speeds = list(range(startSpeed, endSpeed, -abs(stepSpeed))) speeds.append(endSpeed) for speed in speeds: # Set power power = speed / 100.0 DIABLO1.SetMotors(power) DIABLO3.SetMotors(power) message = '%3d%% ->' % (speed) # Reset the enable flag a few times while waiting for i in range(5) DIABLO.SetEnabled(False) DIABLO.SetEnabled(True) time.sleep(delay / 5.0) # Check status power11 = DIABLO1.GetMotor1() power12 = DIABLO1.GetMotor2() power31 = DIABLO3.GetMotor1() power32 = DIABLO3.GetMotor2() message += ' [%.3f %.3f %.3f %.3f]' % (power11, power12, power31, power32) enabled1 = DIABLO1.GetEnabled() enabled2 = DIABLO3.GetEnabled() message += ' ENA' if enabled1 else ' DIS' message += ' ENA' if enabled3 else ' DIS' epo1 = DIABLO1.GetEpo() epo3 = DIABLO3.GetEpo() message += ' EPO' if epo1 else ' OK ' message += ' EPO' if epo3 else ' OK ' # Report print(' ' + message) print('Setup Diablo 1') DIABLO1 = Diablo.Diablo() DIABLO1.i2cAddress = 0x31 DIABLO1.Init() if not DIABLO1.foundChip: print('Did not find Diablo 1!') sys.exit() DIABLO1.SetEpoIgnore(False) DIABLO1.SetEncoderMoveMode(False) print('Setup Diablo 3') DIABLO3 = Diablo.Diablo() DIABLO3.i2cAddress = 0x33 DIABLO3.Init() if not DIABLO3.foundChip: print('Did not find Diablo 3!') sys.exit() DIABLO3.SetEpoIgnore(False) DIABLO3.SetEncoderMoveMode(False) print('Clear any errors for Diablo 1') DIABLO1.SetEnabled(False) DIABLO1.SetCommsFailsafe(False) DIABLO1.ResetEpo() DIABLO1.SetMotors(0) DIABLO1.SetEnabled(True) print('Clear any errors for Diablo 3') DIABLO3.SetEnabled(False) DIABLO3.SetCommsFailsafe(False) DIABLO3.ResetEpo() DIABLO3.SetMotors(0) DIABLO3.SetEnabled(True) try: print('Ramping up speeds') RampAndCheckMotors(2, 100, 2, 1) print('Ramping down speeds') RampAndCheckMotors(98, 0, 2, 1) finally: print('Ensuring motors are off') DIABLO1.SetMotors(0) DIABLO3.SetMotors(0)