def test_tack_error_function(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS + 0.1
# Enter TACKABLE mode from UNKNOWN
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value)
self.assertEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
mock_current_heading = math.pi
mock_desired_heading = math.pi / 2
mock_wind_angle = math.pi / 4
# Error function in heading controller should be the same as tack controller error functions
self.assertAlmostEqual(
hc.get_heading_error(mock_current_heading, mock_desired_heading,
mock_wind_angle),
TackController.get_heading_error_tackable(mock_desired_heading,
mock_current_heading))
def test_jibe_error_function(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS - 0.1
# Enter JIBE_ONLY mode from UNKNOWN
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value)
self.assertEqual(hc.getControlModeID(), ControlModes.JIBE_ONLY.value)
mock_current_heading = -math.pi
mock_desired_heading = math.pi / 2
mock_wind_angle = math.pi / 4
jibe_direction = JibeOnlyRudderController.get_jibe_controller_direction(
mock_current_heading, mock_desired_heading, mock_wind_angle)
# Error function in heading controller should be the same as jibe controller error functions
self.assertAlmostEqual(
hc.get_heading_error(mock_current_heading, mock_desired_heading,
mock_wind_angle),
JibeOnlyRudderController.get_jibe_controller_error(
mock_current_heading, mock_desired_heading, jibe_direction))