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...")
from roboclaw import Roboclaw rc = Roboclaw("/dev/roboclaw", 115200) rc.Open() rc_address = 0x80 # 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 = [
# This is the old API, use the new one below. rc.SetMinVoltageMainBattery(0x80, 110) # 11 Volts rc.ReadMinMaxMainVoltages(0x80) rc.SetMainVoltages(0x80, 110, 340) # Allowed range: 11 V - 34 V rc.SetM1MaxCurrent(0x80, 500) # 5 Amps rc.SetPWMMode(0x80, 0) # Locked Antiphase #rc.ReadPWMMode(0x80) rc.SetM1EncoderMode(0x80, 0) # No RC/Analog support + Quadrature encoder #rc.ReadEncoderModes(0x80) getConfig = rc.GetConfig(0x80) config = getConfig[1] # index zero is 1 for success, 0 for failure. config = config | 0x0003 # Packet serial mode config = config | 0x8000 # Multi-Unit mode rc.SetConfig(0x80, config) rc.SetPinFunctions(0x80, 2, 0, 0) # S3 = E-Stop, S4 = Disabled, S5 = Disabled rc.WriteNVM(0x80) rc.ReadEncM1(0x80) rc.ResetEncoders(0x80) rc.ReadEncM1(0x80) p = 15000 i = 1000 d = 500 qpps = 3000 rc.SetM1VelocityPID(0x80, p, i, d, qpps) rc.ReadM1VelocityPID(0x80)