def publishRudderWinchAngle(event):
if (rudderAngleRad is not None and sailWinchPosition is not None
and jibWinchPosition is not None):
rospy.loginfo(
"\n" + "Published on {}\n".format(
datetime.fromtimestamp(int(event.current_real.to_sec()))) +
"\n" + "SENSOR READINGS\n" +
"\tCurrent Heading: {} radians\n".format(headingMeasureRad) +
"\tDesired Heading: {} radians\n".format(headingSetPointRad) +
"\tWind Angle: {} radians\n".format(apparentWindAngleRad) +
"\tGround Speed: {} knots\n".format(groundspeedKnots) +
"\tLow Wind: {}\n".format(lowWind) +
"\tLow Voltage: {}\n".format(lowVoltage) + "\n" +
"CONTROLLER STATE\n" + "\tControl Mode: {}\n".format(CONTROL_MODES[
controller.getControlModeID()]) +
"\tFixed Control State: {}\n".format(controller.controlModeIsFixed)
+ "\tLow Power: {}\n".format(lowVoltage or lowWind) +
"\tLow Power Disabled: {}\n".format(controller.lowPowerDisabled) +
"\n" + "PUBLISHED VALUES\n" +
"\tRudder Angle: {} radians\n".format(rudderAngleRad) +
"\tSail Winch Position: {}\n".format(sailWinchPosition) +
"\tJib Winch Position: {}\n".format(jibWinchPosition) + "\n")
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(
sailWinchPosition, sailbot_constants.MAX_WINCH_POSITION,
sailbot_constants.MIN_WINCH_POSITION),
ControllerOutputRefiner.saturate(
jibWinchPosition, sailbot_constants.MAX_WINCH_POSITION,
sailbot_constants.MIN_WINCH_POSITION))
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 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 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_saturate_justAboveLowerBound(self):
self.assertEqual(
ControllerOutputRefiner.saturate(math.pi**3.11, 100,
math.pi**3.11 - (1 / 149)**16.03),
math.pi**3.11)
def test_saturate_justBelowUpperBound(self):
self.assertEqual(
ControllerOutputRefiner.saturate(math.pi - (1 / 100000)**2.43,
math.pi, 0),
math.pi - (1 / 100000)**2.43)
def test_saturate_justAboveUpperBound(self):
self.assertEqual(
ControllerOutputRefiner.saturate(math.pi + (1 / 11)**2.1, math.pi,
0), math.pi)
def test_saturate_smallerThanLowerBound(self):
self.assertEqual(ControllerOutputRefiner.saturate(-15, 10, -10), -10)
def test_saturate_greaterThanUpperBound(self):
self.assertEqual(ControllerOutputRefiner.saturate(3.5, 3, -1), 3)
def test_saturate_inputWithinBound(self):
self.assertEqual(
ControllerOutputRefiner.saturate(math.pi, 2 * math.pi,
-2 * math.pi), math.pi)
def test_lowPowerAngle_min_angle_difference1(self):
self.assertTrue(
ControllerOutputRefiner.lowPowerAngle(1.0,
1.0 + 5.1 * math.pi / 180.0))
def test_saturate_equalUpperAndLower1(self):
self.assertEqual(
ControllerOutputRefiner.saturate(math.pi, math.pi, math.pi),
math.pi)
def test_lowPowerAngle_equal2(self):
self.assertFalse(ControllerOutputRefiner.lowPowerAngle(0.0, 0.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_difference3(self):
self.assertTrue(
ControllerOutputRefiner.lowPowerAngle(
-1.076, -1.076 + 5.0 * math.pi / 180.0))
def test_lowPowerAngle_min_angle_difference2(self):
self.assertFalse(
ControllerOutputRefiner.lowPowerAngle(1.0 + 4.9 * math.pi / 180.0,
1.0))
def test_saturate_justBelowLowerBound(self):
self.assertEqual(
ControllerOutputRefiner.saturate(
10, 20, 10.0000000001 - (1 / 343443)**2.45435),
10.0000000001 - (1 / 343443)**2.45435)
def test_lowPowerAngle_dont_switch2(self):
self.assertFalse(
ControllerOutputRefiner.lowPowerAngle(-2.0756, -2.0851))
def test_saturate_badBoundInputs(self):
with self.assertRaises(AssertionError):
ControllerOutputRefiner.saturate(0, 10, 10.0000001)
def test_lowPowerAngle_equal3(self):
self.assertFalse(ControllerOutputRefiner.lowPowerAngle(-2.567, -2.567))
def test_saturate_equalUpperAndLower2(self):
self.assertEqual(ControllerOutputRefiner.saturate(3.00001, 3, 3), 3)
def test_lowPowerAngle_dont_switch1(self):
self.assertFalse(ControllerOutputRefiner.lowPowerAngle(1.0054, 1.0))