def get_jib_winch_position(self, apparentWindAngleRad, X1, X2):
"""
Calculates the winch position of the jib according to the apparent wind angle.
Arguments
---------
float : apparentWindAngleRad
The wind angle in radians
float : X1
Parameter for jib saturation function
float : X2
Parameter for jib saturation function
Returns
-------
int
The winch position of the jib
"""
jibAngle = JibController.get_jib_angle(apparentWindAngleRad, X1, X2)
smallChange = not ControllerOutputRefiner.lowPowerAngle(
inputSignal=jibAngle, currAngle=self.__currJibAngleRad)
if (self.__controlModeID
== ControlModes.LOW_POWER.value) and (smallChange):
winchPosition = JibController.get_winch_position(
self.__currJibAngleRad, self.__winchQuantParam)
else:
self.__currJibAngleRad = jibAngle
winchPosition = JibController.get_winch_position(
jibAngle, self.__winchQuantParam)
return winchPosition
def get_heading_error(self, current_heading, desired_heading,
apparent_wind_angle):
"""
Calculates the heading error. The heading error varies depending on the current control
mode of the boat.
Arguments
---------
float : current_heading
The current direction of the boat in radians.
float : desired_heading
The desired direction of the boat in radians.
float : apparent_wind_angle
The apparent wind angle in radians.
Returns
-------
float
Returns the heading error depending on the current control mode in radians.
"""
if (self.__controlModeID == ControlModes.JIBE_ONLY.value):
jibe_direction = JibeOnlyRudderController.get_jibe_controller_direction(
current_heading=current_heading,
desired_heading=desired_heading,
apparent_wind_angle=apparent_wind_angle)
error = JibeOnlyRudderController.get_jibe_controller_error(
current_heading=current_heading,
desired_heading=desired_heading,
jibe_direction=jibe_direction)
return error
# TACKABLE
elif (self.__controlModeID == ControlModes.TACKABLE.value):
error = TackController.get_heading_error_tackable(
setPoint=desired_heading, measure=current_heading)
return error
# LOW POWER or UNKNOWN
else:
if (ControllerOutputRefiner.lowPowerAngle(
inputSignal=desired_heading, currAngle=current_heading)):
error = TackController.get_heading_error_tackable(
setPoint=desired_heading, measure=current_heading)
return error
else:
return 0
def test_lowPowerAngle_min_angle_difference4(self):
self.assertTrue(
ControllerOutputRefiner.lowPowerAngle(
-2.0453 + 5.1 * math.pi / 180.0, -2.0453))
def test_lowPowerAngle_min_angle_difference2(self):
self.assertFalse(
ControllerOutputRefiner.lowPowerAngle(1.0 + 4.9 * math.pi / 180.0,
1.0))
def test_lowPowerAngle_min_angle_difference3(self):
self.assertTrue(
ControllerOutputRefiner.lowPowerAngle(
-1.076, -1.076 + 5.0 * math.pi / 180.0))
def test_lowPowerAngle_min_angle_difference1(self):
self.assertTrue(
ControllerOutputRefiner.lowPowerAngle(1.0,
1.0 + 5.1 * math.pi / 180.0))
def test_lowPowerAngle_equal3(self):
self.assertFalse(ControllerOutputRefiner.lowPowerAngle(-2.567, -2.567))
def test_lowPowerAngle_equal2(self):
self.assertFalse(ControllerOutputRefiner.lowPowerAngle(0.0, 0.0))
def test_lowPowerAngle_dont_switch2(self):
self.assertFalse(
ControllerOutputRefiner.lowPowerAngle(-2.0756, -2.0851))
def test_lowPowerAngle_dont_switch1(self):
self.assertFalse(ControllerOutputRefiner.lowPowerAngle(1.0054, 1.0))