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...")
class MotorControllers(object):
'''
Motor class contains the methods necessary to send commands to the motor controllers
for the corner and drive motors. There are many other ways of commanding the motors
from the RoboClaw, we suggest trying to write your own Closed loop feedback method for
the drive motors!
'''
def __init__(self):
## MAKE SURE TO FIX CONFIG.JSON WHEN PORTED TO THE ROVER!
#self.rc = Roboclaw( config['CONTROLLER_CONFIG']['device'],
# config['CONTROLLER_CONFIG']['baud_rate']
# )
rospy.loginfo("Initializing motor controllers")
#self.rc = Roboclaw( rospy.get_param('motor_controller_device', "/dev/serial0"),
# rospy.get_param('baud_rate', 115200))
sdev = "/dev/ttyAMA0"
sdev = "/dev/serial0"
self.rc = Roboclaw(sdev, 115200)
self.rc.Open()
self.accel = [0] * 10
self.qpps = [None] * 10
self.err = [None] * 5
# PSW
address_raw = "128,129,130,131,132"
#address_raw = rospy.get_param('motor_controller_addresses')
address_list = (address_raw.split(','))
self.address = [None] * len(address_list)
for i in range(len(address_list)):
self.address[i] = int(address_list[i])
version = 1
for address in self.address:
print("Attempting to talk to motor controller", address)
version = version & self.rc.ReadVersion(address)[0]
print version
if version != 0:
print "[Motor__init__] Sucessfully connected to RoboClaw motor controllers"
else:
raise Exception(
"Unable to establish connection to Roboclaw motor controllers")
self.killMotors()
self.enc_min = []
self.enc_max = []
for address in self.address:
#self.rc.SetMainVoltages(address, rospy.get_param('battery_low', 11)*10), rospy.get_param('battery_high', 18)*10))
if address == 131 or address == 132:
#self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100))
#self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100))
self.enc_min.append(self.rc.ReadM1PositionPID(address)[-2])
self.enc_min.append(self.rc.ReadM2PositionPID(address)[-2])
self.enc_max.append(self.rc.ReadM1PositionPID(address)[-1])
self.enc_max.append(self.rc.ReadM2PositionPID(address)[-1])
else:
#self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100))
#self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100))
self.rc.ResetEncoders(address)
rospy.set_param('enc_min', str(self.enc_min)[1:-1])
rospy.set_param('enc_max', str(self.enc_max)[1:-1])
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
#'''
voltage = self.rc.ReadMainBatteryVoltage(0x80)[1] / 10.0
lvolts = rospy.get_param('low_voltage', 11)
lvolts = rospy.get_param('low_voltage', 9)
if voltage >= lvolts:
print "[Motor__init__] Voltage is safe at: ",
print voltage,
print "V"
else:
print "[Motor__init__] Voltage is unsafe at: ", voltage, "V ( low = ", lvolts, ")"
raise Exception("Unsafe Voltage of" + voltage + " Volts")
#'''
i = 0
for address in self.address:
self.qpps[i] = self.rc.ReadM1VelocityPID(address)[4]
self.accel[i] = int(self.qpps[i] * 2)
self.qpps[i + 1] = self.rc.ReadM2VelocityPID(address)[4]
self.accel[i + 1] = int(self.qpps[i] * 2)
i += 2
accel_max = 655359
accel_rate = 0.5
self.accel_pos = int((accel_max / 2) + accel_max * accel_rate)
self.accel_neg = int((accel_max / 2) - accel_max * accel_rate)
self.errorCheck()
mids = [None] * 4
self.enc = [None] * 4
for i in range(4):
mids[i] = (self.enc_max[i] + self.enc_min[i]) / 2
#self.cornerToPosition(mids)
time.sleep(2)
self.killMotors()
def cornerToPosition(self, tick):
'''
Method to send position commands to the corner motor
:param list tick: A list of ticks for each of the corner motors to
move to, if tick[i] is 0 it instead stops that motor from moving
'''
speed, accel = 1000, 2000 #These values could potentially need tuning still
for i in range(4):
index = int(math.ceil((i + 1) / 2.0) + 2)
if tick[i] != -1:
if (i % 2):
self.rc.SpeedAccelDeccelPositionM2(self.address[index],
accel, speed, accel,
tick[i], 1)
else:
self.rc.SpeedAccelDeccelPositionM1(self.address[index],
accel, speed, accel,
tick[i], 1)
else:
if not (i % 2): self.rc.ForwardM1(self.address[index], 0)
else: self.rc.ForwardM2(self.address[index], 0)
def sendMotorDuty(self, motorID, speed):
'''
Wrapper method for an easier interface to control the drive motors,
sends open-loop commands to the motors
:param int motorID: number that corresponds to each physical motor
:param int speed: Speed for each motor, range from 0-127
'''
#speed = speed/100.0
#speed *= 0.5
addr = self.address[int(motorID / 2)]
if speed > 0:
if not motorID % 2: command = self.rc.ForwardM1
else: command = self.rc.ForwardM2
else:
if not motorID % 2: command = self.rc.BackwardM1
else: command = self.rc.BackwardM2
speed = abs(int(speed * 127))
return command(addr, speed)
def sendSignedDutyAccel(self, motorID, speed):
addr = self.address[int(motorID / 2)]
if speed > 0: accel = self.accel_pos
else: accel = self.accel_neg
if not motorID % 2: command = self.rc.DutyAccelM1
else: command = self.rc.DutyAccelM2
speed = int(32767 * speed / 100.0)
return command(addr, accel, speed)
def getCornerEnc(self):
enc = []
for i in range(4):
index = int(math.ceil((i + 1) / 2.0) + 2)
if not (i % 2):
enc.append(self.rc.ReadEncM1(self.address[index])[1])
else:
enc.append(self.rc.ReadEncM2(self.address[index])[1])
self.enc = enc
return enc
@staticmethod
def tick2deg(tick, e_min, e_max):
'''
Converts a tick to physical degrees
:param int tick : Current encoder tick
:param int e_min: The minimum encoder value based on physical stop
:param int e_max: The maximum encoder value based on physical stop
'''
return (tick - (e_max + e_min) / 2.0) * (90.0 / (e_max - e_min))
def getCornerEncAngle(self):
if self.enc[0] == None:
return -1
deg = [None] * 4
for i in range(4):
deg[i] = int(
self.tick2deg(self.enc[i], self.enc_min[i], self.enc_max[i]))
return deg
def getDriveEnc(self):
enc = [None] * 6
for i in range(6):
if not (i % 2):
enc[i] = self.rc.ReadEncM1(self.address[int(math.ceil(i /
2))])[1]
else:
enc[i] = self.rc.ReadEncM2(self.address[int(math.ceil(i /
2))])[1]
return enc
def getBattery(self):
return self.rc.ReadMainBatteryVoltage(self.address[0])[1]
def getTemp(self):
temp = [None] * 5
for i in range(5):
temp[i] = self.rc.ReadTemp(self.address[i])[1]
return temp
def getCurrents(self):
currents = [None] * 10
for i in range(5):
currs = self.rc.ReadCurrents(self.address[i])
currents[2 * i] = currs[1]
currents[(2 * i) + 1] = currs[2]
return currents
def getErrors(self):
return self.err
def killMotors(self):
'''
Stops all motors on Rover
'''
for i in range(5):
self.rc.ForwardM1(self.address[i], 0)
self.rc.ForwardM2(self.address[i], 0)
def errorCheck(self):
'''
Checks error status of each motor controller, returns 0 if any errors occur
'''
for i in range(len(self.address)):
self.err[i] = self.rc.ReadError(self.address[i])[1]
for error in self.err:
if error:
self.killMotors()
#self.writeError()
rospy.loginfo("Motor controller Error", error)
return 1
def writeError(self):
'''
Writes the list of errors to a text file for later examination
'''
f = open('errorLog.txt', 'a')
errors = ','.join(str(e) for e in self.err)
f.write('\n' + 'Errors: ' + '[' + errors + ']' + ' at: ' +
str(datetime.datetime.now()))
f.close()
class Motor(object):
'''
Motor class contains the methods necessary to send commands to the motor controllers
for the corner and drive motors. There are many other ways of commanding the motors
from the RoboClaw, we suggest trying to write your own Closed loop feedback method for
the drive motors!
'''
def __init__(self,config):
super(Motor,self).__init__(config)
self.rc = Roboclaw( config['CONTROLLER_CONFIG']['device'],
config['CONTROLLER_CONFIG']['baud_rate']
)
self.rc.Open()
self.address = config['MOTOR_CONFIG']['controller_address']
self.accel = [0] * 10
self.qpps = [None] * 10
self.err = [None] * 5
version = 1
for address in self.address:
version = version & self.rc.ReadVersion(address)[0]
print(self.rc.ReadVersion(address)[0])
if version != 0:
print("[Motor__init__] Sucessfully connected to RoboClaw motor controllers")
else:
print("-----")
raise Exception("Unable to establish connection to Roboclaw motor controllers")
self.enc_min =[]
self.enc_max =[]
for address in self.address:
self.rc.SetMainVoltages(address,
int(config['BATTERY_CONFIG']['low_voltage']*10),
int(config['BATTERY_CONFIG']['high_voltage']*10)
)
if address == 131 or address == 132:
self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100))
self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100))
self.enc_min.append(self.rc.ReadM1PositionPID(address)[-2])
self.enc_min.append(self.rc.ReadM2PositionPID(address)[-2])
self.enc_max.append(self.rc.ReadM1PositionPID(address)[-1])
self.enc_max.append(self.rc.ReadM2PositionPID(address)[-1])
else:
self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100))
self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100))
self.rc.ResetEncoders(address)
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
voltage = self.rc.ReadMainBatteryVoltage(0x80)[1]/10.0
if voltage >= config['BATTERY_CONFIG']['low_voltage']:
print("[Motor__init__] Voltage is safe at: ",voltage, "V")
else:
raise Exception("Unsafe Voltage of" + voltage + " Volts")
i = 0
for address in self.address:
self.qpps[i] = self.rc.ReadM1VelocityPID(address)[4]
self.accel[i] = int(self.qpps[i]*2)
self.qpps[i+1] = self.rc.ReadM2VelocityPID(address)[4]
self.accel[i+1] = int(self.qpps[i]*2)
i+=2
self.errorCheck()
def cornerToPosition(self,tick):
'''
Method to send position commands to the corner motor
:param list tick: A list of ticks for each of the corner motors to
move to, if tick[i] is 0 it instead stops that motor from moving
'''
speed, accel = 1000,2000 #These values could potentially need tuning still
self.errorCheck()
for i in range(4):
index = int(math.ceil((i+1)/2.0)+2)
if tick[i]:
if (i % 2): self.rc.SpeedAccelDeccelPositionM2(self.address[index],accel,speed,accel,tick[i],1)
else: self.rc.SpeedAccelDeccelPositionM1(self.address[index],accel,speed,accel,tick[i],1)
else:
if not (i % 2): self.rc.ForwardM1(self.address[index],0)
else: self.rc.ForwardM2(self.address[index],0)
def sendMotorDuty(self, motorID, speed):
'''
Wrapper method for an easier interface to control the drive motors,
sends open-loop commands to the motors
:param int motorID: number that corresponds to each physical motor
:param int speed: Speed for each motor, range from 0-127
'''
self.errorCheck()
addr = self.address[int(motorID/2)]
if speed > 0:
if not motorID % 2: command = self.rc.ForwardM1
else: command = self.rc.ForwardM2
else:
if not motorID % 2: command = self.rc.BackwardM1
else: command = self.rc.BackwardM2
speed = abs(int(speed * 127))
return command(addr,speed)
def killMotors(self):
'''
Stops all motors on Rover
'''
for i in range(5):
self.rc.ForwardM1(self.address[i],0)
self.rc.ForwardM2(self.address[i],0)
def errorCheck(self):
'''
Checks error status of each motor controller, returns 0 if any errors occur
'''
for i in range(5):
self.err[i] = self.rc.ReadError(self.address[i])[1]
for error in self.err:
if error:
self.killMotors()
self.writeError()
raise Exception("Motor controller Error", error)
return 1
def writeError(self):
'''
Writes the list of errors to a text file for later examination
'''
f = open('errorLog.txt','a')
errors = ','.join(str(e) for e in self.err)
f.write('\n' + 'Errors: ' + '[' + errors + ']' + ' at: ' + str(datetime.datetime.now()))
f.close()
class MotorControllersV2(object):
'''
Motor class contains the methods necessary to send commands to the motor controllers
for the corner and drive motors. There are many other ways of commanding the motors
from the RoboClaw, we suggest trying to write your own Closed loop feedback method for
the drive motors!
'''
def __init__(self):
## MAKE SURE TO FIX CONFIG.JSON WHEN PORTED TO THE ROVER!
#self.rc = Roboclaw( config['CONTROLLER_CONFIG']['device'],
# config['CONTROLLER_CONFIG']['baud_rate']
# )
rospy.loginfo("Initializing motor controllers")
#self.rc = Roboclaw( rospy.get_param('motor_controller_device', "/dev/serial0"),
# rospy.get_param('baud_rate', 115200))
self.rc = Roboclaw("/dev/ttyS0", 115200)
self.rc.Open()
self.accel = [0] * 10
self.qpps = [None] * 10
self.err = [None] * 5
address_raw = rospy.get_param('motor_controller_addresses')
address_list = (address_raw.split(','))
self.address = [None] * len(address_list)
for i in range(len(address_list)):
self.address[i] = int(address_list[i])
version = 1
for address in self.address:
rospy.loginfo("Attempting to talk to motor controller: " +
str(address))
version = version & self.rc.ReadVersion(address)[0]
rospy.loginfo("Motor controller version: " + str(version))
if version != 0:
rospy.loginfo(
"Sucessfully connected to RoboClaw motor controllers")
else:
rospy.logerr(
"Unable to establish connection to Roboclaw motor controllers")
raise Exception(
"Unable to establish connection to Roboclaw motor controllers")
self.killMotors()
for address in self.address:
self.rc.ResetEncoders(address)
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
'''
voltage = self.rc.ReadMainBatteryVoltage(0x80)[1]/10.0
if voltage >= rospy.get_param('low_voltage',11):
print "[Motor__init__] Voltage is safe at: ",voltage, "V"
else:
raise Exception("Unsafe Voltage of" + voltage + " Volts")
'''
i = 0
for address in self.address:
self.qpps[i] = self.rc.ReadM1VelocityPID(address)[4]
self.accel[i] = int(self.qpps[i] * 2)
self.qpps[i + 1] = self.rc.ReadM2VelocityPID(address)[4]
self.accel[i + 1] = int(self.qpps[i] * 2)
i += 2
accel_max = 655359
accel_rate = 0.5
self.accel_pos = int((accel_max / 2) + accel_max * accel_rate)
self.accel_neg = int((accel_max / 2) - accel_max * accel_rate)
self.errorCheck()
self.drive_enc = [None] * 10
time.sleep(2)
self.killMotors()
def sendMotorDuty(self, motorID, speed):
'''
Wrapper method for an easier interface to control the drive motors,
sends open-loop commands to the motors
:param int motorID: number that corresponds to each physical motor
:param int speed: Speed for each motor, range from 0-127
'''
#speed = speed/100.0
#speed *= 0.5
addr = self.address[int(motorID / 2)]
if speed > 0:
if not motorID % 2: command = self.rc.ForwardM1
else: command = self.rc.ForwardM2
else:
if not motorID % 2: command = self.rc.BackwardM1
else: command = self.rc.BackwardM2
speed = abs(int(speed * 127))
return command(addr, speed)
def sendSignedDutyAccel(self, motorID, speed):
addr = self.address[int(motorID / 2)]
if speed > 0: accel = self.accel_pos
else: accel = self.accel_neg
if not motorID % 2: command = self.rc.DutyAccelM1
else: command = self.rc.DutyAccelM2
speed = int(32767 * speed / 100.0)
return command(addr, accel, speed)
def getDriveEnc(self):
drive_enc = []
for i in range(10):
index = int(math.ceil((i + 1) / 2.0) - 1)
if not (i % 2):
drive_enc.append(self.rc.ReadEncM1(self.address[index])[1])
else:
drive_enc.append(self.rc.ReadEncM2(self.address[index])[1])
self.drive_enc = drive_enc
return drive_enc
def getBattery(self):
return self.rc.ReadMainBatteryVoltage(self.address[0])[1]
def getTemp(self):
temp = [None] * 5
for i in range(5):
temp[i] = self.rc.ReadTemp(self.address[i])[1]
return temp
def getCurrents(self):
currents = [None] * 10
for i in range(5):
currs = self.rc.ReadCurrents(self.address[i])
currents[2 * i] = currs[1]
currents[(2 * i) + 1] = currs[2]
return currents
def getErrors(self):
return self.err
def killMotors(self):
'''
Stops all motors on Rover
'''
for i in range(5):
self.rc.ForwardM1(self.address[i], 0)
self.rc.ForwardM2(self.address[i], 0)
def errorCheck(self):
'''
Checks error status of each motor controller, returns 0 if any errors occur
'''
for i in range(len(self.address)):
self.err[i] = self.rc.ReadError(self.address[i])[1]
for error in self.err:
if error:
self.killMotors()
#self.writeError()
rospy.loginfo("Motor controller Error: " + str(error))
return 1
def writeError(self):
'''
Writes the list of errors to a text file for later examination
'''
f = open('errorLog.txt', 'a')
errors = ','.join(str(e) for e in self.err)
f.write('\n' + 'Errors: ' + '[' + errors + ']' + ' at: ' +
str(datetime.datetime.now()))
f.close()
# 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 = [
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) rc.SpeedM1(0x80, 250) rc.ReadM1MaxCurrent(0x80) rc.ReadCurrents(0x80) error = rc.ReadError(0x80)[1] format(error, "08b") # "Position" API appears to be for times when we want to act like # a servo motor. Probably not applicable here. """ P = 600 I = 0 D = 400
