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 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_saturate_equalUpperAndLower2(self):
self.assertEqual(ControllerOutputRefiner.saturate(3.00001, 3, 3), 3)
def test_saturate_equalUpperAndLower1(self):
self.assertEqual(
ControllerOutputRefiner.saturate(math.pi, math.pi, math.pi),
math.pi)
def test_saturate_badBoundInputs(self):
with self.assertRaises(AssertionError):
ControllerOutputRefiner.saturate(0, 10, 10.0000001)
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_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)