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
else:
print "failed",
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed "
rc.Open()
address = 0x80
version = rc.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
while (1):
rc.SpeedAccelM1(address, 12000, 12000)
rc.SpeedAccelM2(address, 12000, -12000)
for i in range(0, 200):
displayspeed()
time.sleep(0.01)
rc.SpeedAccelM1(address, 12000, -12000)
rc.SpeedAccelM2(address, 12000, 12000)
for i in range(0, 200):
displayspeed()
time.sleep(0.01)
class SerialDriver(object):
'''
Class for serial UART interface to the RoboClaw Motor Controllers
'''
def __init__(self):
rospy.loginfo("Initilizing the motor controllers..")
self.e_stop = 1
self.reg_enabled = 0
self.temp = 0
self.error = 0
self.voltage = 0
self.currents = [0,0]
self._thread_lock = False
self.prev_enc_ts = None
self.prev_tick = [None, None]
self.start_time = datetime.now()
self.left_currents = []
self.right_currents = []
self.max_left_integrator = 0
self.max_right_integrator = 0
self.left_integrator = 0
self.right_integrator = 0
self.motor_lockout = 0
self._cmd_buf_flag = 0
self._l_vel_cmd_buf = 0
self._r_vel_cmd_buf = 0
self.battery_percent = 0
self.shutdown_warning = False
self.shutdown_flag = False
self.rate = rospy.get_param("/puffer/rate")
self.delta_t = 2.0/self.rate
self.rc = Roboclaw(
rospy.get_param("/motor_controllers/serial_device_name"),
rospy.get_param("/motor_controllers/baud_rate")
)
self.rc.Open()
self._set_operating_params()
self._get_version()
self.set_estop(0) #Clears E-stop pin
self.enable_12v_reg(1) #Disables 12V Regulator
self.kill_motors() #Start the motors not moving
#Private Methods
def _get_version(self):
'''
Version check for communication verification to the motor controllers
returns ther version number if sucessful, and 0 if not
'''
version = self.rc.ReadVersion(self.address)
if version != 0:
rospy.loginfo("[Motor __init__ ] Sucessfully connected to all Motor Controllers!")
rospy.loginfo( version )
rospy.set_param("/motor_controllers/firmware_version",version)
else:
raise Exception("Unable to establish connection to Motor controllers")
return version
def _set_operating_params(self):
'''
Sets all the operating parameters for control of PUFFER, Pinouts, and
safety parameters.
returns None
'''
#GPIO settings for E-stop and Regulator Enable pins
self.e_stop_pin = rospy.get_param("/gpio_pins/e_stop",1)
self.reg_en_pin = rospy.get_param("/gpio_pins/reg_en",1)
try:
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(self.e_stop_pin, GPIO.OUT)
GPIO.setup(self.reg_en_pin, GPIO.OUT)
except:
pass
#Threadlock used for serial comm to RoboClaw
self.thread_timeout = rospy.get_param("/threadlock/timeout")
#Motor operating parameters
self.wheel_d = rospy.get_param("/wheels/diameter")
self.enc_cpr = rospy.get_param("/wheels/encoder/cts_per_rev")
factor = rospy.get_param("/wheels/encoder/factor")
stage_1 = rospy.get_param("/wheels/gearbox/stage1")
stage_2 = rospy.get_param("/wheels/gearbox/stage2")
stage_3 = rospy.get_param("/wheels/gearbox/stage3")
self.accel_const = rospy.get_param("/puffer/accel_const")
self.max_vel_per_s = rospy.get_param("/puffer/max_vel_per_s")
self.tick_per_rev = int(self.enc_cpr * factor * stage_1 * stage_2 * stage_3)
rospy.loginfo(self.tick_per_rev)
rospy.set_param("tick_per_rev", self.tick_per_rev)
self.max_cts_per_s = int((self.max_vel_per_s * self.tick_per_rev)/(math.pi * self.wheel_d))
self.max_accel = int(self.max_cts_per_s * self.accel_const)
rospy.set_param("max_cts_per_s", self.max_cts_per_s)
rospy.set_param("max_accel", self.max_accel)
self.address = rospy.get_param("/motor_controllers/address", 0x80)
self.rc.SetMainVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/main/low", 12.0) * 10),int(rospy.get_param("motor_controllers/battery/main/high", 18.0 ) * 10))
self.rc.SetLogicVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/logic/low") * 10),int(rospy.get_param("motor_controllers/battery/logic/high") * 10))
self.max_current = rospy.get_param("/motor_controllers/current/max_amps")
self.motor_lockout_time = rospy.get_param("/puffer/motor_lockout_time")
m1p = rospy.get_param("/motor_controllers/m1/p")
m1i = rospy.get_param("/motor_controllers/m1/i")
m1d = rospy.get_param("/motor_controllers/m1/d")
m1qpps = rospy.get_param("/motor_controllers/m1/qpps")
m2p = rospy.get_param("/motor_controllers/m2/p")
m2i = rospy.get_param("/motor_controllers/m2/i")
m2d = rospy.get_param("/motor_controllers/m2/d")
m2qpps = rospy.get_param("/motor_controllers/m2/qpps")
self.battery_max_time = rospy.get_param("/battery/max_time")
self.battery_max_volts = rospy.get_param("/battery/max_volts")
self.battery_coef_a = rospy.get_param("/battery/a")
self.battery_coef_b = rospy.get_param("/battery/b")
self.battery_coef_c = rospy.get_param("/battery/c")
self.battery_warning = rospy.get_param("/battery/warning_percent")
self.battery_shutdown = rospy.get_param("/battery/shutdown_percent")
self.rc.SetM1VelocityPID(self.address,m1p, m1i, m1d, m1qpps)
self.rc.SetM2VelocityPID(self.address,m2p, m2i, m2d, m2qpps)
self.rc.WriteNVM(self.address)
time.sleep(0.001)
self.rc.ReadNVM(self.address)
def _lock_thread(self,lock):
'''
Checks the thread lock and then grabs it when it frees up
no return value
'''
if (lock):
start = time.time()
while self._thread_lock:
#rospy.loginfo("in threadlock")
if time.time() - start > self.thread_timeout:
raise Exception("Thread lock timeout")
time.sleep(0.001)
self._thread_lock = True
else:
self._thread_lock = False
def _get_Temp(self):
'''
Gets the temperature of the motor controllers
return:
list [2] (int): Temperature values * 10 degrees C
'''
self._lock_thread(1)
self.temp = self.rc.ReadTemp(self.address)[1]
self._lock_thread(0)
self.temp = int(self.temp*100)/1000.0
return self.temp
def _get_Voltage(self):
'''
Gets the voltage of the motor controllers
return:
voltage (int) : Voltage values * 10 volts
'''
self._lock_thread(1)
v = self.rc.ReadMainBatteryVoltage(self.address)[1]
v = int(v*100)/1000.0
if v != 0:
self.voltage = v + 0.4 #accounts for the voltage drop in the diode
self._lock_thread(0)
return v
def _get_Currents(self):
'''
Gets the current of the motor controllers
return:
list [2] (int): Current values * 100 Amps
'''
self._lock_thread(1)
cur = self.rc.ReadCurrents(self.address)
self._lock_thread(0)
r_current = int(cur[1])/100.0
l_current = int(cur[2])/100.0
self.currents = [l_current,r_current]
self.left_currents.insert(0, l_current)
self.right_currents.insert(0, r_current)
if (len(self.left_currents) > self.rate/2):
del self.left_currents[-1]
del self.right_currents[-1]
left_power = 0
right_power = 0
for i in range(len(self.left_currents)):
left_power += math.pow(self.left_currents[i],2) * self.delta_t
right_power += math.pow(self.right_currents[i],2) * self.delta_t
if (left_power >= math.pow(self.max_current,2) or right_power >= math.pow(self.max_current,2)):
rospy.loginfo("Motor power exceeded max allowed! Disabling motors for %d seconds" %(self.motor_lockout_time))
self.motor_lockout = 1
self.set_estop(1)
self.lockout_timestamp = time.time()
if (self.motor_lockout and (time.time() - self.lockout_timestamp >= self.motor_lockout_time)):
rospy.loginfo("Re-enabling the motors from timeout lock")
self.motor_lockout = 0
self.set_estop(0)
self.left_integrator = left_power
self.right_integrator = right_power
self.max_left_integrator = max(self.max_left_integrator, self.left_integrator)
self.max_right_integrator = max(self.max_right_integrator, self.right_integrator)
#print self.left_integrator, self.right_integrator
return self.currents
def _get_Errors(self):
'''
Gets the error status of the motor controllers
return:
error (int): Error code for motor controller
'''
self._lock_thread(1)
self.error = self.rc.ReadError(self.address)[1]
self._lock_thread(0)
return self.error
def _get_Encs(self):
'''
Gets the encoder values of the motor controllers
return:
list [2] (int): Speed of motors in radians/s, computed at each delta T
'''
self._lock_thread(1)
r_enc = self.rc.ReadEncM1(self.address)[1]
l_enc = self.rc.ReadEncM2(self.address)[1]
dt = datetime.now()
self._lock_thread(0)
if self.prev_tick == [None, None]:
l_vel_rad_s, r_vel_rad_s = 0,0
self.timestamp = 0
else:
delta_t = ((dt-self.prev_enc_ts).microseconds)/1000000.0
self.timestamp = int(self.timestamp + (delta_t * 100000))
l_vel = (l_enc - self.prev_tick[0])/(delta_t)
r_vel = (r_enc - self.prev_tick[1])/(delta_t)
l_vel_rad_s = l_vel * (2 * math.pi/self.tick_per_rev)
r_vel_rad_s = r_vel * (2 * math.pi/self.tick_per_rev)
l_vel_rad_s = int(l_vel_rad_s * 1000)/1000.0
r_vel_rad_s = int(r_vel_rad_s * 1000)/1000.0
self.prev_enc_ts = dt
self.prev_tick = [l_enc, r_enc]
self.enc = [l_vel_rad_s, r_vel_rad_s]
return l_vel_rad_s, r_vel_rad_s
def _send_motor_cmds(self):
l_vel = self._l_vel_cmd_buf
r_vel = self._r_vel_cmd_buf
l_vel *= (self.tick_per_rev/(2*math.pi))
r_vel *= (self.tick_per_rev/(2*math.pi))
l_vel = max(min(l_vel,self.max_cts_per_s), -self.max_cts_per_s)
r_vel = max(min(r_vel,self.max_cts_per_s), -self.max_cts_per_s)
if (( abs(l_vel) <= self.max_cts_per_s) and (abs(r_vel) <= self.max_cts_per_s)):
if not self.motor_lockout:
self._lock_thread(1)
self.rc.SpeedAccelM1(self.address, self.max_accel, int(r_vel))
self.rc.SpeedAccelM2(self.address, self.max_accel, int(l_vel))
self._lock_thread(0)
else:
rospy.loginfo( "values not in accepted range" )
self.send_motor_cmds(0,0)
# Public Methods
def set_estop(self,val):
'''
Sets the E-stop pin to stop Motor control movement until cleared
Parameters:
val (int):
0 - Clears E-stop
1 - Sets E-stop, disabling motor movement
no return value
'''
if (val != self.e_stop):
if val:
rospy.loginfo( "Enabling the E-stop")
GPIO.output(self.e_stop_pin, 1)
self.e_stop = 1
else:
rospy.loginfo( "Clearing the E-stop")
GPIO.output(self.e_stop_pin, 0)
self.e_stop = 0
def battery_state_esimator(self):
x = self.voltage
y = math.pow(x,2) * self.battery_coef_a + self.battery_coef_b * x + self.battery_coef_c
self.battery_percent = (100 * y/float(self.battery_max_time))
if (self.battery_percent <= self.battery_warning):
self.shutdown_warning = True
else:
self.shutdown_warning = False
if (self.battery_percent <= self.battery_shutdown):
self.shutdown_flag = True
self.set_estop(1)
def update_cmd_buf(self, l_vel, r_vel):
'''
Updates the command buffers and sets the flag that new command has been received
Parameters:
l_vel (int): [-0.833, 0.833] units of [rad/s]
r_vel (int): [-0.833, 0.833] units of [rad/s]
no return value
'''
self._l_vel_cmd_buf = l_vel
self._r_vel_cmd_buf = r_vel
self._cmd_buf_flag = 1
def kill_motors(self):
'''
Stops all motors on the assembly
'''
self._lock_thread(1)
self.rc.ForwardM1(self.address, 0)
self.rc.ForwardM2(self.address, 0)
self._lock_thread(0)
def loop(self, counter):
'''
Gets the data from the motor controllers to populate as class variables
all data function classes are private because of threadlocks
Downsample non-critical values to keep serial comm line free
Parameters:
counter (int):
no return value
'''
if (self._cmd_buf_flag):
self._send_motor_cmds()
self._cmd_buf_flag = 0
self._get_Encs()
if not counter % 2:
self._get_Currents()
if not counter % 5:
self._get_Errors()
if not counter % 20:
self._get_Temp()
self._get_Voltage()
self.battery_state_esimator()
def enable_12v_reg(self, en):
'''
Turns on/off the 12V regulator GPIO pin
Parameters:
en (int):
0: Disabled
1: Enabled
'''
try:
if (en != self.reg_enabled):
if en:
rospy.loginfo("Enabling the 12V Regulator")
GPIO.output(self.reg_en_pin, 1)
self.reg_enabled = 1
else:
rospy.loginfo("Disabling the 12V Regulator")
GPIO.output(self.reg_en_pin, 0)
self.reg_enabled = 0
except:
pass
def cleanup(self):
'''
Cleans up the motor controller node, stopping motors and killing all threads
no return value
'''
rospy.loginfo("Cleaning up the motor_controller node..")
self._thread_lock = False
self.kill_motors()
self.set_estop(1)
try:
GPIO.cleanup()
except:
pass
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 channel == "M1":
return self.rc.SpeedAccelM1(address, accel, target_qpps)
elif channel == "M2":
return self.rc.SpeedAccelM2(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 Controller(object):
def __init__(self,
serial_port='/dev/ttyACM0',
baudrate=115200,
mode='PWM',
motors_connected={
"left": [],
"right": []
},
index=0):
self.name = "mtr_" + str(index)
self._serial_port = serial_port
self._baudrate = baudrate
self._address = 0x80
self._rc = Roboclaw(self._serial_port, self._baudrate)
self._rc.Open()
self.stop_service = rospy.Service(self.name + '_stop_motors', stop,
self.stop_srv)
self._mode = mode
self.current_speed = 0
if len(motors_connected["left"]) + len(motors_connected["right"]) > 2:
raise ValueError(
"You can only connect two motors to these controllers!")
self.motors_connected = {"left": [], "right": []}
self.motor_number = 0
for side in motors_connected.keys():
for motor in motors_connected[side]:
if motor:
if self.motor_number == 0:
self.motors_connected[side].append(
Motor(name=str(motor), side=side))
self.motor_number += 1
elif self.motor_number == 1:
self.motors_connected[side].append(
Motor(name=str(motor), side=side))
else:
raise ValueError('Too many motors, numer is: ' +
str(self.motor_number))
## ROS Interfaces:
self.r = rospy.Rate(10)
self.encoder_publisher = rospy.Publisher(self.name + '_enc_val',
encoder_values,
queue_size=1)
self.speed_publisher = rospy.Publisher(self.name + '_speed_val',
speed_values,
queue_size=1)
self.mode_service = rospy.Service(self.name + "_mode_service",
set_controller_mode,
self.set_controller_mode)
def get_motors_connected(self, side=None):
if side == None or side not in ["left", "right"]:
return self.motors_connected
else:
return self.motors_connected[side]
def get_controller_mode(self):
return self._mode
def set_controller_mode(self, data):
if data.controller_mode not in ['SPEED', 'PWM']:
rospy.logerr_once(
'ERROR: Given controller mode is not supported, either enter "SPEED" or "PWM"'
+ ', I got: ' + str(data.controller_mode))
return set_controller_modeResponse("Cannot change mode to: " +
str(data.controller_mode))
self._mode = data.controller_mode
return set_controller_modeResponse(
'Successfully changed the controller mode to {0}'.format(
self._mode))
## Mapping of RC Control methods:
def set_speed(self, side, speed):
#print "I got: "+ str(side)+ " and " + str(speed)
# if speed == self.current_speed:
# return
if self.motors_connected[side] == []:
return
for motor in self.motors_connected[side]:
if motor.speed == speed:
return
acceleration = 2000
if abs(speed) > 10000 and self._mode == "SPEED":
speed = 10000 * numpy.sign(speed)
if abs(speed) > 16 and self._mode == "PWM":
speed = 16 * numpy.sign(speed)
print "speed1:" + str(speed)
if motor.isM1() == True:
if self._mode == "SPEED":
print "Sending to M1:" + str(speed)
try:
self._rc.SpeedAccelM1(self._address, acceleration,
speed)
except:
rospy.loginfo("Cannot communicate with controllers")
elif self._mode == "PWM":
pwm = 64 + speed
print "sending to M1: " + str(pwm)
try:
self._rc.ForwardBackwardM1(self._address, pwm)
except:
rospy.loginfo("Cannot communicate with controller")
print "speed2:" + str(speed)
if motor.isM2() == True:
if self._mode == "SPEED":
print "Sending to M2:" + str(speed)
try:
self._rc.SpeedAccelM2(self._address, acceleration,
speed)
except:
rospy.loginfo("Cannot communicate with controllers")
elif self._mode == "PWM":
pwm = 64 + speed
print "sending to M2: " + str(pwm)
try:
self._rc.ForwardBackwardM2(self._address, pwm)
except:
rospy.loginfo("Cannot communicate with controller")
motor.set_speed(speed)
## Stop Method
def stop(self):
rospy.loginfo(self.name + ': Stopping both motors')
if self._mode == "SPEED":
try:
self._rc.SpeedM1M2(self._address, 0, 0)
except:
rospy.logerr('Could not contact controller to stop motor!!!')
if self._mode == "PWM":
try:
self._rc.ForwardBackwardM1(self._address, 64)
self._rc.ForwardBackwardM2(self._address, 64)
except:
rospy.logerr('Could not contact controller to stop motor!!!')
return
## Stop service method
def stop_srv(self, data):
if data.request:
self.stop()
return stopResponse('Motors are stopped')
return stopResponse(
"Don't call stop service with false, who does that?")
def run_publishers(self):
rc = self._rc
address = self._address
encoder_message = encoder_values()
speed_message = speed_values()
enc1 = rc.ReadEncM1(address)
enc2 = rc.ReadEncM2(address)
speed1 = rc.ReadSpeedM1(address)
speed2 = rc.ReadSpeedM2(address)
if (enc1[0] == 1):
encoder_message.enc1 = enc1[1]
else:
rospy.logerr(self.name + ': Could not read data from first motor')
if (enc2[0] == 1):
encoder_message.enc2 = enc2[1]
else:
rospy.logerr(self.name + ': Could not read data from second motor')
if (speed1[0]):
print speed1[1],
else:
rospy.logerr(self.name + ': Could not read data from encoder 2')
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed "
class Controller():
'''
Controller class where it contains the PID equation
'''
def __init__(self, configPath):
print 'reading config from path...' + configPath
parser = SafeConfigParser()
parser.read(configPath)
#Reading the configurations
self._P = parser.get('pid_coeff', 'P')
self._I = parser.get('pid_coeff', 'I')
self._D = parser.get('pid_coeff', 'D')
self._PR = parser.get('pid_coeff', 'PR')
self._PL = parser.get('pid_coeff', 'PL')
self._speed = parser.get('motion', 'speed')
self._accel = parser.get('motion', 'accel')
self._samplingRate = parser.get('pid_coeff', 'samplingRate')
self._port = parser.get('systems', 'port')
self._robot = Robot()
#setup the roboclaw here
self._rc = Roboclaw(self._port, 115200)
self._rc.Open()
#Obsolete
# def P():
# doc = "The P property."
# def fget(self):
# return self._P
# def fset(self, value):
# self._P = value
# def fdel(self):
# del self._P
# return locals()
# P = property(**P())
#
# def I():
# doc = "The I property."
# def fget(self):
# return self._I
# def fset(self, value):
# self._I = value
# def fdel(self):
# del self._I
# return locals()
# I = property(**I())
#
# def D():
# doc = "The D property."
# def fget(self):
# return self._D
# def fset(self, value):
# self._D = value
# def fdel(self):
# del self._D
# return locals()
# D = property(**D())
def _getCurDegree(self):
return self._robot.degree
def _getCorrection(self, toBeDegree):
#error Negative value == moving to the left
#error Positive value == moving to the right
error = toBeDegree - self._getCurDegree()
print 'Error: ' + str(error)
ctlSig = self._P * error #Simple P control Signal
print 'Control signal: ' + str(ctlSig)
return ctlSig
def _getLeftWheelSpeed(self, toBeDegree):
ctlSig = self._getCorrection(toBeDegree)
speed = self._speed - self._speed * ctlSig
return speed
def _getRightWheelSpeed(self, toBeDegree):
ctlSig = self._getCorrection(toBeDegree)
speed = (self._speed + self._speed * ctlSig) * -1
return speed
def _moveFW(self, speedL, speedR):
self._rc.SpeedAccelM1(constants.ADDRESS, self._accel, speedL)
self._rc.SpeedAccelM2(constants.ADDRESS, self._accel, speedR)
def stop(self):
self._rc.SpeedM1(constants.ADDRESS, 0)
self._rc.SpeedM2(constants.ADDRESS, 0)
def moveFWControlled(self, degree, seconds):
time = 0
while (time < seconds):
speedL = self._getLeftWheelSpeed(degree)
speedR = self._getRightWheelSpeed(degree)
self._moveFW(speedL, speedR)
sleep(self._samplingRate / 1000) #convert ms to secs
time = time + self._samplingRate / 1000
self.stop()
else:
print "failed "
rc.Open()
address = 0x80
version = rc.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
# rc.SpeedAccelDistanceM1(address,10000,2000,200,1);
# rc.SpeedAccelDistanceM2(address,10000,-2000,200,1);
rc.SpeedAccelM1(address, 0, 1500)
rc.SpeedAccelM2(address, 0, 1500)
time.sleep(1)
rc.SpeedAccelM1(address, 10000, 0)
rc.SpeedAccelM2(address, 10000, -0)
# for i in range(0,200):
# displayspeed()
# time.sleep(0.01)
#
# rc.SpeedAccelM1(address,12000,12000)
# rc.SpeedAccelM2(address,12000,12000)
# for i in range(0,200):
# displayspeed()
# time.sleep(0.01)
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)
print "failed " ,
print "Speed1:",
if(speed1[0]):
print speed1[1],
else:
print "failed",
print("Speed2:"),
if(speed2[0]):
print speed2[1]
else:
print "failed "
rc.Open()
address = 0x80
version = rc.ReadVersion(address)
if version[0]==False:
print "GETVERSION Failed"
else:
print repr(version[1])
i = 1
while(1):
rc.SpeedAccelM1(address,12000,i)
rc.SpeedAccelM2(address,12000,-i)
displayspeed()
i = i + 1
print(i)
