def get_sail_winch_position(self, apparentWindAngleRad, X1, X2):
"""
Calculates the winch position of the sail according to the apparent wind angle.
Arguments
---------
float : apparentWindAngleRad
The wind angle in radians
float : X1
Parameter for sail saturation function
float : X2
Parameter for sail saturation function
Returns
-------
int
The winch position of the sail
"""
sailAngle = SailController.get_sail_angle(apparentWindAngleRad, X1, X2)
smallChange = not ControllerOutputRefiner.lowPowerAngle(
inputSignal=sailAngle, currAngle=self.__currSailAngleRad)
if (self.__controlModeID
== ControlModes.LOW_POWER.value) and (smallChange):
winchPosition = SailController.get_winch_position(
self.__currSailAngleRad, self.__winchQuantParam)
else:
self.__currSailAngleRad = sailAngle
winchPosition = SailController.get_winch_position(
sailAngle, 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(
SailController.get_sail_angle((X2_TEST + X1_TEST) / 2, X1_TEST, X2_TEST),
sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE / 2
)
self.assertAlmostEqual(
SailController.get_sail_angle((X2_TEST * 0.75 + X1_TEST * 0.25), X1_TEST, X2_TEST),
sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE / 4
)
self.assertAlmostEqual(
SailController.get_sail_angle((X2_TEST * 0.25 + X1_TEST * 0.75), X1_TEST, X2_TEST),
sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE * 0.75
)
def test_getSailAngle_apparentWindAngleZero(self):
X1 = 0
X2 = math.pi
self.assertEqual(
SailController.get_sail_angle(0, X1, X2),
sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE,
)
def test_getSailAngle_apparentWindAnglePositiveNotBounded(self):
X1 = 0
X2 = math.pi
self.assertAlmostEqual(
SailController.get_sail_angle(2 + 10 * math.pi, X1, X2),
sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE / (-math.pi) * 2
+ sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE,
)
def test_getSailAngle_apparentWindAngle_boundedBetweenPiAnd2Pi(self):
X1 = 0
X2 = math.pi
self.assertAlmostEqual(
SailController.get_sail_angle(3 / 2 * math.pi, X1, X2),
sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE /
(-math.pi) * (math.pi / 2)
+ sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE,
)
def test_quant_winch_position_360(self):
quant_param = 360
self.assertEqual(SailController.get_winch_position(math.pi / 5, quant_param), 144)
self.assertEqual(SailController.get_winch_position(math.pi / 8, quant_param), 90)
self.assertEqual(SailController.get_winch_position(math.pi / 16, quant_param), 45)
self.assertEqual(SailController.get_winch_position(math.pi / 17, quant_param), 42)
self.assertEqual(SailController.get_winch_position(math.pi / 3, quant_param), 239)
self.assertEqual(SailController.get_winch_position(math.pi / 2.2, quant_param), 327)
self.assertEqual(SailController.get_winch_position(math.pi / 2.5, quant_param), 288)
def test_quant_winch_position_4(self):
quant_param = 4
self.assertEqual(SailController.get_winch_position(math.pi / 5, quant_param), 180)
self.assertEqual(SailController.get_winch_position(math.pi / 8, quant_param), 90)
self.assertEqual(SailController.get_winch_position(math.pi / 16, quant_param), 90)
self.assertEqual(SailController.get_winch_position(math.pi / 17, quant_param), 0)
self.assertEqual(SailController.get_winch_position(math.pi / 3, quant_param), 270)
self.assertEqual(SailController.get_winch_position(math.pi / 2.2, quant_param), 360)
self.assertEqual(SailController.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_apparentWindAngleBetweenNegativePiAnd0(self):
self.assertAlmostEqual(
SailController.get_sail_angle(-2),
sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE / (-math.pi) * 2 +
sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE,
)
def test_getSailAngle_apparentWindAngleNegativePi(self):
self.assertEqual(SailController.get_sail_angle(-math.pi), 0)
def test_getSailAngle_X2_varying(self):
X1 = 0
self.assertAlmostEqual(
SailController.get_sail_angle(3 * math.pi / 4 + 0.01, X1, 3 * math.pi / 4),
0
)
def test_getSailAngle_X1_varying(self):
X2 = math.pi
self.assertAlmostEqual(
SailController.get_sail_angle(math.pi / 4 - 0.01, math.pi / 4, X2),
sailbot_constants.SAIL_CONTROLLER_MAX_SAIL_ANGLE
)
def test_getSailAngle_apparentWindAnglePi(self):
X1 = 0
X2 = math.pi
self.assertEqual(SailController.get_sail_angle(math.pi, X1, X2), 0)