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)