def test_feedback_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)
self.assertEqual(
hc.get_feed_back_gain(-1.2),
sailbot_constants.KP / (1 + sailbot_constants.CP * abs(-1.2)),
)
def test_reachedHeading_exitTackable(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS + 0.1
# Enter TACKABLE from UNKNOWN
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value)
mock_heading_error = -0.5 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
# A switch occurs
self.assertTrue(
hc.switchControlMode(heading_error=mock_heading_error,
boat_speed=mock_speed))
# Should not have changed modes
self.assertEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
def test_noExit_tackableMode(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS + 0.1
mock_heading_error = 2 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
# Enter TACKABLE mode from UNKNOWN
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value)
# Adjust parameters
mock_speed *= 2
mock_heading_error = 1.5 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
# No switch
self.assertFalse(
hc.switchControlMode(heading_error=mock_heading_error,
boat_speed=mock_speed))
self.assertEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
def test_slowSpeed_exitTackable(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS + 0.1
mock_heading_error = 2 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
# Enter TACKABLE from UNKNOWN
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value)
# Make speed low
mock_speed -= 0.2
self.assertTrue(
hc.switchControlMode(heading_error=mock_heading_error,
boat_speed=mock_speed))
# Should not be in TACKABLE or UNKNOWN
self.assertNotEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
self.assertNotEqual(hc.getControlModeID(), ControlModes.UNKNOWN.value)
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_tack_to_jibe(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS + 0.1
mock_heading_error = 2 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
# Enter TACKABLE from UNKNOWN
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value)
self.assertEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
# Adjust parameters to switch to JIBE_ONLY
mock_speed = 0.5 * sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS
# A switch should occur
self.assertTrue(
hc.switchControlMode(heading_error=mock_heading_error,
boat_speed=mock_speed))
# Should now be in JIBE_ONLY mode
self.assertEqual(hc.getControlModeID(), ControlModes.JIBE_ONLY.value)
def test_fixed_low_power(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS - 0.1
mock_heading_error = 2 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
fixedMode = ControlModes.LOW_POWER.value
# Enter LOWER_POWER from UNKNOWN
hc = HeadingController(mock_speed,
ControlModes.UNKNOWN.value,
fixedControlMode=fixedMode)
self.assertEqual(hc.getControlModeID(), ControlModes.LOW_POWER.value)
# A switch should NOT occur
self.assertFalse(
hc.switchControlMode(heading_error=mock_heading_error,
boat_speed=mock_speed))
# Should now be in LOW_POWER mode
self.assertEqual(hc.getControlModeID(), ControlModes.LOW_POWER.value)
self.assertTrue(hc.controlModeIsFixed)
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))
def test_tack_disabled_low_power(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS + 0.1
mock_heading_error = 2 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
# Enter TACKABLE from UNKNOWN
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value, True)
self.assertEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
# Adjust parameters to switch to LOW POWER
low_battery = 0
low_wind = 1
# A switch should NOT occur
self.assertFalse(
hc.switchControlMode(heading_error=mock_heading_error,
boat_speed=mock_speed,
low_battery_level=low_battery,
low_wind=low_wind))
# Should now be in TACKABLE mode and not in LOW_POWER mode
self.assertEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
def test_switch_from_low_power(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS - 0.1
# Can be anything
mock_heading_error = 2 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
# Enter JIBE_ONLY from UNKNOWN
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value)
self.assertEqual(hc.getControlModeID(), ControlModes.JIBE_ONLY.value)
# Adjust parameters to switch to LOW POWER
# Only one or both of these need to be 1
low_battery = 1
low_wind = 1
# A switch should occur
self.assertTrue(
hc.switchControlMode(heading_error=mock_heading_error,
boat_speed=mock_speed,
low_battery_level=low_battery,
low_wind=low_wind))
# Adjust parameters to switch to TACKABLE
mock_speed = 2 * sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS
mock_heading_error = 0.5 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
# A switch should occur
self.assertTrue(
hc.switchControlMode(heading_error=mock_heading_error,
boat_speed=mock_speed))
# Should now be in TACKABLE mode
self.assertEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
def test_fixed_tack_mode(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS + 0.1
mock_heading_error = 2 * sailbot_constants.MIN_HEADING_ERROR_FOR_SWITCH
fixedMode = ControlModes.TACKABLE.value
# Enter TACKABLE from UNKNOWN
hc = HeadingController(mock_speed,
ControlModes.UNKNOWN.value,
fixedControlMode=fixedMode)
self.assertEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
# Adjust parameters to switch to JIBE_ONLY
mock_speed = 0.5 * sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS
# A switch should NOT occur
self.assertFalse(
hc.switchControlMode(heading_error=mock_heading_error,
boat_speed=mock_speed))
# Should still be in TACKABLE mode
self.assertEqual(hc.getControlModeID(), ControlModes.TACKABLE.value)
self.assertTrue(hc.controlModeIsFixed)
def test_feedback_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)
heading_error = -1.2
# Generate List of evenly Spaced floats from -4pi to 4pi
testFloatList = []
for i in range(0, (int)(8 * math.pi / 0.01)):
testFloatList.append(-4 * math.pi + 0.01)
self.assertEqual(
hc.get_feed_back_gain(-1.2, 0),
sailbot_constants.KP /
(1 + sailbot_constants.CP * abs(heading_error)),
)
self.assertAlmostEqual(
hc.get_feed_back_gain(-1.2, math.pi),
sailbot_constants.MAX_ABS_RUDDER_ANGLE_RAD_IRONS /
(abs(heading_error) + 0.01),
)
# Test Symmetry of function for all possible cases
for windAngle in testFloatList:
for headingError in testFloatList:
self.assertAlmostEqual(
hc.get_feed_back_gain(headingError, windAngle),
hc.get_feed_back_gain(-headingError, windAngle),
)
# Ensure function is positive and works for all possible cases
for windAngle in testFloatList:
for headingError in testFloatList:
self.assertGreaterEqual(
hc.get_feed_back_gain(headingError, windAngle),
0,
)
import sailbot_constants
import rospy
import threading
from sailbot_msg.msg import actuation_angle, heading, Sensors
import math
lock = threading.Lock()
# define global variables for the needed topics
headingSetPointRad = None
headingMeasureRad = None
apparentWindAngleRad = None
groundspeedKnots = None
rudderAngleRad = 0
sailAngle = 0
jibAngle = 0
controller = HeadingController(boat_speed=0)
rudder_winch_actuation_angle_pub = rospy.Publisher(
"/rudder_winch_actuation_angle", actuation_angle, queue_size=1)
def sensorsCallBack(sensors_msg_instance):
lock.acquire()
global headingMeasureRad, apparentWindAngleRad, groundspeedKnots
headingMeasureRad = sensors_msg_instance.gps_can_true_heading_degrees * math.pi / 180
apparentWindAngleRad = sensors_msg_instance.wind_sensor_1_angle_degrees * math.pi / 180
groundspeedKnots = sensors_msg_instance.gps_can_groundspeed_knots
publishRudderWinchAngle()
lock.release()
def test_initialize_jibeMode(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS - 0.1
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value)
self.assertEqual(hc.getControlModeID(), ControlModes.JIBE_ONLY.value)
def test_badControlModeInput_ValueError(self):
with self.assertRaises(ValueError):
HeadingController(0.1, -1)
lock = threading.Lock()
# Global variables for configuring controller (should be command line arguments ideally)
DISABLE_LOW_POWER = False
"""
No Fixed State: None
Jibe: 1
Tack: 2
Low Power: 3
"""
FIXED_CTRL_STATE = None
WINCH_QUANTIZATION_PARAMETER = 4
# Boat control logic
controller = HeadingController(boat_speed=0,
disableLowPower=DISABLE_LOW_POWER,
fixedControlMode=FIXED_CTRL_STATE,
winchQuantParam=WINCH_QUANTIZATION_PARAMETER)
# Control node publisher
rudder_winch_actuation_angle_pub = rospy.Publisher(
"/rudder_winch_actuation_angle", actuation_angle, queue_size=1)
def windSensorCallBack(windsensor_msg_instance):
global apparentWindAngleRad
apparentWindAngleDegrees = windsensor_msg_instance.measuredBearingDegrees
apparentWindAngleRad = apparentWindAngleDegrees * sailbot_constants.DEGREES_TO_RADIANS
def gpsCallBack(gps_msg_instance):
global headingMeasureRad, groundspeedKnots