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 test_getSailAngle_X1_X2_mid(self):
X2_TEST = 3 * math.pi / 4 + 0.2
X1_TEST = math.pi / 4 + 0.2
self.assertAlmostEqual(
JibController.get_jib_angle((X2_TEST + X1_TEST) / 2, X1_TEST,
X2_TEST),
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE / 2)
self.assertAlmostEqual(
JibController.get_jib_angle((X2_TEST * 0.75 + X1_TEST * 0.25),
X1_TEST, X2_TEST),
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE / 4)
self.assertAlmostEqual(
JibController.get_jib_angle((X2_TEST * 0.25 + X1_TEST * 0.75),
X1_TEST, X2_TEST),
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE * 0.75)
def test_getSailAngle_apparentWindAngleZero(self):
X1 = 0
X2 = math.pi
self.assertEqual(
JibController.get_jib_angle(0, X1, X2),
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE,
)
def test_getSailAngle_apparentWindAnglePositiveNotBounded(self):
X1 = 0
X2 = math.pi
self.assertAlmostEqual(
JibController.get_jib_angle(2 + 10 * math.pi, X1, X2),
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE / (-math.pi) * 2 +
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE,
)
def test_getSailAngle_apparentWindAngle_boundedBetweenPiAnd2Pi(self):
X1 = 0
X2 = math.pi
self.assertAlmostEqual(
JibController.get_jib_angle(3 / 2 * math.pi, X1, X2),
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE / (-math.pi) *
(math.pi / 2) + sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE,
)
def test_getSailAngle_apparentWindAngleBetweenNegativePiAnd0(self):
X1 = 0
X2 = math.pi
self.assertAlmostEqual(
JibController.get_jib_angle(-2, X1, X2),
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE / (-math.pi) * 2 +
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE,
)
def test_quant_winch_position_360(self):
quant_param = 360
self.assertEqual(
JibController.get_winch_position(math.pi / 5, quant_param), 144)
self.assertEqual(
JibController.get_winch_position(math.pi / 8, quant_param), 90)
self.assertEqual(
JibController.get_winch_position(math.pi / 16, quant_param), 45)
self.assertEqual(
JibController.get_winch_position(math.pi / 17, quant_param), 42)
self.assertEqual(
JibController.get_winch_position(math.pi / 3, quant_param), 239)
self.assertEqual(
JibController.get_winch_position(math.pi / 2.2, quant_param), 327)
self.assertEqual(
JibController.get_winch_position(math.pi / 2.5, quant_param), 288)
def test_quant_winch_position_4(self):
quant_param = 4
self.assertEqual(
JibController.get_winch_position(math.pi / 5, quant_param), 180)
self.assertEqual(
JibController.get_winch_position(math.pi / 8, quant_param), 90)
self.assertEqual(
JibController.get_winch_position(math.pi / 16, quant_param), 90)
self.assertEqual(
JibController.get_winch_position(math.pi / 17, quant_param), 0)
self.assertEqual(
JibController.get_winch_position(math.pi / 3, quant_param), 270)
self.assertEqual(
JibController.get_winch_position(math.pi / 2.2, quant_param), 360)
self.assertEqual(
JibController.get_winch_position(math.pi / 2.5, quant_param), 270)
def publishRudderWinchAngle():
if (headingSetPointRad is not None and headingMeasureRad is not None
and apparentWindAngleRad is not None
and groundspeedKnots is not None):
global rudderAngleRad
heading_error = controller.get_heading_error(
current_heading=headingMeasureRad,
desired_heading=headingSetPointRad,
apparent_wind_angle=apparentWindAngleRad)
controller.switchControlMode(heading_error=heading_error,
boat_speed=groundspeedKnots)
rudderAngleRad = (controller.get_feed_back_gain(heading_error) *
heading_error)
global sailAngle
sailAngle = (int(
SailController.get_sail_angle(apparentWindAngleRad) *
(360 / (math.pi / 2))))
global jibAngle
jibAngle = (int(
JibController.get_jib_angle(apparentWindAngleRad) *
(360 / (math.pi / 2))))
rudder_winch_actuation_angle_pub.publish(
ControllerOutputRefiner.saturate(
rudderAngleRad, sailbot_constants.MAX_ABS_RUDDER_ANGLE_RAD,
-sailbot_constants.MAX_ABS_RUDDER_ANGLE_RAD),
ControllerOutputRefiner.saturate(
sailAngle, sailbot_constants.MAX_WINCH_POSITION,
sailbot_constants.MIN_WINCH_POSITION),
ControllerOutputRefiner.saturate(
jibAngle, sailbot_constants.MAX_WINCH_POSITION,
sailbot_constants.MIN_WINCH_POSITION))
def test_getSailAngle_X2_varying(self):
X1 = 0
self.assertAlmostEqual(
JibController.get_jib_angle(3 * math.pi / 4 + 0.01, X1,
3 * math.pi / 4), 0)
def test_getSailAngle_X1_varying(self):
X2 = math.pi
self.assertAlmostEqual(
JibController.get_jib_angle(math.pi / 4 - 0.01, math.pi / 4, X2),
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE)
def test_getSailAngle_apparentWindAngleNegativePi(self):
X1 = 0
X2 = math.pi
self.assertEqual(JibController.get_jib_angle(-math.pi, X1, X2), 0)
def test_getSailAngle_apparentWindAngleBetween0AndPi(self):
self.assertAlmostEqual(
JibController.get_jib_angle(2),
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE / (-math.pi) * 2 +
sailbot_constants.JIB_CONTROLLER_MAX_SAIL_ANGLE,
)
def test_getSailAngle_apparentWindAnglePi(self):
self.assertEqual(JibController.get_jib_angle(math.pi), 0)
