示例#1
0
def test_pid_wrapper():
    """
    """
    from pid import PID
    history_length = 5
    setpoint = 100.0
    Kp = 1.5
    Ki = 0.0
    Kd = 0.0

    pid = PID(history_length, setpoint, Kp, Ki, Kd)

    process_value = 90.0
    delta_time = 1.0
    control = pid.control(process_value, delta_time)
    print("control = {0}".format(control))
示例#2
0
class Camera():
    def __init__(self, S, dir_queue, cam_data, getCoords_event, navigate_event,
                 END_event):
        self.font = cv2.FONT_HERSHEY_SIMPLEX
        self.cam_data = cam_data

        # for calibration
        self.db = 0
        self.chbEdgeLength = CHB_SIDE

        self.start = True
        self.tstart = 0
        self.calib = False

        # termination criteria
        self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
                         30, 0.001)

        # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)
        self.objp = np.zeros((6 * 9, 3), np.float32)
        self.objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1,
                                                        2) * self.chbEdgeLength

        # arrays to store object points and image points from all the images
        self.objpoints = []  # 3d point in real world space
        self.imgpoints = []  # 2d points in image plane.

        # for loading camera matrices
        self.not_loaded = True

        # drone navigation
        self.speed = S
        self.amplify = 10
        self.dir_queue = dir_queue
        self.getCoords_event = getCoords_event
        self.navigate_event = navigate_event
        self.END_event = END_event
        self.resetCoords = False
        self.t_lost = 0.
        self.t_inPos = 0.
        self.last_marker_pos = 1.
        self.beenThere = []
        self.TargetID = 1
        self.findNew = False
        self.MarkerTarget = TargetDefine()

        # controller
        self.yaw_pid = PID(0.1, 0.00001, 0.001)
        self.v_pid = PID(0.5, 0.00001, 0.0001)
        self.vz_pid = PID(0.8, 0.00001, 0.0001)
        self.TargetPos = np.array([[0., 0., 1., 0.]])

        # for aruco markers
        self.markerEdge = MARKER_SIDE  # ArUco marker edge length in meters
        self.seenMarkers = Markers(MARKER_SIDE, self.getCoords_event)
        self.getFirst = True

    # Calibrate camera
    def calibrator(self, frame):
        if self.calib == False:
            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            # find the chessboard corners
            ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)

        if self.db < 20:
            # if found, add object points, image points (after refining them)
            if ret == True and self.start:
                self.start = False
                self.tstart = time.time()
                self.objpoints.append(self.objp)

                corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
                                            self.criteria)
                self.imgpoints.append(corners2)

                # draw and display the corners
                frame = cv2.drawChessboardCorners(frame, (9, 6), corners2, ret)
                self.db = self.db + 1
            elif ret == True and time.time() - self.tstart > 0.5:
                self.tstart = time.time()
                self.objpoints.append(self.objp)

                corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
                                            self.criteria)
                self.imgpoints.append(corners2)

                # draw and display the corners
                frame = cv2.drawChessboardCorners(frame, (9, 6), corners2, ret)
                self.db = self.db + 1
            else:
                if ret == True:
                    corners2 = cv2.cornerSubPix(gray, corners, (11, 11),
                                                (-1, -1), self.criteria)
                    frame = cv2.drawChessboardCorners(frame, (9, 6), corners2,
                                                      ret)
                else:
                    cv2.putText(frame, "Please show chessboard.", (0, 64),
                                self.font, 1, (0, 0, 255), 2, cv2.LINE_AA)
        else:
            if self.calib == False:  # save the camera matrices first
                self.calib = True
                ret, self.mtx, self.dist, self.rvecs, self.tvecs = cv2.calibrateCamera(
                    self.objpoints, self.imgpoints, gray.shape[::-1], None,
                    None)
                h, w = frame.shape[:2]
                self.newcameramtx, self.roi = cv2.getOptimalNewCameraMatrix(
                    self.mtx, self.dist, (w, h), 1, (w, h))
                np.savez("camcalib",
                         ret=ret,
                         mtx=self.mtx,
                         dist=self.dist,
                         rvecs=self.rvecs,
                         tvecs=self.tvecs)

            # undistort
            frame = cv2.undistort(frame, self.mtx, self.dist, None,
                                  self.newcameramtx)
            # crop the image
            x, y, w, h = self.roi
            frame = frame[y:y + h, x:x + w]
            cv2.putText(frame, "Camera calibrated.", (0, 64), self.font, 1,
                        (0, 255, 0), 2, cv2.LINE_AA)

        return frame

    # Write battery percentage on frame
    def writeBattery(self, frame, bat):
        w = frame.shape[1]
        h = frame.shape[0]
        if bat < 25:
            cv2.putText(frame, "Battery: " + str(bat), (w - 170, h - 10),
                        self.font, 0.8, (0, 0, 255), 2, cv2.LINE_AA)
        elif bat < 50:
            cv2.putText(frame, "Battery: " + str(bat), (w - 170, h - 10),
                        self.font, 0.8, (0, 255, 255), 2, cv2.LINE_AA)
        else:
            cv2.putText(frame, "Battery: " + str(bat), (w - 170, h - 10),
                        self.font, 0.8, (0, 255, 0), 2, cv2.LINE_AA)

        return frame

    # Detect ArUco markers
    def aruco(self, frame, CoordGet, CoordReset, angles_tof):
        # Get the calibrated camera matrices
        if self.not_loaded:
            with np.load(self.cam_data) as X:
                self.mtx = X['mtx']
                self.dist = X['dist']
            self.not_loaded = False

        h, w = frame.shape[:2]
        newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
            self.mtx, self.dist, (w, h), 1, (w, h))

        # Undistort
        frame = cv2.undistort(frame, self.mtx, self.dist, None, newcameramtx)

        # Crop image
        x, y, w, h = roi
        frame = frame[y:y + h, x:x + w]
        #origFrame=np.copy(frame)

        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_7X7_100)
        parameters = cv2.aruco.DetectorParameters_create()

        # detecting markers: get corners and IDs
        corners, ids, _ = cv2.aruco.detectMarkers(gray,
                                                  aruco_dict,
                                                  parameters=parameters)

        # list for all currently seen IDs
        id_list = []

        if CoordReset or self.resetCoords:
            print("Coordinates reset")
            self.resetCoords = False
            self.seenMarkers.nullCoords()

        if self.getCoords_event.is_set():
            CoordGet = True

        if np.all(ids != None):
            ### IDs found
            # pose estimation with marker edge length
            rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
                corners, self.markerEdge, self.mtx, self.dist)

            for i in range(0, ids.size):
                cv2.aruco.drawAxis(frame, self.mtx, self.dist, rvecs[i],
                                   tvecs[i], 0.1)  # Draw axis

                id_list.append(ids[i][0])

            # draw square around the markers
            cv2.aruco.drawDetectedMarkers(frame, corners)

            # only if navigation is turned on
            if self.navigate_event.is_set():
                if not self.findNew:
                    # draw yellow line to target
                    frame = self.drawCenter(frame, id_list, corners, w, h)
                    # marker navigation
                    self.navigateToMarker(id_list, tvecs, rvecs)
                else:
                    # if no new markers can be seen, rotate
                    if self.TargetPos[0][3] <= 0 and self.TargetPos[0][3] != -5:
                        self.dir_queue.put([0, 0, 0, self.speed * 2])
                    else:
                        self.dir_queue.put([0, 0, 0, -self.speed * 2])
                    # if new marker found, set as target
                    for ID in id_list:
                        if ID not in self.beenThere:
                            self.TargetID = ID
                            self.TargetPos = self.MarkerTarget.changeTarget(ID)
                            self.findNew = False
                            break

                if not self.getFirst:
                    # Reject markers, which have corners on the edges of the frame
                    id_list, tvecs, rvecs = self.filterCorners(
                        w, h, corners, id_list, tvecs, rvecs)

                    if len(id_list) > 0:
                        # from drone state queue
                        angles = np.array(
                            [[angles_tof[0], angles_tof[1], angles_tof[2]]])
                        tof = angles_tof[3]
                        # add new marker
                        self.seenMarkers.appendMarker(id_list, tvecs, rvecs,
                                                      angles, tof)
                        # calculate positions, if marker is already allowed
                        self.seenMarkers.getMov(id_list, tvecs, rvecs, angles)

                    # after the end, it should close, back to first
                    if not self.getCoords_event.is_set():
                        self.getFirst = True
        else:
            ### No IDs found
            cv2.putText(frame, "No Ids", (0, 64), self.font, 1, (0, 0, 255), 2,
                        cv2.LINE_AA)
            if timer() - self.t_lost > 2:
                if self.last_marker_pos >= 0:
                    self.dir_queue.put([0, 0, 0, self.speed * 2])
                else:
                    self.dir_queue.put([0, 0, 0, -self.speed * 2])

        return frame

    # Look for origin marker and set directions
    def navigateToMarker(self, seen_id_list, tvecs, rvecs):
        if self.TargetID not in seen_id_list:
            # if lost, rotate in direction of last seen marker's x position
            if timer() - self.t_lost > 1:
                if self.last_marker_pos >= 0:
                    self.dir_queue.put([0, 0, 0, self.speed * 2])
                else:
                    self.dir_queue.put([0, 0, 0, -self.speed * 2])
        else:
            # select needed vectors
            ind = seen_id_list.index(self.TargetID)
            tvec = tvecs[ind]
            rvec = rvecs[ind]
            # only selected vectors from now
            self.last_marker_pos = tvec[0][0]
            rvec = tf.rotationVectorToEulerAngles(rvec) * 180 / math.pi
            # flip the yaw angle if marker is upside down
            if abs(rvec[0][2]) > 90:
                rvec[0][1] = -rvec[0][1]
            # print(tvec)
            # print(rvec)

            directions = [0., 0., 0., 0.]
            A = self.amplify * self.speed  # amplifier

            # yaw speed control
            err_yaw = rvec[0][1] - self.TargetPos[0][3]
            directions[3] = self.speed / 2 * self.yaw_pid.control(err_yaw)

            # vx, vy, vz speed control
            err_x = self.TargetPos[0][0] - tvec[0][0]
            directions[0] = -A * self.v_pid.control(err_x)
            err_y = self.TargetPos[0][2] - tvec[0][2]
            directions[1] = -A * self.v_pid.control(err_y)
            err_z = self.TargetPos[0][1] - tvec[0][1]
            directions[2] = A * self.vz_pid.control(err_z)

            # aligned to setpoint
            if abs(err_x) < ERROR and abs(err_y) < ERROR and abs(
                    err_z) < ERROR and abs(err_yaw) < 5:
                if timer() - self.t_inPos > 1:
                    if self.TargetID == 1:  # origin marker
                        self.getFirst = False
                        print("Positioned to origin, begin navigation")
                        self.seenMarkers.nullCoords()
                        self.getCoords_event.set()
                    if self.TargetID == 50:  # end marker
                        print("End of flight")
                        self.getCoords_event.clear()
                        self.resetNavigators()
                        self.navigate_event.clear()
                        self.END_event.set()

                    # target change
                    if not self.getFirst and not self.END_event.is_set():
                        self.beenThere.append(self.TargetID)
                        self.changeObjective(seen_id_list, tvecs)
                        if not self.findNew:
                            self.TargetPos = self.MarkerTarget.changeTarget(
                                self.TargetID)
                            # reset PIDs
                            self.yaw_pid.reset()
                            self.v_pid.reset()
                            self.vz_pid.reset()
                            print("Target changed to " + str(self.TargetID))
                        else:
                            print("Searching for new marker...")
            else:
                self.t_inPos = timer()  # if not in position, null timer

            self.t_lost = timer()  # null lost timer
            self.dir_queue.put(
                directions)  # put directions intu navigation queue

    # Reset all variables needed for navigation
    def resetNavigators(self):
        self.beenThere = []
        self.TargetID = 1
        self.yaw_pid.reset()
        self.v_pid.reset()
        self.vz_pid.reset()
        self.t_lost = 0.
        self.t_inPos = 0.
        self.last_marker_pos = 1.
        self.TargetPos = np.array([[0., 0., 0.8, 0.]])

    # Change target of navigation
    def changeObjective(self, seen_id_list, tvecs):
        length = len(seen_id_list)
        dist_minimum = 10000
        min_ind = self.TargetID
        for i in range(length):
            if np.linalg.norm(tvecs[i]) < dist_minimum and seen_id_list[
                    i] not in self.beenThere:
                dist_minimum = np.linalg.norm(tvecs[i])
                min_ind = seen_id_list[i]

        if self.TargetID == min_ind:
            self.findNew = True
        else:
            self.TargetID = min_ind

    # Draw line to targeted marker
    def drawCenter(self, frame, seen_id_list, corners, w, h):
        if self.TargetID not in seen_id_list:
            pass
        else:
            ind = seen_id_list.index(self.TargetID)
            cx = int((corners[ind][0][0][0] + corners[ind][0][1][0] +
                      corners[ind][0][2][0] + corners[ind][0][3][0]) / 4)
            cy = int((corners[ind][0][0][1] + corners[ind][0][1][1] +
                      corners[ind][0][2][1] + corners[ind][0][3][1]) / 4)
            cv2.line(frame, (int(w / 2), int(h / 2)), (cx, cy), (0, 255, 255),
                     3)

        return frame

    # Filter out marker corners on the frame's edges
    def filterCorners(self, w, h, corners, id_list, tvecs, rvecs):
        length = len(id_list)
        rejectIDs = []
        mask = np.ones(length)

        for i in range(length):
            for j in range(4):
                xc = corners[i][0][j][0]
                yc = corners[i][0][j][1]
                if (xc < w * EDGE) or (xc > w - w * EDGE) or (
                        yc < h * EDGE) or (yc > h - h * EDGE):
                    rejectIDs.append(id_list[i])
                    mask[i] = 0
                    #print("ID "+str(id_list[i])+" rejected")
                    break

        for i in range(len(rejectIDs)):
            id_list.remove(rejectIDs[i])

        okay = np.where(mask > 0)
        tvecs = np.r_[tvecs[okay]]
        rvecs = np.r_[rvecs[okay]]

        return id_list, tvecs, rvecs
示例#3
0
class Drone(VRepObject):
    SAFETY_RADIUS = 0.3  # meters. Value to add to the real radius of the UAV

    def __init__(self, client: VRepClient):
        self._body = client.get_object("Quadricopter_base")
        self._model = client.get_object("Quadricopter")
        super().__init__(client.id, self._body.handle, "")
        self._target = client.get_object("Quadricopter_target")
        self.sensor = client.get_object("fast3DLaserScanner_sensor")

        self._rotation_pid = PID(0.2, 0.05, 0.2, 1, max_int=3)
        self._altitude_pid = PID(0.2, 0.02, 0.2, 1)
        self._pid = PID(4.5, 0.01, 0.1, 3, 0.15, max_int=10)

        self.total_distance = 0
        self.sensor_offset = self._body.get_position(self.sensor)
        self.radius = self._get_radius() + self.SAFETY_RADIUS

        # Base and height of the visibility cone (actually a pyramid)
        B = 2 * self.sensor.max_depth * sin(radians(self.sensor.angle))
        H = 2 * self.sensor.max_depth * cos(radians(self.sensor.angle))
        # Constant for pixel-to-meters conversion
        self.K = self.sensor.res
        if abs(B - H) > 1e-3:
            self.K *= H / B


    def _get_radius(self) -> float:
        """Return the effective radius of the drone

        The radius is half of the distance between the extrema of the model's
        bounding box.
        """
        bbox = self._model.get_bbox()
        bbox_span = np.linalg.norm(bbox[1]-bbox[0])
        return bbox_span / 2

    def altitude_adjust(self, goal: VRepObject) -> None:
        good = err = 0.5  # meters
        while abs(err) >= good:
            goal_pos = goal.get_position(self._body)
            err = goal_pos[2]  # z-coordinate
            correction = self._altitude_pid.control(err)
            if __debug__:
                print("Adjusting altitude...", correction)
            self._target.set_position(self._target.get_position() +
                                      np.array([0, 0, correction]))
            self.total_distance += np.linalg.norm(correction)
            sleep(1)
        else:
            if __debug__:
                print("...Adjusted. Goal at {} m".format(err))
            self._altitude_pid.reset()
            sleep(2)  # Wait for the drone to stabilize

    def can_reach(self, goal: VRepObject):
        dist, azimuth, elevation = goal.get_spherical(self._body,
                                                      self.sensor_offset)
        delta = goal.get_position(self._target)
        h_dist = np.linalg.norm(delta[0:2])

        res, d = self.sensor.get_depth_buffer()

        X, Y = pinhole_projection(azimuth, elevation)
        ball_r = self.radius_to_pixels(dist)
        mask = cv2.circle(np.zeros_like(d), (X, Y), ball_r, 1, -1)
        try:
            min_depth = np.min(d[mask == 1]) * self.sensor.max_depth
        except ValueError:
            # Mask has no white pixel. Center view on goal and retry
            self.lock(goal)
            return self.can_reach(goal)
        reachable = h_dist < 1 or dist - min_depth < -self.radius or \
            min_depth == self.sensor.max_depth
        return reachable, d, min_depth, mask

    def radius_to_pixels(self, dist: float) -> int:
        """Converts a drone radius in pixels, at the given distance.

        This function returns the size in pixels of a segment of length RADIUS,
        placed at distance `dist` and orthogonal to the principal axis of the
        camera.
        """
        return max(int(self.K * self.radius / dist), 1)

    def reset_controllers(self):
        self._pid.reset()
        self._altitude_pid.reset()
        self._rotation_pid.reset()

    def rotate_towards(self, goal: VRepObject):
        """Rotate the drone until it points towards the goal.

        Actually, the function rotates the `target` object which is then
        followed by the `drone` (inside V-REP).
        """
        good = azimuth = 2  # Degrees
        while abs(azimuth) >= good:
            euler = self._target.get_orientation()
            __, azimuth, __ = goal.get_spherical(self._body,
                                                 self.sensor_offset)
            correction_angle = self._rotation_pid.control(azimuth)
            if __debug__:
                print("Adjusting orientation...", correction_angle)
            euler[2] += radians(correction_angle)  # euler[2] = Yaw
            self._target.set_orientation(euler)
            sleep(1)
        else:
            if __debug__:
                print("...Adjusted. Goal at {}°".format(azimuth))
            self._rotation_pid.reset()
            self.stabilize()  # Wait for the drone to stabilize on new angle

    def lock(self, goal: VRepObject):
        __, azimuth, elevation = goal.get_spherical(self._body,
                                                    self.sensor_offset)
        X, Y = pinhole_projection(azimuth, elevation)
        if abs(elevation) > self.sensor.angle or not 0 <= Y < 256:
            self.altitude_adjust(goal)
        self.rotate_towards(goal)

    def stabilize(self):
        eps = 0.001
        if __debug__:
            print("UAV stabilization in progress...")
        while True:
            lin_v, ang_v = self._body.get_velocity()
            if all(i < eps for i in lin_v) and all(i < eps for i in ang_v):
                if __debug__:
                    sleep(0.5)
                    print("...done.")
                return
            else:
                sleep(0.05)

    def step_towards(self, goal: VRepObject):
        """Move the drone towards the goal.
        """
        target_pos = self._target.get_position()
        correction = self._pid.control(-self._target.get_position(goal))
        self.total_distance += np.linalg.norm(correction)
        self._target.set_position(target_pos + correction)

    def escape(self, goal):
        # TODO implement wall-following algorithm
        self.rotate(60)
        __, d = self.sensor.get_depth_buffer()
        left_space = len(d[d == 1])
        self.rotate(-120)
        __, d = self.sensor.get_depth_buffer()
        right_space = len(d[d == 1])
        go_left = left_space >= right_space

    def rotate(self, angle: float):
        """Perform an arbitrary yaw rotation.
        
        Args:
            angle (float): Yaw angle, in degrees. Positive = rotates left
        """
        self._rotation_pid.reset()
        while abs(angle) > 2:
            euler = self._target.get_orientation()
            correction = self._rotation_pid.control(angle)
            angle -= correction
            euler[2] += radians(correction)  # euler[2] = Yaw
            self._target.set_orientation(euler)
            sleep(1)
        self.stabilize()
示例#4
0
class PIDDemoBase(object):
    """
    """

    def __init__(self):
        """
        """

        self._units = None

        # simulation time
        self._delta_time = None  # [s]
        self._max_time = None  # [s]
        self._time = None  # [s]

        # controller time
        self._control_delta = None  # [s]
        self._control_interval = None  # [steps]

        # forcing
        self._forcing_mean = None
        self._forcing_standard_deviation = None
        self._forcing = None

        # state
        self._control = None
        self._state_control = None
        self._state_no_control = None

        # controller
        self._set_point = None
        self._control_bias = None
        self._pid = None

    def __str__(self):
        """
        """
        message = ''
        message += "control_delta = {0} s".format(self._control_delta)
        return message

    def _initialize_units(self, units):
        """Assign user supplied units to various variables.
        """
        default = '?'
        if not units:
            units = []

        needed_units = ['time', 'process', 'control', 'forcing']
        for var in needed_units:
            if var not in units:
                units[var] = default
        self._units = units

    def _initialize_simulation_time(self, delta_time, max_time):
        """Initialize the time stepping for the process simeanlation
        """
        self._delta_time = delta_time
        self._max_time = max_time
        self._time = np.arange(0.0, self._max_time, self._delta_time)

    def _initialize_controller_time(self, control_delta):
        """Initialize the timing of the controller, e.g. how often process
        samples are taken and send to the controller for computation
        of the control variable.

        """
        self._control_delta = control_delta
        self._control_interval = int(self._control_delta / self._delta_time)
        print("Control delta = {0} [s]".format(self._control_delta))
        print("Control interval = {0} [time steps]".format(
            self._control_interval))

    def _initialize_constant_forcing(self, forcing_mean):
        """Use a constant forcing.
        """
        self._forcing_mean = forcing_mean
        self._forcing = self._forcing_mean * np.ones(len(self._time))

    def _initialize_normal_forcing(self,
                                   forcing_mean, forcing_standard_deviation):
        """Use a random forcing from a normal distribution with the specified
        meand and standard deviation.

        """
        self._forcing_mean = forcing_mean
        self._forcing_standard_deviation = forcing_standard_deviation
        np.random.seed(770405)
        self._forcing = np.random.normal(
            self._forcing_mean, self._forcing_standard_deviation,
            len(self._time))

    def _initialize_pid(self, history_length, set_point,
                        Kp, Ki, Kd, control_bias):
        """
        """
        print("Initializing PID controller:")
        print("  history length = {0}".format(history_length))
        print("  set_point = {0}".format(set_point))
        print("  Kp = {0}".format(Kp))
        print("  Ki = {0}".format(Ki))
        print("  Kd = {0}".format(Kd))
        self._pid = PID(history_length, set_point, Kp, Ki, Kd)
        self._control_bias = control_bias

    def simulate_no_control(self):
        """
        """
        self._state_no_control = np.zeros(len(self._time))
        self._state_no_control[0] = self._initial_condition
        for t in range(1, len(self._time)):
            previous_state = self._state_no_control[t-1]
            self._state_no_control[t] = self.process(
                self._forcing[t], self._delta_time, previous_state,
                self._control_bias)

    def simulate_with_control(self):
        """
        """
        self._state_control = np.zeros(len(self._time))
        self._control = np.zeros(len(self._time))
        self._state_control[0] = self._initial_condition
        self._control[0] = self._control_bias
        control = self._control[0]
        for t in range(1, len(self._time)):
            previous_state = self._state_control[t-1]
            process_value = self.process(self._forcing[t], self._delta_time,
                                         previous_state, control)
            self._state_control[t] = process_value
            if t % self._control_interval == 0:
                control_delta = self._pid.control(process_value,
                                                  self._delta_time)
                control = self._control_bias - control_delta
            # NOTE(bja, 2016-11) for plotting. Want control at all
            # time points, not just when it is being changed!
            self._control[t] = control

    def plot(self):
        """
        """
        nrows = 3
        ncols = 1
        plt.figure(1)
        plt.subplot(nrows, ncols, 1)
        plt.plot(self._time, self._state_control, label='controled')
        plt.plot(self._time, self._state_no_control, label='no control')
        set_point = self._set_point * np.ones(len(self._time))
        plt.plot(self._time, set_point, label='set point')
        plt.legend(loc='best', ncol=2)
        plt.ylabel("Process variable [{0}]".format(self._units['process']))

        plt.subplot(nrows, ncols, 2)
        plt.plot(self._time, self._control, label='control variable')
        control_bias = self._control_bias * np.ones(len(self._time))
        plt.plot(self._time, control_bias, label='control_bias')
        plt.legend(loc='best', ncol=2)
        plt.ylabel("control variable [{0}]".format(self._units['control']))

        plt.subplot(nrows, ncols, 3)
        plt.plot(self._time, self._forcing, label='forcing')
        forcing_mean = self._forcing_mean * np.ones(len(self._time))
        plt.plot(self._time, forcing_mean, label='forcing mean')

        plt.legend(loc='best', ncol=2)
        plt.ylabel("Forcing [{0}]".format(self._units['forcing']))
        plt.xlabel("time [{0}]".format(self._units['time']))
        plt.show()

    def summary(self):
        """summar of the final system state
        """
        print("System summary:")
        print("  Without control:")
        value = self._state_no_control[-1]
        print("    Final process value = {0:1.6e} [{1}]".format(
            value, self._units['process']))
        print("    pv - sp = {0:1.6e} [{1}]".format(
            value - self._set_point, self._units['process']))

        print("  With control:")
        value = self._state_no_control[-1]
        print("    Final process value = {0:1.6e} [{1}]".format(
            value, self._units['process']))
        print("    pv - sp = {0:1.6e} [{1}]".format(
            value - self._set_point, self._units['process']))
        value = self._control[-1]
        print("    Final control value = {0:1.6e} [{1}]".format(
            value, self._units['control']))
        print("    control - bias = {0:1.6e} [{1}]".format(
            value - self._control_bias, self._units['control']))

    def run(self):
        """
        """
        self.simulate_no_control()
        self.simulate_with_control()
        self.summary()
        self.plot()

    @abc.abstractmethod
    def process(self, forcing, delta_time, previous_state, control_bias):
        """
        """
        return None

    @abc.abstractmethod
    def _calculate_control_bias(self, steady_state_forcing, set_point):
        """
        """
        return None

    @abc.abstractmethod
    def _calculate_set_point(self):
        """
        """
        return None

    @abc.abstractmethod
    def _calculate_dCVdPV(self):
        """Calculate the steady state dCV/dPV as an approximation for Kp.
        """
        return None

    @abc.abstractmethod
    def _calculate_time_scales(self, q_in, h_sp, a_out, h):
        """Estimate the time scales of the problem:
        """
        return None