def test_heading_should_be_positive_for_180_turns(self):
self.assertEqual(angle_between(1,181),180)
self.assertEqual(angle_between(181,1),180)
self.assertEqual(angle_between(180,0),180)
self.assertEqual(angle_between(0,180),180)
self.assertEqual(angle_between(90,270),180)
self.assertEqual(angle_between(270,90),180)
self.assertEqual(angle_between(360,180),180)
self.assertEqual(angle_between(360,180),180)
self.assertEqual(angle_between(179,359),180)
self.assertEqual(angle_between(359,179),180)
def __init__(self,gps=False,servo_port=SERVO_PORT):
# devices
self.compass = Compass(I2C(COMPASS_I2C_ADDRESS),I2C(ACCELEROMETER_I2C_ADDRESS))
self.rudder_servo = Servo(serial.Serial(servo_port),RUDDER_SERVO_CHANNEL,RUDDER_MIN_PULSE,RUDDER_MIN_ANGLE,RUDDER_MAX_PULSE,RUDDER_MAX_ANGLE)
try:
while True:
current_bearing = self.compass.bearing
difference = angle_between(TARGET_BEARING, current_bearing)
# for definite rudder deflection at low angles for difference:
# scaled_diff = max(5, abs(difference/3))
# rudder_angle = copysign(scaled_diff,difference)
# for tolerance of small differences
rudder_angle = difference/3 if abs(difference) > 5 else 0
self.rudder_servo.set_position(rudder_angle)
print('Current bearing {:+.1f}, target {:+.1f}, rudder {:+.1f}'.format(current_bearing, TARGET_BEARING, rudder_angle))
# smooth response with a sleep time of less than 100ms ?
time.sleep(0.1)
except (KeyboardInterrupt, SystemExit):
quit()
def steer_course(self,requested_heading,for_seconds):
wind_course_angle = angle_between(requested_heading, self.sensors.wind_direction)
if abs(wind_course_angle) <= self.no_go:
self._tack(requested_heading, wind_course_angle, for_seconds)
else:
self.helm.steer_course(requested_heading, for_seconds, self.no_go)
def _tack_if_needed(self,requested_heading, time_to_next_review):
wind_course_angle = angle_between(requested_heading, self.sensors.wind_direction_abs_average)
if abs(wind_course_angle) <= self.no_go:
self._start_tack(requested_heading, wind_course_angle, time_to_next_review)
else:
self.exchange.publish(Event(EventName.set_course, heading=requested_heading))
def _steer_avoiding_no_go_area(self, heading, no_go_angle):
heading_wind_diff = angle_between(heading, self.sensors.wind_direction)
if abs(heading_wind_diff) <= no_go_angle:
self.helm.steer(heading - copysign(
no_go_angle - abs(heading_wind_diff), heading_wind_diff))
else:
self.helm.steer(heading)
def check_course(self, unused_check_course_event):
if self.turning:
return
heading = self.sensors.compass_heading_average
rate_of_turn = self.sensors.rate_of_turn_average
if abs(angle_between(heading, self.requested_heading)) > self.on_course_threshold:
self._start_turning()
self.steerer.steer(self.requested_heading, heading, rate_of_turn, self.reduction_factor)
def _calculate_rate_of_turn(self,bearing):
time_now = self.system_time()
if time_now <= self._previous_time:
self._rate_of_turn = 0
else:
self._rate_of_turn = angle_between(self._previous_bearing,bearing)/(time_now - self._previous_time)
self._previous_bearing = bearing
self._previous_time = time_now
def _calculate_rate_of_turn(self,bearing):
time_now = self.system_time()
if time_now <= self._previous_time:
self._rate_of_turn = 0
else:
rate_of_turn = angle_between(self._previous_compass_smoothed, bearing) / (time_now - self._previous_time)
self._rate_of_turn = copysign(min(MAX_RATE_OF_TURN,abs(rate_of_turn)),rate_of_turn)
self._previous_compass_smoothed = bearing
self._previous_time = time_now
def test_heading_difference_should_produce_value_between_zero_and_180_when_turning_right_across_due_north(self):
self.assertEqual(angle_between(270,0),90)
self.assertEqual(angle_between(359,1),2)
self.assertEqual(angle_between(359,178),179)
self.assertEqual(angle_between(181,0),179)
self.assertEqual(angle_between(181,360),179)
self.assertEqual(angle_between(181,270),89)
self.assertEqual(angle_between(270,89),179)
def test_heading_difference_should_produce_value_between_zero_and_minus_180_when_turning_left(self):
self.assertEqual(angle_between(0,0),0)
self.assertEqual(angle_between(1,0),-1)
self.assertEqual(angle_between(90,0),-90)
self.assertEqual(angle_between(180,1),-179)
self.assertEqual(angle_between(359,270),-89)
self.assertEqual(angle_between(359,180),-179)
self.assertEqual(angle_between(360,270),-90)
def _calculate_rate_of_turn(self, bearing):
time_now = self.system_time()
if time_now <= self._previous_time:
self._rate_of_turn = 0
else:
rate_of_turn = angle_between(
self._previous_compass_smoothed,
bearing) / (time_now - self._previous_time)
self._rate_of_turn = copysign(
min(MAX_RATE_OF_TURN, abs(rate_of_turn)), rate_of_turn)
self._previous_compass_smoothed = bearing
self._previous_time = time_now
def __init__(self,gps,wind_direction,compass_heading):
self.gps = gps
self.time = 0
self.speed = 1
self.track_error = 1
self.speed_error = 0.1
self.rate_of_turn = 1
self.rate_of_turn_average = 1
self.wind_direction_abs_average = wind_direction
self.wind_direction_relative_instant = to_360(angle_between(wind_direction,compass_heading))
self.wind_direction_relative_average = self.wind_direction_relative_instant
self.compass_heading_average = compass_heading
self.compass_heading_smoothed = compass_heading
def __init__(self, gps, wind_direction, compass_heading):
self.gps = gps
self.time = 0
self.speed = 1
self.track_error = 1
self.speed_error = 0.1
self.rate_of_turn = 1
self.rate_of_turn_average = 1
self.wind_direction_abs_average = wind_direction
self.wind_direction_relative_instant = to_360(
angle_between(wind_direction, compass_heading))
self.wind_direction_relative_average = self.wind_direction_relative_instant
self.compass_heading_average = compass_heading
self.compass_heading_smoothed = compass_heading
def deviation(self,heading,jitter): return abs(abs(angle_between(self.sensors.compass_heading_smoothed,heading)) - jitter)
def angle(self):
return to_360(angle_between(self._vehicle_bearing,self._abs_wind))
def _steer_avoiding_no_go_area(self, heading, no_go_angle):
heading_wind_diff = angle_between(heading, self.sensors.wind_direction)
if abs(heading_wind_diff) <= no_go_angle:
self.helm.steer(heading - copysign(no_go_angle - abs(heading_wind_diff), heading_wind_diff))
else:
self.helm.steer(heading)
def angle(self):
return to_360(angle_between(self._vehicle_bearing, self._abs_wind))
def _deviation(self,requested_heading,heading):
return angle_between(heading,requested_heading)
def test_heading_difference_should_produce_value_between_zero_and_minus_180_when_turning_left_across_due_north(self):
self.assertEqual(angle_between(0,359),-1)
self.assertEqual(angle_between(0,270),-90)
self.assertEqual(angle_between(1,270),-91)
self.assertEqual(angle_between(180,1),-179)
self.assertEqual(angle_between(1,182),-179)
def deviation(self, heading, jitter):
return abs(
abs(angle_between(self.sensors.compass_heading_smoothed, heading))
- jitter)
def test_heading_difference_should_produce_value_between_zero_and_180_when_turning_right(self):
self.assertEqual(angle_between(0,1),1)
self.assertEqual(angle_between(0,90),90)
self.assertEqual(angle_between(1,180),179)
self.assertEqual(angle_between(181,0),179)
self.assertEqual(angle_between(300,320),20)