class RoboclawWrapper(object):
"""Interface between the roboclaw motor drivers and the higher level rover code"""
def __init__(self):
rospy.loginfo( "Initializing motor controllers")
# initialize attributes
self.rc = None
self.err = [None] * 5
self.address = []
self.current_enc_vals = None
self.mutex = False
self.roboclaw_mapping = rospy.get_param('~roboclaw_mapping')
self.encoder_limits = {}
self.establish_roboclaw_connections()
self.killMotors() # don't move at start
self.setup_encoders()
# save settings to non-volatile (permanent) memory
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
self.corner_max_vel = 1000
self.corner_accel = 2000
self.roboclaw_overflow = 2**15-1
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.killMotors()
# set up publishers and subscribers
self.corner_cmd_sub = rospy.Subscriber("/cmd_corner", CommandCorner, self.corner_cmd_cb, queue_size=1)
self.drive_cmd_sub = rospy.Subscriber("/cmd_drive", CommandDrive, self.drive_cmd_cb, queue_size=1)
self.enc_pub = rospy.Publisher("/encoder", JointState, queue_size=1)
self.status_pub = rospy.Publisher("/status", Status, queue_size=1)
def run(self):
"""Blocking loop which runs after initialization has completed"""
rate = rospy.Rate(5)
mutex_rate = rospy.Rate(10)
status = Status()
counter = 0
while not rospy.is_shutdown():
while self.mutex and not rospy.is_shutdown():
mutex_rate.sleep()
self.mutex = True
# read from roboclaws and publish
try:
self.read_encoder_values()
self.enc_pub.publish(self.current_enc_vals)
except AssertionError as read_exc:
rospy.logwarn( "Failed to read encoder values")
if (counter >= 10):
status.battery = self.getBattery()
status.temp = self.getTemp()
status.current = self.getCurrents()
status.error_status = self.getErrors()
self.status_pub.publish(status)
counter = 0
self.mutex = False
counter += 1
rate.sleep()
def establish_roboclaw_connections(self):
"""
Attempt connecting to the roboclaws
:raises Exception: when connection to one or more of the roboclaws is unsuccessful
"""
self.rc = Roboclaw(rospy.get_param('/motor_controller/device', "/dev/serial0"),
rospy.get_param('/motor_controller/baud_rate', 115200))
self.rc.Open()
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])
# initialize connection status to successful
all_connected = True
for address in self.address:
rospy.logdebug("Attempting to talk to motor controller ''".format(address))
version_response = self.rc.ReadVersion(address)
connected = bool(version_response[0])
if not connected:
rospy.logerr("Unable to connect to roboclaw at '{}'".format(address))
all_connected = False
else:
rospy.logdebug("Roboclaw version for address '{}': '{}'".format(address, version_response[1]))
if all_connected:
rospy.loginfo("Sucessfully connected to RoboClaw motor controllers")
else:
raise Exception("Unable to establish connection to one or more of the Roboclaw motor controllers")
def setup_encoders(self):
"""Set up the encoders"""
for motor_name, properties in self.roboclaw_mapping.iteritems():
if "corner" in motor_name:
enc_min, enc_max = self.read_encoder_limits(properties["address"], properties["channel"])
self.encoder_limits[motor_name] = (enc_min, enc_max)
else:
self.encoder_limits[motor_name] = (None, None)
self.rc.ResetEncoders(properties["address"])
def read_encoder_values(self):
"""Query roboclaws and update current motors status in encoder ticks"""
enc_msg = JointState()
enc_msg.header.stamp = rospy.Time.now()
for motor_name, properties in self.roboclaw_mapping.iteritems():
enc_msg.name.append(motor_name)
position = self.read_encoder_position(properties["address"], properties["channel"])
velocity = self.read_encoder_velocity(properties["address"], properties["channel"])
current = self.read_encoder_current(properties["address"], properties["channel"])
enc_msg.position.append(self.tick2position(position,
self.encoder_limits[motor_name][0],
self.encoder_limits[motor_name][1],
properties['ticks_per_rev'],
properties['gear_ratio']))
enc_msg.velocity.append(self.qpps2velocity(velocity,
properties['ticks_per_rev'],
properties['gear_ratio']))
enc_msg.effort.append(current)
self.current_enc_vals = enc_msg
def corner_cmd_cb(self, cmd):
r = rospy.Rate(10)
rospy.logdebug("Corner command callback received: {}".format(cmd))
while self.mutex and not rospy.is_shutdown():
r.sleep()
self.mutex = True
# convert position to tick
encmin, encmax = self.encoder_limits["corner_left_front"]
left_front_tick = self.position2tick(cmd.left_front_pos, encmin, encmax,
self.roboclaw_mapping["corner_left_front"]["ticks_per_rev"],
self.roboclaw_mapping["corner_left_front"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_left_back"]
left_back_tick = self.position2tick(cmd.left_back_pos, encmin, encmax,
self.roboclaw_mapping["corner_left_back"]["ticks_per_rev"],
self.roboclaw_mapping["corner_left_back"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_right_back"]
right_back_tick = self.position2tick(cmd.right_back_pos, encmin, encmax,
self.roboclaw_mapping["corner_right_back"]["ticks_per_rev"],
self.roboclaw_mapping["corner_right_back"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_right_front"]
right_front_tick = self.position2tick(cmd.right_front_pos, encmin, encmax,
self.roboclaw_mapping["corner_right_front"]["ticks_per_rev"],
self.roboclaw_mapping["corner_right_front"]["gear_ratio"])
self.send_position_cmd(self.roboclaw_mapping["corner_left_front"]["address"],
self.roboclaw_mapping["corner_left_front"]["channel"],
left_front_tick)
self.send_position_cmd(self.roboclaw_mapping["corner_left_back"]["address"],
self.roboclaw_mapping["corner_left_back"]["channel"],
left_back_tick)
self.send_position_cmd(self.roboclaw_mapping["corner_right_back"]["address"],
self.roboclaw_mapping["corner_right_back"]["channel"],
right_back_tick)
self.send_position_cmd(self.roboclaw_mapping["corner_right_front"]["address"],
self.roboclaw_mapping["corner_right_front"]["channel"],
right_front_tick)
self.mutex = False
def drive_cmd_cb(self, cmd):
r = rospy.Rate(10)
rospy.logdebug("Drive command callback received: {}".format(cmd))
while self.mutex and not rospy.is_shutdown():
r.sleep()
self.mutex = True
props = self.roboclaw_mapping["drive_left_front"]
vel_cmd = self.velocity2qpps(cmd.left_front_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_left_middle"]
vel_cmd = self.velocity2qpps(cmd.left_middle_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_left_back"]
vel_cmd = self.velocity2qpps(cmd.left_back_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_back"]
vel_cmd = self.velocity2qpps(cmd.right_back_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_middle"]
vel_cmd = self.velocity2qpps(cmd.right_middle_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_front"]
vel_cmd = self.velocity2qpps(cmd.right_front_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
self.mutex = False
def send_position_cmd(self, address, channel, target_tick):
"""
Wrapper around one of the send position commands
:param address:
:param channel:
:param target_tick: int
"""
cmd_args = [self.corner_accel, self.corner_max_vel, self.corner_accel, target_tick, 1]
if channel == "M1":
return self.rc.SpeedAccelDeccelPositionM1(address, *cmd_args)
elif channel == "M2":
return self.rc.SpeedAccelDeccelPositionM2(address, *cmd_args)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
def read_encoder_position(self, address, channel):
"""Wrapper around self.rc.ReadEncM1 and self.rcReadEncM2 to simplify code"""
if channel == "M1":
val = self.rc.ReadEncM1(address)
elif channel == "M2":
val = self.rc.ReadEncM2(address)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
assert val[0] == 1
return val[1]
def read_encoder_limits(self, address, channel):
"""Wrapper around self.rc.ReadPositionPID and returns subset of the data
:return: (enc_min, enc_max)
"""
if channel == "M1":
result = self.rc.ReadM1PositionPID(address)
elif channel == "M2":
result = self.rc.ReadM2PositionPID(address)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
assert result[0] == 1
return (result[-2], result[-1])
def send_velocity_cmd(self, address, channel, target_qpps):
"""
Wrapper around one of the send velocity commands
:param address:
:param channel:
:param target_qpps: int
"""
# clip values
target_qpps = max(-self.roboclaw_overflow, min(self.roboclaw_overflow, target_qpps))
accel = self.accel_pos
if target_qpps < 0:
accel = self.accel_neg
if channel == "M1":
return self.rc.DutyAccelM1(address, accel, target_qpps)
elif channel == "M2":
return self.rc.DutyAccelM2(address, accel, target_qpps)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
def read_encoder_velocity(self, address, channel):
"""Wrapper around self.rc.ReadSpeedM1 and self.rcReadSpeedM2 to simplify code"""
if channel == "M1":
val = self.rc.ReadSpeedM1(address)
elif channel == "M2":
val = self.rc.ReadSpeedM2(address)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
assert val[0] == 1
return val[1]
def read_encoder_current(self, address, channel):
"""Wrapper around self.rc.ReadCurrents to simplify code"""
if channel == "M1":
return self.rc.ReadCurrents(address)[0]
elif channel == "M2":
return self.rc.ReadCurrents(address)[1]
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
def tick2position(self, tick, enc_min, enc_max, ticks_per_rev, gear_ratio):
"""
Convert the absolute position from ticks to radian relative to the middle position
:param tick:
:param enc_min:
:param enc_max:
:param ticks_per_rev:
:return:
"""
ticks_per_rad = ticks_per_rev / (2 * math.pi)
if enc_min is None or enc_max is None:
return tick / ticks_per_rad
mid = enc_min + (enc_max - enc_min) / 2
# positive values correspond to the wheel turning left (z-axis points up)
return -(tick - mid) / ticks_per_rad * gear_ratio
def position2tick(self, position, enc_min, enc_max, ticks_per_rev, gear_ratio):
"""
Convert the absolute position from radian relative to the middle position to ticks
Clip values that are outside the range [enc_min, enc_max]
:param position:
:param enc_min:
:param enc_max:
:param ticks_per_rev:
:return:
"""
# positive values correspond to the wheel turning left (z-axis points up)
position *= -1
ticks_per_rad = ticks_per_rev / (2 * math.pi)
if enc_min is None or enc_max is None:
return position * ticks_per_rad
mid = enc_min + (enc_max - enc_min) / 2
tick = int(mid + position * ticks_per_rad / gear_ratio)
return max(enc_min, min(enc_max, tick))
def qpps2velocity(self, qpps, ticks_per_rev, gear_ratio):
"""
Convert the given quadrature pulses per second to radian/s
:param qpps: int
:param ticks_per_rev:
:param gear_ratio:
:return:
"""
return qpps / (2 * math.pi * gear_ratio * ticks_per_rev)
def velocity2qpps(self, velocity, ticks_per_rev, gear_ratio):
"""
Convert the given velocity to quadrature pulses per second
:param velocity: rad/s
:param ticks_per_rev:
:param gear_ratio:
:return: int
"""
return int(velocity * 2 * math.pi * gear_ratio * ticks_per_rev)
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()
rospy.logerr("Motor controller Error: \n'{}'".format(error))
print "Name of the file: ", fo.name
print "Closed or not : ", fo.closed
print "Opening mode : ", fo.mode
print "Softspace flag : ", fo.softspace
j = 0
data = ""
i = 0
motor_data = []
motor_data.append(0)
motor_data.append(0)
motor_data.append(0)
motor_data.append(0)
motor_data.append(0)
rc.SpeedAccelDeccelPositionM1(address, 0, 0, 0, 0, 0)
rc.SpeedAccelDeccelPositionM2(address, 0, 0, 0, 0, 0)
time.sleep(1)
init_time = time.clock()
while True:
chunk = fo.readline()
if chunk == '':
break
else:
#print "Data is :",chunk
for j in range(0, len(chunk)):
if (chunk[j] == ','):
motor_data[i] = float(data)
class RoboclawWrapper(object):
"""Interface between the roboclaw motor drivers and the higher level rover code"""
def __init__(self):
rospy.loginfo("Initializing motor controllers")
# initialize attributes
self.rc = None
self.err = [None] * 5
self.address = []
self.current_enc_vals = None
self.corner_cmd_buffer = None
self.drive_cmd_buffer = None
self.roboclaw_mapping = rospy.get_param('~roboclaw_mapping')
self.encoder_limits = {}
self.establish_roboclaw_connections()
self.stop_motors() # don't move at start
self.setup_encoders()
# save settings to non-volatile (permanent) memory
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
self.corner_max_vel = 1000
# corner motor acceleration
# Even though the actual method takes longs (2*32-1), roboclaw blog says 2**15 is 100%
accel_max = 2**15 - 1
accel_rate = rospy.get_param('/corner_acceleration_factor', 0.8)
self.corner_accel = int(accel_max * accel_rate)
self.roboclaw_overflow = 2**15 - 1
# drive motor acceleration
accel_max = 2**15 - 1
accel_rate = rospy.get_param('/drive_acceleration_factor', 0.5)
self.drive_accel = int(accel_max * accel_rate)
self.velocity_timeout = rospy.Duration(
rospy.get_param('/velocity_timeout', 2.0))
self.time_last_cmd = rospy.Time.now()
self.stop_motors()
# set up publishers and subscribers
self.corner_cmd_sub = rospy.Subscriber("/cmd_corner",
CommandCorner,
self.corner_cmd_cb,
queue_size=1)
self.drive_cmd_sub = rospy.Subscriber("/cmd_drive",
CommandDrive,
self.drive_cmd_cb,
queue_size=1)
self.enc_pub = rospy.Publisher("/encoder", JointState, queue_size=1)
self.status_pub = rospy.Publisher("/status", Status, queue_size=1)
def run(self):
"""Blocking loop which runs after initialization has completed"""
rate = rospy.Rate(8)
status = Status()
counter = 0
while not rospy.is_shutdown():
# Check to see if there are commands in the buffer to send to the motor controller
if self.drive_cmd_buffer:
drive_fcn = self.send_drive_buffer_velocity
drive_fcn(self.drive_cmd_buffer)
self.drive_cmd_buffer = None
if self.corner_cmd_buffer:
self.send_corner_buffer(self.corner_cmd_buffer)
self.corner_cmd_buffer = None
# read from roboclaws and publish
try:
self.read_encoder_values()
self.enc_pub.publish(self.current_enc_vals)
except AssertionError as read_exc:
rospy.logwarn("Failed to read encoder values")
# Downsample the rate of less important data
if (counter >= 5):
status.battery = self.read_battery()
status.temp = self.read_temperatures()
status.current = self.read_currents()
status.error_status = self.read_errors()
counter = 0
# stop the motors if we haven't received a command in a while
now = rospy.Time.now()
if now - self.time_last_cmd > self.velocity_timeout:
# rather than a hard stop, send a ramped velocity command
self.drive_cmd_buffer = CommandDrive()
self.send_drive_buffer_velocity(self.drive_cmd_buffer)
self.time_last_cmd = now # so this doesn't get called all the time
self.status_pub.publish(status)
counter += 1
rate.sleep()
def establish_roboclaw_connections(self):
"""
Attempt connecting to the roboclaws
:raises Exception: when connection to one or more of the roboclaws is unsuccessful
"""
self.rc = Roboclaw(
rospy.get_param('/motor_controller/device', "/dev/serial0"),
rospy.get_param('/motor_controller/baud_rate', 115200))
self.rc.Open()
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])
# initialize connection status to successful
all_connected = True
for address in self.address:
rospy.logdebug(
"Attempting to talk to motor controller ''".format(address))
version_response = self.rc.ReadVersion(address)
connected = bool(version_response[0])
if not connected:
rospy.logerr(
"Unable to connect to roboclaw at '{}'".format(address))
all_connected = False
else:
rospy.logdebug(
"Roboclaw version for address '{}': '{}'".format(
address, version_response[1]))
if all_connected:
rospy.loginfo(
"Sucessfully connected to RoboClaw motor controllers")
else:
raise Exception(
"Unable to establish connection to one or more of the Roboclaw motor controllers"
)
def setup_encoders(self):
"""Set up the encoders"""
for motor_name, properties in self.roboclaw_mapping.iteritems():
if "corner" in motor_name:
enc_min, enc_max = self.read_encoder_limits(
properties["address"], properties["channel"])
self.encoder_limits[motor_name] = (enc_min, enc_max)
else:
self.encoder_limits[motor_name] = (None, None)
self.rc.ResetEncoders(properties["address"])
def read_encoder_values(self):
"""Query roboclaws and update current motors status in encoder ticks"""
enc_msg = JointState()
enc_msg.header.stamp = rospy.Time.now()
for motor_name, properties in self.roboclaw_mapping.iteritems():
enc_msg.name.append(motor_name)
position = self.read_encoder_position(properties["address"],
properties["channel"])
velocity = self.read_encoder_velocity(properties["address"],
properties["channel"])
current = self.read_encoder_current(properties["address"],
properties["channel"])
enc_msg.position.append(
self.tick2position(position,
self.encoder_limits[motor_name][0],
self.encoder_limits[motor_name][1],
properties['ticks_per_rev'],
properties['gear_ratio']))
enc_msg.velocity.append(
self.qpps2velocity(velocity, properties['ticks_per_rev'],
properties['gear_ratio']))
enc_msg.effort.append(current)
self.current_enc_vals = enc_msg
def corner_cmd_cb(self, cmd):
"""
Takes the corner command and stores it in the buffer to be sent
on the next iteration of the run() loop.
"""
rospy.logdebug("Corner command callback received: {}".format(cmd))
self.time_last_cmd = rospy.Time.now()
self.corner_cmd_buffer = cmd
def send_corner_buffer(self, cmd):
"""
Sends the corner command to the motor controller.
"""
# convert position to tick
encmin, encmax = self.encoder_limits["corner_left_front"]
left_front_tick = self.position2tick(
cmd.left_front_pos, encmin, encmax,
self.roboclaw_mapping["corner_left_front"]["ticks_per_rev"],
self.roboclaw_mapping["corner_left_front"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_left_back"]
left_back_tick = self.position2tick(
cmd.left_back_pos, encmin, encmax,
self.roboclaw_mapping["corner_left_back"]["ticks_per_rev"],
self.roboclaw_mapping["corner_left_back"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_right_back"]
right_back_tick = self.position2tick(
cmd.right_back_pos, encmin, encmax,
self.roboclaw_mapping["corner_right_back"]["ticks_per_rev"],
self.roboclaw_mapping["corner_right_back"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_right_front"]
right_front_tick = self.position2tick(
cmd.right_front_pos, encmin, encmax,
self.roboclaw_mapping["corner_right_front"]["ticks_per_rev"],
self.roboclaw_mapping["corner_right_front"]["gear_ratio"])
self.send_position_cmd(
self.roboclaw_mapping["corner_left_front"]["address"],
self.roboclaw_mapping["corner_left_front"]["channel"],
left_front_tick)
self.send_position_cmd(
self.roboclaw_mapping["corner_left_back"]["address"],
self.roboclaw_mapping["corner_left_back"]["channel"],
left_back_tick)
self.send_position_cmd(
self.roboclaw_mapping["corner_right_back"]["address"],
self.roboclaw_mapping["corner_right_back"]["channel"],
right_back_tick)
self.send_position_cmd(
self.roboclaw_mapping["corner_right_front"]["address"],
self.roboclaw_mapping["corner_right_front"]["channel"],
right_front_tick)
def drive_cmd_cb(self, cmd):
"""
Takes the drive command and stores it in the buffer to be sent
on the next iteration of the run() loop.
"""
rospy.logdebug("Drive command callback received: {}".format(cmd))
self.drive_cmd_buffer = cmd
self.time_last_cmd = rospy.Time.now()
def send_drive_buffer_velocity(self, cmd):
"""
Sends the drive command to the motor controller, closed loop velocity commands
"""
props = self.roboclaw_mapping["drive_left_front"]
vel_cmd = self.velocity2qpps(cmd.left_front_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_left_middle"]
vel_cmd = self.velocity2qpps(cmd.left_middle_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_left_back"]
vel_cmd = self.velocity2qpps(cmd.left_back_vel, props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_back"]
vel_cmd = self.velocity2qpps(cmd.right_back_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_middle"]
vel_cmd = self.velocity2qpps(cmd.right_middle_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_front"]
vel_cmd = self.velocity2qpps(cmd.right_front_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
def send_position_cmd(self, address, channel, target_tick):
"""
Wrapper around one of the send position commands
:param address:
:param channel:
:param target_tick: int
"""
cmd_args = [
self.corner_accel, self.corner_max_vel, self.corner_accel,
target_tick, 1
]
if channel == "M1":
return self.rc.SpeedAccelDeccelPositionM1(address, *cmd_args)
elif channel == "M2":
return self.rc.SpeedAccelDeccelPositionM2(address, *cmd_args)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
def read_encoder_position(self, address, channel):
"""Wrapper around self.rc.ReadEncM1 and self.rcReadEncM2 to simplify code"""
if channel == "M1":
val = self.rc.ReadEncM1(address)
elif channel == "M2":
val = self.rc.ReadEncM2(address)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
assert val[0] == 1
return val[1]
def read_encoder_limits(self, address, channel):
"""Wrapper around self.rc.ReadPositionPID and returns subset of the data
:return: (enc_min, enc_max)
"""
if channel == "M1":
result = self.rc.ReadM1PositionPID(address)
elif channel == "M2":
result = self.rc.ReadM2PositionPID(address)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
assert result[0] == 1
return (result[-2], result[-1])
def send_velocity_cmd(self, address, channel, target_qpps):
"""
Wrapper around one of the send velocity commands
:param address:
:param channel:
:param target_qpps: int
"""
# clip values
target_qpps = max(-self.roboclaw_overflow,
min(self.roboclaw_overflow, target_qpps))
if channel == "M1":
return self.rc.SpeedAccelM1(address, self.drive_accel, target_qpps)
elif channel == "M2":
return self.rc.SpeedAccelM2(address, self.drive_accel, target_qpps)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
def read_encoder_velocity(self, address, channel):
"""Wrapper around self.rc.ReadSpeedM1 and self.rcReadSpeedM2 to simplify code"""
if channel == "M1":
val = self.rc.ReadSpeedM1(address)
elif channel == "M2":
val = self.rc.ReadSpeedM2(address)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
assert val[0] == 1
return val[1]
def read_encoder_current(self, address, channel):
"""Wrapper around self.rc.ReadCurrents to simplify code"""
if channel == "M1":
return self.rc.ReadCurrents(address)[0]
elif channel == "M2":
return self.rc.ReadCurrents(address)[1]
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
def tick2position(self, tick, enc_min, enc_max, ticks_per_rev, gear_ratio):
"""
Convert the absolute position from ticks to radian relative to the middle position
:param tick:
:param enc_min:
:param enc_max:
:param ticks_per_rev:
:return:
"""
ticks_per_rad = ticks_per_rev / (2 * math.pi)
if enc_min is None or enc_max is None:
return tick / ticks_per_rad
mid = enc_min + (enc_max - enc_min) / 2
return (tick - mid) / ticks_per_rad * gear_ratio
def position2tick(self, position, enc_min, enc_max, ticks_per_rev,
gear_ratio):
"""
Convert the absolute position from radian relative to the middle position to ticks
Clip values that are outside the range [enc_min, enc_max]
:param position:
:param enc_min:
:param enc_max:
:param ticks_per_rev:
:return:
"""
ticks_per_rad = ticks_per_rev / (2 * math.pi)
if enc_min is None or enc_max is None:
return position * ticks_per_rad
mid = enc_min + (enc_max - enc_min) / 2
tick = int(mid + position * ticks_per_rad / gear_ratio)
return max(enc_min, min(enc_max, tick))
def qpps2velocity(self, qpps, ticks_per_rev, gear_ratio):
"""
Convert the given quadrature pulses per second to radian/s
:param qpps: int
:param ticks_per_rev:
:param gear_ratio:
:return:
"""
return qpps * 2 * math.pi / (gear_ratio * ticks_per_rev)
def velocity2qpps(self, velocity, ticks_per_rev, gear_ratio):
"""
Convert the given velocity to quadrature pulses per second
:param velocity: rad/s
:param ticks_per_rev:
:param gear_ratio:
:return: int
"""
return int(velocity * gear_ratio * ticks_per_rev / (2 * math.pi))
def read_battery(self):
"""Read battery voltage from one of the roboclaws as a proxy for all roboclaws"""
# roboclaw reports value in 10ths of a Volt
return self.rc.ReadMainBatteryVoltage(self.address[0])[1] / 10.0
def read_temperatures(self):
temp = [None] * 5
for i in range(5):
# reported by roboclaw in 10ths of a Celsius
temp[i] = self.rc.ReadTemp(self.address[i])[1] / 10.0
return temp
def read_currents(self):
currents = [None] * 10
for i in range(5):
currs = self.rc.ReadCurrents(self.address[i])
# reported by roboclaw in 10ths of an Ampere
currents[2 * i] = currs[1] / 100.0
currents[(2 * i) + 1] = currs[2] / 100.0
return currents
def stop_motors(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 read_errors(self):
"""Checks error status of each motor controller, returns 0 if no errors reported"""
err = [0] * 5
for i in range(len(self.address)):
err[i] = self.rc.ReadError(self.address[i])[1]
if err[i] != 0:
rospy.logerr(
"Motor controller '{}' reported error code {}".format(
self.address[i], err[i]))
return err
roboclaw = Roboclaw("/dev/ttyS0", 38400)
roboclaw.Open()
# Read encoder
motor_1_count = roboclaw.ReadEncM1(0x80)
print "Original:"
print motor_1_count
sleep(2)
# Set encoder and then read and print to test operation
roboclaw.SetEncM1(0x80, 10000)
motor_1_count = roboclaw.ReadEncM1(0x80)
print "After setting count:"
print motor_1_count
sleep(2)
# Reset encoders and read and print value to test operation
roboclaw.ResetEncoders(0x80)
motor_1_count = roboclaw.ReadEncM1(0x80)
print "After resetting:"
print motor_1_count
sleep(2)
# Position motor, these values may have to be changed to suit the motor/encoder combination in use
roboclaw.SpeedAccelDeccelPositionM1(0x80,10000,2000,10000,15000,1)
class Rover():
'''
Rover class contains all the math and motor control algorithms to move the rover
In order to call command the rover the only method necessary is the drive() method
'''
def __init__(self):
'''
Initialization of communication parameters for the Motor Controllers
'''
self.rc = Roboclaw("/dev/ttyS0", 115200)
self.rc.Open()
self.address = [0x80, 0x81, 0x82, 0x83, 0x84]
self.rc.ResetEncoders(self.address[0])
self.rc.ResetEncoders(self.address[1])
self.rc.ResetEncoders(self.address[2])
self.err = [None] * 5
def getCornerDeg(self):
'''
Returns a list of angles [Deg] that each of the Corners are currently pointed at
'''
encoders = [0] * 4
for i in range(4):
index = int(math.ceil((i + 1) / 2.0) + 2)
if (i % 2):
enc = self.rc.ReadEncM2(self.address[index])[1]
else:
enc = self.rc.ReadEncM1(self.address[index])[1]
encoders[i] = int(cals[i][0] * math.pow(enc, 2) +
cals[i][1] * enc + cals[i][2])
return encoders
@staticmethod
def approxTurningRadius(enc):
'''
Takes the list of current corner angles and approximates the current turning radius [inches]
:param list [int] enc: List of encoder ticks for each corner motor
'''
if enc[0] == None:
return 250
try:
if enc[0] > 0:
r1 = (d1 / math.tan(math.radians(abs(enc[0])))) + d3
r2 = (d2 / math.tan(math.radians(abs(enc[1])))) + d3
r3 = (d2 / math.tan(math.radians(abs(enc[2])))) - d3
r4 = (d1 / math.tan(math.radians(abs(enc[3])))) - d3
else:
r1 = -(d1 / math.tan(math.radians(abs(enc[0])))) - d3
r2 = -(d2 / math.tan(math.radians(abs(enc[1])))) - d3
r3 = -(d2 / math.tan(math.radians(abs(enc[2])))) + d3
r4 = -(d1 / math.tan(math.radians(abs(enc[3])))) + d3
radius = (r1 + r2 + r3 + r4) / 4
return radius
except:
return 250
@staticmethod
def calTargetDeg(radius):
'''
Takes a turning radius and calculates what angle [degrees] each corner should be at
:param int radius: Radius drive command, ranges from -100 (turning left) to 100 (turning right)
'''
#Scaled from 250 to 220 inches. For more information on these numbers look at the Software Controls.pdf
if radius == 0:
r = 250
elif -100 <= radius <= 100:
r = 220 - abs(radius) * (250 / 100)
else:
r = 250
if r == 250:
return [0] * 4
ang1 = int(math.degrees(math.atan(d1 / (abs(r) + d3))))
ang2 = int(math.degrees(math.atan(d2 / (abs(r) + d3))))
ang3 = int(math.degrees(math.atan(d2 / (abs(r) - d3))))
ang4 = int(math.degrees(math.atan(d1 / (abs(r) - d3))))
if radius > 0:
return [ang2, -ang1, -ang4, ang3]
else:
return [-ang4, ang3, ang2, -ang1]
@staticmethod
def calVelocity(v, r):
'''
Returns a list of speeds for each individual drive motor based on current turning radius
:param int v: Drive speed command range from -100 to 100
:param int r: Turning radius command range from -100 to 100
'''
v = int(v) * (127 / 100)
if (v == 0):
return [v] * 6
if (r == 0 or r >= 250 or r <= -250):
return [v] * 6 # No turning radius, all wheels same speed
else:
x = v / (abs(r) + d4) # Wheels can't move faster than max (127)
a = math.pow(d2, 2)
b = math.pow(d3, 2)
c = math.pow(abs(r) + d1, 2)
d = math.pow(abs(r) - d1, 2)
e = abs(r) - d4
v1 = int(x * math.sqrt(b + d))
v2 = int(x * e) # Slowest wheel
v3 = int(x * math.sqrt(a + d))
v4 = int(x * math.sqrt(a + c))
v5 = int(v) # Fastest wheel
v6 = int(x * math.sqrt(b + c))
if (r < 0):
velocity = [v1, v2, v3, v4, v5, v6]
elif (r > 0):
velocity = [v6, v5, v4, v3, v2, v1]
return velocity
def cornerPosControl(self, tar_enc):
'''
Takes the target angle and gets what encoder tick that value is for position control
:param list [int] tar_enc: List of target angles in degrees for each corner
'''
x = [0] * 4
for i in range(4):
a, b, c = cals[i][0], cals[i][1], cals[i][2] - tar_enc[i]
d = b**2 - 4 * a * c
if d < 0:
print 'no soln'
elif d == 0:
x[i] = int((-b + math.sqrt(d[i])) / (2 * a))
else:
x1 = (-b + math.sqrt(d)) / (2 * a)
x2 = (-b - math.sqrt(d)) / (2 * a)
if x1 > 0 and x2 <= 0:
x[i] = int(x1)
else:
x[i] = int(x2) #I don't think this case can ever happen.
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 (i % 2):
self.rc.SpeedAccelDeccelPositionM2(self.address[index], accel,
speed, accel, x[i], 1)
else:
self.rc.SpeedAccelDeccelPositionM1(self.address[index], accel,
speed, accel, x[i], 1)
def motorDuty(self, motorID, speed):
'''
Wrapper method for an easier interface to control the motors
:param int motorID: number that corresponds to each physical motor
:param int speed: Speed for each motor, range from 0-127
'''
addr = {
0: self.address[3],
1: self.address[3],
2: self.address[4],
3: self.address[4],
4: self.address[0],
5: self.address[0],
6: self.address[1],
7: self.address[1],
8: self.address[2],
9: self.address[2]
}
#drive forward
if (speed >= 0):
command = {
0: self.rc.ForwardM1,
1: self.rc.ForwardM2,
2: self.rc.ForwardM1,
3: self.rc.ForwardM2,
4: self.rc.ForwardM1,
5:
self.rc.BackwardM2, #some are backward based on wiring diagram
6: self.rc.ForwardM1,
7: self.rc.ForwardM2,
8: self.rc.BackwardM1,
9: self.rc.ForwardM2
}
#drive backward
else:
command = {
0: self.rc.BackwardM1,
1: self.rc.BackwardM2,
2: self.rc.BackwardM1,
3: self.rc.BackwardM2,
4: self.rc.BackwardM1,
5: self.rc.ForwardM2,
6: self.rc.BackwardM1,
7: self.rc.BackwardM2,
8: self.rc.ForwardM1,
9: self.rc.BackwardM2
}
speed = abs(speed)
return command[motorID](addr[motorID], speed)
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 != 0:
return 0
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()
def drive(self, v, r):
'''
Driving method for the Rover, rover will not do any commands if any motor controller
throws an error
:param int v: driving velocity command, % based from -100 (backward) to 100 (forward)
:param int r: driving turning radius command, % based from -100 (left) to 100 (right)
'''
if self.errorCheck():
current_radius = self.approxTurningRadius(self.getCornerDeg())
velocity = self.calVelocity(v, current_radius)
self.cornerPosControl(self.calTargetDeg(r))
for i in range(6):
self.motorDuty(i + 4, velocity[i])
else:
self.writeError()
raise Exception(
"Fatal: Motor controller(s) reported errors. See errorLog.txt."
)
def killMotors(self):
'''
Stops all motors on Rover
'''
for i in range(0, 10):
self.motorDuty(i, 0)
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()
version1 = rc.ReadVersion(address1)
version2 = rc.ReadVersion(address2)
version3 = rc.ReadVersion(address3)
if version1[0] == False:
print "GETVERSION Failed"
else:
print repr(version1[1])
print repr(version2[1])
print repr(version3[1])
while (1):
rc.SpeedAccelDeccelPositionM2(address1, 2000, 10000, 10000, 39000, 100)
rc.SpeedAccelDeccelPositionM1(address2, 2000, 10000, 10000, 12000, 100)
rc.SpeedAccelDeccelPositionM2(address2, 2000, 10000, 10000, 5000, 100)
movedifftrans(address3, 30, 0)
time.sleep(4)
rc.SpeedAccelDeccelPositionM2(address1, 2000, 10000, 10000, 39000, 100)
rc.SpeedAccelDeccelPositionM1(address2, 2000, 10000, 10000, 13000, 100)
rc.SpeedAccelDeccelPositionM2(address2, 2000, 10000, 10000, 6000, 100)
movedifftrans(address3, 30, 0)
time.sleep(1)
rc.SpeedM1(address1, 50)
time.sleep(3)
rc.SpeedAccelDeccelPositionM2(address1, 2000, 10000, 10000, 24000, 100)
rc.SpeedAccelDeccelPositionM1(address2, 2000, 10000, 10000, 2500, 100)
if (speed1[0]):
print speed1[1],
else:
print "failed",
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed "
rc.Open()
address = 0x80
while (1):
print "Pos 50000"
rc.SpeedAccelDeccelPositionM1(address, 32000, 12000, 32000, 50000, 0)
for i in range(0, 80):
displayspeed()
time.sleep(0.1)
time.sleep(2)
print "Pos 0"
rc.SpeedAccelDeccelPositionM1(address, 32000, 12000, 32000, 0, 0)
for i in range(0, 80):
displayspeed()
time.sleep(0.1)
time.sleep(2)
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()
def arduinoRead(port, port2):
f = open(outputFile, "r+")
rc = Roboclaw(port2, 115200)
rc.Open()
rc.SpeedAccelDeccelPositionM1(address, 0, 0, 0, 0, 0)
rc.SpeedAccelDeccelPositionM2(address, 0, 0, 0, 0, 0)
t10 = 0
k10 = 0
box = serial.Serial(port, 115200)
while True:
x = box.readline()
try:
x = x.decode("utf-8")
#print("Input")
# print(x)
#print("input end")
data = x.split(',')
t1 = float(data[0])
t2 = float(data[1])
k1 = float(data[2])
k2 = float(data[3])
#print("INDV")
print(t1),
print(t2),
print(k1),
print(k2)
#print("indv end")
except:
print("This Line not processed")
continue
try:
t2 = 89
k2 = 89
t1 = inv.getLeftHip(t1) - 544.7462849003102
except:
print("t1")
try:
t2 = inv.getRightHip(t2) - 544.7462849003102
k1 = inv.getLeftKnee(k1) - 288.5185921120647
k2 = inv.getRightKnee(k2) - 288.5185921120647
except:
print("k1")
continue
#print("INVK")
f.write(str(t1))
f.write(',')
f.write(str(k1))
f.write('\n')
print(t1),
print(t2),
print(k1),
print(k2)
#print("invk end")
try:
#rc1.SpeedAccelDeccelPositionM1(address,0,0,0,int(-(k2/360)*2000),0)
#rc1.SpeedAccelDeccelPositionM2(address,0,0,0,int(-(t2/360)*2000),0)
rc.SpeedAccelDeccelPositionM1(address, 0, 0, 0, -int(
(t1 / 360) * 2000), 0)
rc.SpeedAccelDeccelPositionM2(address, 0, 0, 0, -int(
(k1 / 360) * 2000), 0)
except:
print("Error rotating motor")
class comm:
def __init__(self):
self.rc = Roboclaw("/dev/ttyS0", 115200)
self.rc.Open()
self.accel = [0] * 10
self.qpps = [None] * 10
self.err = [None] * 5
self.address = [128, 129, 130, 131, 132]
'''
version = 1
for address in self.address:
print("Attempting to talk to motor controller",address)
version = version & self.rc.ReadVersion(0x80)
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")
'''
def accellDriveMotors(self, speeds):
for i in range(3):
self.rc.SpeedAccelM1(self.address[i], 7000,
speeds[i * 2] * 32767 / 500)
self.rc.SpeedAccelM2(self.address[i], 7000,
speeds[(i * 2 + 1)] * 32767 / 500)
def getCornerEncoders(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])
return enc
def moveCorners(self, tick):
enc = []
speed, accel = 1000, 2000
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])
if (abs(tick[i] - enc[i]) < 25):
tick[i] = -1
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 (i % 2): self.rc.ForwardM2(self.address[index], 0)
else: self.rc.ForwardM1(self.address[index], 0)