fltCo20 = float(resp0[2:])
ser1.write(b'Z\r\n')
resp1 = ser1.read(10)
resp1 = resp1[:8]
fltCo21 = float(resp1[2:])
ser2.write(b'Z\r\n')
resp2 = ser2.read(10)
resp2 = resp2[:8]
fltCo22 = float(resp2[2:])
ser3.write(b'Z\r\n')
resp3 = ser3.read(10)
resp3 = resp3[:8]
fltCo23 = float(resp3[2:])
enc1 = rc.ReadEncM1(address) #Encoder Lines
enc2 = rc.ReadEncM2(address)
speed1 = rc.ReadSpeedM1(address)
speed2 = rc.ReadSpeedM2(address)
print format(enc2[2],'02x')
print format(enc1[2],'02x')
print('Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}'.format(heading, roll, pitch))
print('accelX={0:0.2F} accelY={1:0.2F} accelZ={2:0.2F}'.format(accelX, accelY, accelZ))
print 'CO2 PPM =', fltCo20 *10
print 'CO2 PPM 2 =', fltCo21 *10 # multiplierZ
print 'CO2 PPM 3 =', fltCo22 *10 # multiplierZ
print 'CO2 PPM 4 =', fltCo23 *10 # multiplierZ
time.sleep(.1)
heading, roll, pitch = bno.read_euler()
rc.ForwardMixed(address, 0)
time.sleep(.05)
#print heading
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))
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
class motor_driver_ada:
def __init__(self, log):
self.lfbias = 68 # experimentally determined for 'Spot 2'
self.lrbias = 60
self.rrbias = 57
self.rfbias = 60
self.left_limit = -36
self.right_limit = 36
self.d1 = 7.254 #C/L to corner wheels
self.d2 = 10.5 #mid axle to fwd axle
self.d3 = 10.5 #mid axle to rear axle
self.d4 = 10.073 #C/L to mid wheels
self.rr_motor = kit.servo[0]
self.rf_motor = kit.servo[1]
self.lf_motor = kit.servo[2]
self.lr_motor = kit.servo[3]
self.rr_motor.actuation_range = 120
self.rf_motor.actuation_range = 120
self.lf_motor.actuation_range = 120
self.lr_motor.actuation_range = 120
self.rr_motor.set_pulse_width_range(700, 2300)
self.rf_motor.set_pulse_width_range(700, 2300)
self.lf_motor.set_pulse_width_range(700, 2300)
self.lr_motor.set_pulse_width_range(700, 2300)
self.log = log
self.rc = Roboclaw("/dev/ttyS0", 115200)
i = self.rc.Open()
self.lf_motor.angle = self.rfbias
self.rf_motor.angle = self.lfbias
self.lr_motor.angle = self.lrbias
self.rr_motor.angle = self.rrbias
self.stop_all()
def diag(self):
print("servo rr =" + str(self.rr_motor.angle))
print("servo rf =" + str(self.rf_motor.angle))
print("servo lf =" + str(self.lf_motor.angle))
print("servo lr =" + str(self.lr_motor.angle))
# self.turn_motor(0x80, vel, voc, 1)
def turn_motor(self, address, v, av1, av2):
v1 = int(v * av1)
v2 = int(v * av2)
if v >= 0:
self.rc.ForwardM1(address, v1)
self.rc.ForwardM2(address, v2)
else:
self.rc.BackwardM1(address, abs(v1))
self.rc.BackwardM2(address, abs(v2))
# print("m1, m2 = "+str(v1)+", "+str(v2))
def stop_all(self):
self.turn_motor(0X80, 0, 0, 0)
self.turn_motor(0X81, 0, 0, 0)
self.turn_motor(0X82, 0, 0, 0)
def motor_speed(self):
speed1 = self.rc.ReadSpeedM1(0x80)
speed2 = self.rc.ReadSpeedM2(0x80)
self.log.write("motor speed = %d, %d\n", speed1, speed2)
speed1 = self.rc.ReadSpeedM1(0x81)
speed2 = self.rc.ReadSpeedM2(0x81)
self.log.write("motor speed = %d, %d\n", speed1, speed2)
speed1 = self.rc.ReadSpeedM1(0x82)
speed2 = self.rc.ReadSpeedM2(0x82)
self.log.write("motor speed = %d, %d\n", speed1, speed2)
# based on speed & steer, command all motors
def motor(self, speed, steer):
# print("Motor speed, steer "+str(speed)+", "+str(steer))
if (steer < self.left_limit):
steer = self.left_limit
if (steer > self.right_limit):
steer = self.right_limit
# vel = speed * 1.27
vel = speed * 1.26
voc = 0
vic = 0
#roboclaw speed limit 0 to 127
# see BOT-2/18 (181201)
# rechecked 200329
if steer != 0: #if steering angle not zero, compute angles, wheel speed
angle = math.radians(abs(steer))
ric = self.d3 / math.sin(angle) #turn radius - inner corner
rm = ric * math.cos(angle) + self.d1 #body central radius
roc = math.sqrt((rm + self.d1)**2 + self.d3**2) #outer corner
rmo = rm + self.d4 #middle outer
rmi = rm - self.d4 #middle inner
phi = math.degrees(math.asin(self.d3 / roc))
if steer < 0:
phi = -phi
voc = roc / rmo #velocity corners & middle inner
vic = ric / rmo
vim = rmi / rmo
# SERVO MOTORS ARE COUNTER CLOCKWISE
# left turn
if steer < 0:
self.lf_motor.angle = self.lfbias - steer
self.rf_motor.angle = self.rfbias - phi
self.lr_motor.angle = self.lrbias + steer
self.rr_motor.angle = self.rrbias + phi
self.turn_motor(0x80, vel, voc, 1) #RC 1 - rf, rm
self.turn_motor(0x81, vel, vim, vic) #RC 2 - lm, lf
self.turn_motor(0x82, vel, voc, vic) #RC 3 - rr, lr
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
#right turn
elif steer > 0:
self.lf_motor.angle = self.lfbias - phi
self.rf_motor.angle = self.rfbias - steer
self.lr_motor.angle = self.lrbias + phi
self.rr_motor.angle = self.rrbias + steer
self.turn_motor(0x80, vel, vic, vim)
self.turn_motor(0x81, vel, 1, voc)
self.turn_motor(0x82, vel, vic, voc)
# print("80 vic, vim ",vic,vim)
# print("81 vic, voc ",vic,voc)
# print("82 vom, voc ", 1, voc)
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
#straight ahead
else:
self.lf_motor.angle = self.lfbias
self.rf_motor.angle = self.rfbias
self.lr_motor.angle = self.lrbias
self.rr_motor.angle = self.rrbias
self.turn_motor(0x80, vel, 1, 1)
self.turn_motor(0x81, vel, 1, 1)
self.turn_motor(0x82, vel, 1, 1)
# print("v, vout, vin "+str(vel)+", "+str(voc)+", "+str(vic))
# self.diag()
class motor_driver_ada:
def __init__(self, log):
self.lfbias = 48 # experimentally determined for 'Spot 2'
self.lrbias = 44
self.rrbias = 69
self.rfbias = 40
self.pan_bias = 83
self.left_limit = -36
self.right_limit = 36
self.d1 = 7.254 #C/L to corner wheels
self.d2 = 10.5 #mid axle to fwd axle
self.d3 = 10.5 #mid axle to rear axle
self.d4 = 10.073 #C/L to mid wheels
self.speedfactor = 35 # 8000 counts at 100%
self.rr_motor = kit.servo[0]
self.rf_motor = kit.servo[1]
self.lf_motor = kit.servo[2]
self.lr_motor = kit.servo[3]
self.pan = kit.servo[15]
self.tilt = kit.servo[14]
#pan_bias = 0 self.rr_motor.actuation_range = 120
self.rf_motor.actuation_range = 120
self.lf_motor.actuation_range = 120
self.lr_motor.actuation_range = 120
self.rr_motor.set_pulse_width_range(700, 2300)
self.rf_motor.set_pulse_width_range(700, 2300)
self.lf_motor.set_pulse_width_range(700, 2300)
self.lr_motor.set_pulse_width_range(700, 2300)
self.log = log
self.rc = Roboclaw("/dev/ttyS0", 115200)
i = self.rc.Open()
self.crc = 0
self.port = serial.Serial("/dev/ttyS0", baudrate=115200, timeout=0.1)
self.lf_motor.angle = self.rfbias
self.rf_motor.angle = self.lfbias
self.lr_motor.angle = self.lrbias
self.rr_motor.angle = self.rrbias
self.stop_all()
ver = self.rc.ReadVersion(0x80)
print(ver[0], ver[1])
ver = self.rc.ReadVersion(0x81)
print(ver[0], ver[1])
ver = self.rc.ReadVersion(0x82)
print(ver[0], ver[1])
def diag(self):
print("servo rr =" + str(self.rr_motor.angle))
print("servo rf =" + str(self.rf_motor.angle))
print("servo lf =" + str(self.lf_motor.angle))
print("servo lr =" + str(self.lr_motor.angle))
# self.turn_motor(0x80, vel, voc, 1)
def set_motor(self, address, v, av, m12):
vx = int(v * av)
if (m12 == 1):
self.rc.SpeedM1(address, vx)
else:
self.rc.SpeedM2(address, vx)
'''
def turn_motor(self, address, v, av1, av2):
v1 = int(v * av1)
v2 = int(v * av2)
if v >= 0:
self.rc.ForwardM1(address, v1)
self.rc.ForwardM2(address, v2)
# self.M1Forward(address, v1)
# self.M2Forward(address, v2)
else:
self.rc.BackwardM1(address, abs(v1))
self.rc.BackwardM2(address, abs(v2))
# self.M1Backward(address, abs(v1))
# self.M2Backward(address, abs(v2))
# print("m1, m2 = "+str(v1)+", "+str(v2))
'''
def stop_all(self):
self.set_motor(0X80, 0, 0, 1)
self.set_motor(0X81, 0, 0, 1)
self.set_motor(0X82, 0, 0, 1)
self.set_motor(0X80, 0, 0, 2)
self.set_motor(0X81, 0, 0, 2)
self.set_motor(0X82, 0, 0, 2)
def motor_speed(self):
speed1 = self.rc.ReadSpeedM1(0x80)
speed2 = self.rc.ReadSpeedM2(0x80)
self.log.write("motor speed = %d, %d" % (speed1[1], speed2[1]))
print("motor speed = %d, %d" % (speed1[1], speed2[1]))
speed1 = self.rc.ReadSpeedM1(0x81)
speed2 = self.rc.ReadSpeedM2(0x81)
self.log.write("motor speed = %d, %d" % (speed1[1], speed2[1]))
print("motor speed = %d, %d" % (speed1[1], speed2[1]))
speed1 = self.rc.ReadSpeedM1(0x82)
speed2 = self.rc.ReadSpeedM2(0x82)
self.log.write("motor speed = %d, %d" % (speed1[1], speed2[1]))
print("motor speed = %d, %d" % (speed1[1], speed2[1]))
# self.battery_voltage()
err = self.rc.ReadError(0x80)[1]
if err:
print("status of 0x80", err)
self.log.write("0x80 error: %d" % err)
err = self.rc.ReadError(0x81)[1]
if err:
print("status of 0x81", err)
self.log.write("0x81 error: %d" % err)
err = self.rc.ReadError(0x82)[1]
if err:
print("status of 0x82", err)
self.log.write("0x82 error: %d" % err)
def battery_voltage(self):
volts = self.rc.ReadMainBatteryVoltage(0x80)[1] / 10.0
print("Ada Voltage = ", volts)
self.log.write("Voltage: %5.1f\n" % volts)
return (volts)
# based on speed & steer, command all motors
def motor(self, speed, steer):
# self.log.write("Motor speed, steer "+str(speed)+", "+str(steer)+'\n')
if (steer < self.left_limit):
steer = self.left_limit
if (steer > self.right_limit):
steer = self.right_limit
# vel = speed * 1.26
vel = self.speedfactor * speed
voc = 0
vic = 0
#roboclaw speed limit 0 to 127
# see BOT-2/18 (181201)
# math rechecked 200329
if steer != 0: #if steering angle not zero, compute angles, wheel speed
angle = math.radians(abs(steer))
ric = self.d3 / math.sin(angle) #turn radius - inner corner
rm = ric * math.cos(angle) + self.d1 #body central radius
roc = math.sqrt((rm + self.d1)**2 + self.d3**2) #outer corner
rmo = rm + self.d4 #middle outer
rmi = rm - self.d4 #middle inner
phi = math.degrees(math.asin(self.d3 / roc))
if steer < 0:
phi = -phi
voc = roc / rmo #velocity corners & middle inner
vic = ric / rmo
vim = rmi / rmo
# SERVO MOTORS ARE COUNTER CLOCKWISE
# left turn
if steer < 0:
self.lf_motor.angle = self.lfbias - steer
self.rf_motor.angle = self.rfbias - phi
self.lr_motor.angle = self.lrbias + steer
self.rr_motor.angle = self.rrbias + phi
# self.turn_motor(0x80, vel, voc, 1) #RC 1 - rf, rm
# self.turn_motor(0x81, vel, voc, vic) #RC 2 - lm, lf
# self.turn_motor(0x82, vel, vim, vic) #RC 3 - rr, lr
self.set_motor(0x80, vel, voc, 1) #RC 1 - rf, rm
self.set_motor(0x81, vel, voc, 1) #RC 2 - lm, lf
self.set_motor(0x82, vel, vim, 1) #RC 3 - rr, lr
self.set_motor(0x80, vel, 1, 2) #RC 1 - rf, rm
self.set_motor(0x81, vel, vic, 2) #RC 2 - lm, lf
self.set_motor(0x82, vel, vic, 2) #RC 3 - rr, lr
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
#right turn
elif steer > 0:
self.lf_motor.angle = self.lfbias - phi
self.rf_motor.angle = self.rfbias - steer
self.lr_motor.angle = self.lrbias + phi
self.rr_motor.angle = self.rrbias + steer
# self.turn_motor(0x80, vel, vic, vim)
# self.turn_motor(0x81, vel, vic, voc)
# self.turn_motor(0x82, vel, 1, voc)
self.set_motor(0x80, vel, vic, 1)
self.set_motor(0x81, vel, vic, 1)
self.set_motor(0x82, vel, 1, 1)
self.set_motor(0x80, vel, vim, 2)
self.set_motor(0x81, vel, voc, 2)
self.set_motor(0x82, vel, voc, 2)
# print("80 vic, vim ",vic,vim)
# print("81 vic, voc ",vic,voc)
# print("82 vom, voc ", 1, voc)
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
#straight ahead
else:
self.lf_motor.angle = self.lfbias
self.rf_motor.angle = self.rfbias
self.lr_motor.angle = self.lrbias
self.rr_motor.angle = self.rrbias
self.set_motor(0x80, vel, 1, 1)
self.set_motor(0x81, vel, 1, 1)
self.set_motor(0x82, vel, 1, 1)
self.set_motor(0x80, vel, 1, 2)
self.set_motor(0x81, vel, 1, 2)
self.set_motor(0x82, vel, 1, 2)
# print("v, vout, vin "+str(vel)+", "+str(voc)+", "+str(vic))
# self.diag()
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
def sensor_pan(self, angle):
self.pan.angle = self.pan_bias + angle
def depower(self):
self.lf_motor.angle = None
self.rf_motor.angle = None
self.lr_motor.angle = None
self.rr_motor.angle = None
self.pan.angle = None
class motor_driver:
def __init__(self):
self.i2c = busio.I2C(SCL, SDA)
self.pca = PCA9685(self.i2c)
self.pca.frequency = 50
self.br_motor = servo.Servo(self.pca.channels[0],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.fr_motor = servo.Servo(self.pca.channels[1],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.fl_motor = servo.Servo(self.pca.channels[2],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.bl_motor = servo.Servo(self.pca.channels[3],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.rc = Roboclaw("/dev/ttyS0", 115200)
i = self.rc.Open()
self.d1 = 7.254 #C/L to corner wheels
self.d2 = 10.5 #mid axle to fwd axle
self.d3 = 10.5 #mid axle to rear axle
self.d4 = 10.073 #C/L to mid wheels
self.fl_motor.angle = bias
self.fr_motor.angle = bias
self.bl_motor.angle = bias
self.br_motor.angle = bias
def diag(self):
print("servo br =" + str(self.br_motor.angle))
print("servo fr =" + str(self.fr_motor.angle))
print("servo fl =" + str(self.fl_motor.angle))
print("servo bl =" + str(self.bl_motor.angle))
# self.turn_motor(0x80, vel, voc, 1)
def turn_motor(self, address, v, av1, av2):
v1 = int(v * av1)
v2 = int(v * av2)
if v >= 0:
self.rc.ForwardM1(address, v1)
self.rc.ForwardM2(address, v2)
else:
self.rc.BackwardM1(address, abs(v1))
self.rc.BackwardM2(address, abs(v2))
# print("m1, m2 = "+str(v1)+", "+str(v2))
def stop_all(self):
self.turn_motor(0X80, 0, 0, 0)
self.turn_motor(0X81, 0, 0, 0)
self.turn_motor(0X82, 0, 0, 0)
def motor_speed(self):
speed = self.rc.ReadSpeedM1(0x82)
print("motor speed =" + str(speed))
speed = self.rc.ReadSpeedM2(0x81)
print("motor speed =" + str(speed))
# based on speed & steer, command all motors
def motor(self, speed, steer):
# print("Motor speed, steer "+str(speed)+", "+str(steer))
if (steer < left_limit):
steer = left_limit
if (steer > right_limit):
steer = right_limit
# vel = speed * 1.27
vel = speed * 1.26
voc = 0
vic = 0
#roboclaw speed limit -127 to 127
if steer != 0: #if steering angle not zero, compute angles, wheel speed
angle = math.radians(abs(steer))
ric = self.d3 / math.sin(angle) #turn radius - inner corner
rm = ric * math.cos(angle) + self.d1 #body central radius
roc = math.sqrt((rm + self.d1)**2 + self.d3**2) #outer corner
rmo = rm + self.d4 #middle outer
rmi = rm - self.d4 #middle inner
phi = math.degrees(math.asin(self.d3 / roc))
if steer < 0:
phi = -phi
voc = roc / rmo #velocity corners & middle inner
vic = ric / rmo
vim = rmi / rmo
# SERVO MOTORS ARE COUNTER CLOCKWISE
# left turn
if steer < 0:
self.fl_motor.angle = bias - steer
self.fr_motor.angle = bias - phi
self.bl_motor.angle = bias + steer
self.br_motor.angle = bias + phi
self.turn_motor(0x80, vel, vic, vim)
self.turn_motor(0x81, vel, vic, voc)
self.turn_motor(0x82, vel, 1, voc)
#right turn
elif steer > 0:
self.fl_motor.angle = bias - phi
self.fr_motor.angle = bias - steer
self.bl_motor.angle = bias + phi
self.br_motor.angle = bias + steer
self.turn_motor(0x80, vel, voc, 1)
self.turn_motor(0x81, vel, voc, vic)
self.turn_motor(0x82, vel, vim, vic)
#straight ahead
else:
self.fl_motor.angle = bias
self.fr_motor.angle = bias
self.bl_motor.angle = bias
self.br_motor.angle = bias
self.turn_motor(0x80, vel, 1, 1)
self.turn_motor(0x81, vel, 1, 1)
self.turn_motor(0x82, vel, 1, 1)
class Argo:
def __init__(self):
print "Trying to connect to motors..."
self.motor_status = 0
try: # First step: connect to Roboclaw controller
self.port = "/dev/ttyACM0"
self.argo = Roboclaw(self.port, 115200)
self.argo.Open()
self.address = 0x80 # Roboclaw address
self.version = self.argo.ReadVersion(
self.address
) # Test connection by getting the Roboclaw version
except:
print "Unable to connect to Roboclaw port: ", self.port, "\nCheck your port and setup then try again.\nExiting..."
self.motor_status = 1
return
# Follow through with setup if Roboclaw connected successfully
print "Roboclaw detected! Version:", self.version[1]
print "Setting up..."
# Set up publishers and subscribers
self.cmd_sub = rospy.Subscriber("/argo/wheel_speeds", Twist,
self.get_wheel_speeds)
self.encoder = rospy.Publisher("/argo/encoders", Encoder, queue_size=5)
# Set class variables
self.radius = 0.0728 # Wheel radius (m)
self.distance = 0.372 # Distance between wheels (m)
self.max_speed = 13000 # Global max speed (in QPPS)
self.session_max = 13000 # Max speed for current session (in QPPS)
self.rev_counts = 3200 # Encoder clicks per rotation
self.circ = .4574 # Wheel circumference (m)
self.counts_per_m = int(self.rev_counts /
self.circ) # Encoder counts per meter
self.conv = self.rev_counts / (2 * pi) # Wheel speed conversion factor
self.Lref = 0 # Left wheel reference speed
self.Rref = 0 # Right wheel reference speed
self.Lprev_err = 0 # Previous error value for left wheel
self.Rprev_err = 0 # Previous error value for right wheel
self.Kp = .004 # Proportional gain
self.Kd = .001 # Derivative gain
self.Ki = .0004 # Integral gain
self.LEint = 0 # Left wheel integral gain
self.REint = 0 # Right wheel integral gain
print "Setup complete, let's roll homie ;)\n\n"
def reset_controller(self):
self.LEint = 0
self.REint = 0
self.Lprev_err = 0
self.Rprev_err = 0
def pd_control(self, Lspeed, Rspeed):
# Controller outputs a percent effort (0 - 100) which will be applied to the reference motor speeds
feedback = self.read_encoders()
M1 = feedback.speedM1
M2 = feedback.speedM2
# Calculate current speed error for both motors
Lerror = Lspeed - M2
Rerror = Rspeed - M1
# Calculate derivative error
Ldot = Lerror - self.Lprev_err
Rdot = Rerror - self.Rprev_err
# Calculate integral error
self.LEint += Lerror
self.REint += Rerror
# Compute effort
Lu = self.Kp * Lerror + self.Kd * Ldot + self.Ki * self.LEint
Ru = self.Kp * Rerror + self.Kd * Rdot + self.Ki * self.REint
# Saturate efforts if it is over +/-100%
if Lu > 100.0:
Lu = 100.0
elif Lu < -100.0:
Lu = -100
if Ru > 100.0:
Ru = 100.0
elif Ru < -100.0:
Ru = -100.0
# Set new L and R speeds
Lspeed = int((Lu / 100) * self.session_max)
Rspeed = int((Ru / 100) * self.session_max)
self.Rprev_err = Rerror
self.Lprev_err = Lerror
return (Lspeed, Rspeed)
def move(self, Lspeed, Rspeed):
if Lspeed == 0 and Rspeed == 0:
self.argo.SpeedM1(self.address, 0)
self.argo.SpeedM2(self.address, 0)
return
(Lspeed, Rspeed) = self.pd_control(Lspeed, Rspeed)
self.argo.SpeedM1(self.address, Rspeed)
self.argo.SpeedM2(self.address, Lspeed)
def force_speed(self, Lspeed, Rspeed):
self.argo.SpeedM1(self.address, Rspeed)
self.argo.SpeedM2(self.address, Lspeed)
def get_velocity(self, vx, vy, ax, ay):
v = sqrt((vx * vx) + (vy * vy)) # Linear velocity
# if vx < 0:
# v = -v
w = (vy * ax - vx * ay) / ((vx * vx) + (vy * vy)) # Angular velocity
return (v, w)
def get_wheel_speeds(self, data):
r = self.radius
d = self.distance
v = data.linear.x
w = data.angular.z
# Calculate left/right wheel speeds and round to nearest integer value
Ul = (v - w * d) / r
Ur = (v + w * d) / r
# Convert to QPPS
Rspeed = int(round(Ur * self.conv))
Lspeed = int(round(Ul * self.conv))
if Rspeed > 0:
Rspeed = Rspeed + 80
elif Rspeed < 0:
Rspeed = Rspeed - 20
self.move(Lspeed, Rspeed)
def reset_encoders(self):
self.argo.ResetEncoders(self.address)
def read_encoders(self):
mov = Encoder()
t = rospy.Time.now()
# Get encoder values from Roboclaw
enc2 = self.argo.ReadEncM2(self.address)
enc1 = self.argo.ReadEncM1(self.address)
# Get motor speeds
sp1 = self.argo.ReadSpeedM1(self.address)
sp2 = self.argo.ReadSpeedM2(self.address)
# Extract encoder ticks and motor speeds, and publish to /argo/encoders topic
mov.stamp.secs = t.secs
mov.stamp.nsecs = t.nsecs
mov.encoderM1 = enc1[1]
mov.encoderM2 = enc2[1]
mov.speedM1 = sp1[1]
mov.speedM2 = sp2[1]
return mov
def check_battery(self):
battery = self.argo.ReadMainBatteryVoltage(self.address)
return battery[1]
