def __init__(self, boat, destination, positionThreshold=1.0, controller_name="PointAndShoot"): super(LineFollower, self).__init__(boat) self._x0 = boat.state[0] self._x1 = destination[0] self._y0 = boat.state[1] self._y1 = destination[1] self._dx = self._x1 - self._x0 self._dy = self._y1 - self._y0 self._th = np.arctan2(self._dy, self._dx) self._L = np.sqrt(np.power(self._dx, 2.) + np.power(self._dy, 2.)) self._destination = destination self._lookAhead = 5.0 self._positionThreshold = positionThreshold if controller_name == "PointAndShoot": THRUST_PID = [0.15, 0, 0] #[0.5, 0.01, 10.00] # P, I, D HEADING_PID = [0.1, 0, 0] #[1.0, 0.0, 1.0] # P, I, D HEADING_ERROR_SURGE_CUTOFF_ANGLE = 180.0 # [degrees of heading error at which thrust is forced to be zero, follows a half-cosine shape] self.controller = Controllers.PointAndShootPID( boat, THRUST_PID, HEADING_PID, HEADING_ERROR_SURGE_CUTOFF_ANGLE, positionThreshold) elif controller_name == "QLearnPointAndShoot": self.controller = Controllers.QLearnPointAndShoot(boat)
def __init__(self, boat, destination, positionThreshold=1.0, controller_name="PointAndShoot"): super(DestinationOnly, self).__init__(boat) self._destinationState = destination if controller_name == "PointAndShoot": THRUST_PID = [0.5, 0, 0] #[0.5, 0.01, 10.00] # P, I, D HEADING_PID = [0.7, 0, 0] #[1.0, 0.0, 1.0] # P, I, D HEADING_ERROR_SURGE_CUTOFF_ANGLE = 180.0 # [degrees of heading error at which thrust is forced to be zero, follows a half-cosine shape] self.controller = Controllers.PointAndShootPID( boat, THRUST_PID, HEADING_PID, HEADING_ERROR_SURGE_CUTOFF_ANGLE, positionThreshold) elif controller_name == "QLearnPointAndShoot": self.controller = Controllers.QLearnPointAndShoot(boat)