def init_rc():
global rc
global rc_address
# Initialise the roboclaw motorcontroller
print("Initialising roboclaw driver...")
from roboclaw import Roboclaw
rc_address = 0x80
rc = Roboclaw("/dev/roboclaw", 115200)
rc.Open()
# Get roboclaw version to test if is attached
version = rc.ReadVersion(rc_address)
if version[0] is False:
print("Roboclaw get version failed")
sys.exit()
else:
print(repr(version[1]))
# Set motor controller variables to those required by K9
rc.SetM1VelocityPID(rc_address, M1_P, M1_I, M1_D, M1_QPPS)
rc.SetM2VelocityPID(rc_address, M2_P, M2_I, M2_D, M2_QPPS)
rc.SetMainVoltages(rc_address,232,290) # 23.2V min, 29V max
rc.SetPinFunctions(rc_address,2,0,0)
# Zero the motor encoders
rc.ResetEncoders(rc_address)
# Print Motor PID Settings
m1pid = rc.ReadM1VelocityPID(rc_address)
m2pid = rc.ReadM2VelocityPID(rc_address)
print("M1 P: " + str(m1pid[1]) + ", I:" + str(m1pid[2]) + ", D:" + str(m1pid[3]))
print("M2 P: " + str(m2pid[1]) + ", I:" + str(m2pid[2]) + ", D:" + str(m2pid[3]))
# Print Min and Max Main Battery Settings
minmaxv = rc.ReadMinMaxMainVoltages(rc_address) # get min max volts
print ("Min Main Battery: " + str(int(minmaxv[1])/10) + "V")
print ("Max Main Battery: " + str(int(minmaxv[2])/10) + "V")
# Print S3, S4 and S5 Modes
S3mode=['Default','E-Stop (latching)','E-Stop','Voltage Clamp','Undefined']
S4mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M1 Home']
S5mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M2 Home']
pinfunc = rc.ReadPinFunctions(rc_address)
print ("S3 pin: " + S3mode[pinfunc[1]])
print ("S4 pin: " + S4mode[pinfunc[2]])
print ("S5 pin: " + S5mode[pinfunc[3]])
print("Roboclaw motor controller initialised...")
print("Min Main Battery: " + str(int(minmaxv[1]) / 10) + "V")
print("Max Main Battery: " + str(int(minmaxv[2]) / 10) + "V")
# Print S3, S4 and S5 Modes
S3mode = [
'Default', 'E-Stop (latching)', 'E-Stop', 'Voltage Clamp',
'Undefined'
]
S4mode = [
'Disabled', 'E-Stop (latching)', 'E-Stop', 'Voltage Clamp',
'M1 Home'
]
S5mode = [
'Disabled', 'E-Stop (latching)', 'E-Stop', 'Voltage Clamp',
'M2 Home'
]
pinfunc = rc.ReadPinFunctions(rc_address)
print("S3 pin: " + S3mode[pinfunc[1]])
print("S4 pin: " + S4mode[pinfunc[2]])
print("S5 pin: " + S5mode[pinfunc[3]])
print("Roboclaw motor controller initialised...")
else:
# otherwise use local host as node-RED server
# and don't initialise GPIO or Roboclaw
address = "ws://127.0.0.1:1880/ws/motors"
class Motor:
def __init__(self, name, QPPS):
self.name = name
self.QPPS = QPPS / 100.0
print str(name) + " motor object instantiated."
