class MotionController:
'''
MotionController class. Takes a DriveChain object in
its constructor, which
'''
def __init__(self, drivechain, single_mode=False, path=""):
self.path = path
self.single_mode = single_mode
self.mover = Mover()
self.now = None
# load up DriveChains
self.load_drivechains()
self.active_drivechain = None
self.set_active_drivechain(5)
# set up ROS publishers, subscribers and messages
rospy.Subscriber('/cmd_vel', Twist, self.move, queue_size=1)
print("MotionController loaded")
self.update_pwms()
# this is the only part which (sadly) knows about TwoWheelDriveChain
def load_drivechains(self):
try:
settings = np.loadtxt(self.path + "drivechain_settings", ndmin=2)
drivechain_list = [
TwoWheelDriveChain(*dc) for dc in settings.tolist()
]
if len(drivechain_list) < 10:
drivechain_list.extend([
TwoWheelDriveChain()
for i in range(10 - len(drivechain_list))
])
print("Rock and roll!! DriveChain's loaded :D")
except:
drivechain_list = [
TwoWheelDriveChain(0, 0),
TwoWheelDriveChain(-LINE, -0.5 * LINE, 28, 2),
TwoWheelDriveChain(-LINE, -LINE, 10, 10),
TwoWheelDriveChain(-0.5 * LINE, -LINE, 2, 20),
TwoWheelDriveChain(-TURN * 0.5, TURN * 0.5, 15, 13),
TwoWheelDriveChain(0, 0),
TwoWheelDriveChain(TURN * 0.5, -TURN * 0.5, 14, 13),
TwoWheelDriveChain(0.5 * LINE, LINE, 3, 22),
TwoWheelDriveChain(LINE, LINE, 10, 10),
TwoWheelDriveChain(LINE, 0.5 * LINE, 24, 1)
]
print("No file found in:", self.path,
"Default PID settings loaded - will learn from scratch")
return drivechain_list
def save_settings(self):
settings = np.array([dc.get_settings() for dc in self.drivechain_list])
np.savetxt(self.path + "pid_settings",
settings,
fmt="%1.3f",
header="vel_l(m/s), vel_r(m/s), pwm_l, pwm_r")
def update_pwms(self):
rate = rospy.Rate(10)
last_save = 0
while not rospy.is_shutdown():
now = rospy.get_rostime()
dt = ((now - self.now).to_sec() if self.now else 0)
self.active_drivechain.ticks(
dt) # call our DriveChain to update itself
last_save += 1
if last_save > 600:
self.save_settings()
self.odom_publisher.save_weights()
last_save = 0
self.now = now
rate.sleep()
def set_active_drivechain(self, index=5):
self.active_drivechain = self.drivechain_list[index]
def move(self, twist):
if self.single_mode:
idx = 0
self.drivechain_list[0].reset_targets(*twist_to_wheel_vel(twist))
else:
idx = twist_to_index(twist)
print("Setting drive mode to ", idx)
self.set_active_drivechain(idx)
is_fwd_left, is_fwd_right = self.active_drivechain.get_motor_directions(
)
self.mover.power_motors(is_fwd_left, is_fwd_right)
class MotionController:
def __init__(self, path="", single_mode=False):
self.path = path
self.single_mode = single_mode
self.mover = Mover()
self.wheel_encoder = WheelEncoder()
self.odom_publisher = OdometryPublisher(1 / (3.14 * BASE_WIDTH), path)
self.now = None
self.pid_list = self.load_settings()
self.active_controller = None
self.set_active_controller(5)
# initializing ROS publishers, subscribers and messages
self.prev_pwms = (0, 0)
rospy.Subscriber('/cmd_vel', Twist, self.move, queue_size=1)
rospy.Subscriber("/PID_settings",
PID_settings,
self.set_k,
queue_size=1)
self.pub_l = rospy.Publisher("PID_l", PID_out, queue_size=10)
self.pub_r = rospy.Publisher("PID_r", PID_out, queue_size=10)
self.data_msg_l = PID_out(wheel="left")
self.data_msg_r = PID_out(wheel="right")
print("PID Master loaded")
self.update_pwms()
@staticmethod
def set_k(settings):
print("PID settings changed from ", PIDModel.kp, PIDModel.ki,
PIDModel.kd, " to ", settings.Kp, settings.Ki, settings.Kd)
PIDModel.kp = settings.Kp
PIDModel.ki = settings.Ki
PIDModel.kd = settings.Kd
def save_settings(self):
settings = np.array([pid.get_settings() for pid in self.pid_list])
np.savetxt(self.path + "pid_settings",
settings,
fmt="%1.3f",
header="vel_l(m/s), vel_r(m/s), pwm_l, pwm_r")
def load_settings(self):
try:
settings = np.loadtxt(self.path + "pid_settings", ndmin=2)
pid_list = [PIDController(*pid) for pid in settings.tolist()]
if len(pid_list) < 10:
pid_list.extend(
[PIDController(0, 0) for i in range(10 - len(pid_list))])
print("PID settings loaded")
except:
pid_list = [
PIDController(0, 0),
PIDController(-LINE, -0.65 * LINE, 28, 2),
PIDController(-LINE, -LINE, 10, 10),
PIDController(-0.65 * LINE, -LINE, 2, 20),
PIDController(-TURN * 0.5, TURN * 0.5, 15, 13),
PIDController(0, 0),
PIDController(TURN * 0.5, -TURN * 0.5, 14, 13),
PIDController(0.5 * LINE, LINE, 3, 22),
PIDController(LINE, LINE, 10, 10),
PIDController(LINE, 0.5 * LINE, 24, 1)
]
print("No file found in:", self.path,
"Default PID settings loaded - will learn from scratch")
return pid_list
def update_pwms(self):
rate = rospy.Rate(10)
last_save = 0
while not rospy.is_shutdown():
self.ticks()
last_save += 1
if last_save > 600:
self.save_settings()
self.odom_publisher.save_weights()
last_save = 0
rate.sleep()
def set_active_controller(self, index=5):
self.active_controller = self.pid_list[index]
def get_motor_directions(self):
return self.active_controller.is_left_fwd, self.active_controller.is_right_fwd
def ticks(self):
left_tick_speed, right_tick_speed = self.wheel_encoder.poll_tick_speeds(
)
left_speed, right_speed = self.signed_speed(
left_tick_speed / TICKS_PER_METER,
right_tick_speed / TICKS_PER_METER)
now = rospy.get_rostime()
dt = ((now - self.now).to_sec() if self.now else 0)
self.control(left_speed, right_speed, dt)
self.odom_publisher.publish_odom(left_speed, right_speed, dt, now)
self.now = now
def control(self, left_speed, right_speed, dt):
pwm_left, pwm_right = self.active_controller.control(
left_speed, right_speed, dt)
self.mover.set_duty_cycle(pwm_left, pwm_right)
self.publish_PID_data(left_speed, right_speed, pwm_left, pwm_right)
def publish_PID_data(self, left_speed, right_speed, pwm_left, pwm_right):
left_target, right_target, left_i, right_i = self.active_controller.get_settings(
)
# publishes previous pwm to match up with backward looking speed
update_PID_msg(self.data_msg_l, self.prev_pwms[0], left_target,
left_speed, 100)
update_PID_msg(self.data_msg_r, self.prev_pwms[1], right_target,
right_speed, 100)
self.prev_pwms = (pwm_left, pwm_right)
self.pub_l.publish(self.data_msg_l)
self.pub_r.publish(self.data_msg_r)
def move(self, twist):
if self.single_mode:
idx = 0
self.pid_list[0].reset_targets(*twist_to_wheel_vel(twist))
else:
idx = twist_to_index(twist)
print("Setting drive mode to ", idx)
self.set_active_controller(idx)
is_fwd_left, is_fwd_right = self.get_motor_directions()
self.mover.power_motors(is_fwd_left, is_fwd_right)
def signed_speed(self, left_speed, right_speed):
left_fwd, right_fwd = self.get_motor_directions()
return (left_speed if left_fwd else -left_speed), \
(right_speed if right_fwd else -right_speed)