wall_follow.py 文件源码

def compute_pd_control(self):
        if self.received_data:
            # given the computed wall slope, compute theta, avoid divide by zero error
            if np.abs(self.m) < EPSILON:
                theta = np.pi / 2.0
                x_intercept = 0
            else:
                theta = np.arctan(1.0/self.m)
                # solve for y=0 in y=mx+c
                x_intercept = self.c / self.m

            # x axis is perp. to robot but not perpindicular to wall
            # cosine term solves for minimum distance to wall
            wall_dist = np.abs(np.cos(theta)*x_intercept)

            # control proportional to angular error and distance from wall
            distance_term = self.direction_muliplier * KP * (wall_dist - TARGET_DISTANCE)
            angle_term = KD * theta
            control = angle_term + distance_term
            # avoid turning too sharply
            self.control = (np.clip(control, -0.3, 0.3), SPEED)