def set_parameters(self, params):
"""Set PID values and sensor poses.
The params structure is expected to have sensor poses in the robot's
reference frame as ``params.sensor_poses``.
"""
AvoidObstacles.set_parameters(self,params)
def get_heading(self, state):
"""Blend the two heading vectors"""
# Get the outputs of the two subcontrollers
u_ao = AvoidObstacles.get_heading(self,state)
self.away_angle = math.atan2(u_ao[1],u_ao[0])
u_ao = numpy.array([math.cos(self.away_angle),math.sin(self.away_angle),1])
self.goal_angle = GoToGoal.get_heading_angle(self,state)
u_gtg = numpy.array([math.cos(self.goal_angle),math.sin(self.goal_angle),1])
# Week 5 Assigment Code goes here:
u = u_gtg
# End Week 5 Assigment
return u
