Ejemplo n.º 1
0
 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)
Ejemplo n.º 2
0
    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()
Ejemplo n.º 3
0
    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)
Ejemplo n.º 4
0
    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))
Ejemplo n.º 5
0
 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)
Ejemplo n.º 6
0
 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)
Ejemplo n.º 7
0
 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
Ejemplo n.º 8
0
 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
Ejemplo n.º 9
0
 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)
Ejemplo n.º 10
0
 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)
Ejemplo n.º 11
0
 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
Ejemplo n.º 12
0
 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
Ejemplo n.º 13
0
 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
Ejemplo n.º 14
0
 def deviation(self,heading,jitter):
   return abs(abs(angle_between(self.sensors.compass_heading_smoothed,heading)) - jitter)
Ejemplo n.º 15
0
 def angle(self):
     return to_360(angle_between(self._vehicle_bearing,self._abs_wind))
Ejemplo n.º 16
0
 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)
Ejemplo n.º 17
0
 def angle(self):
     return to_360(angle_between(self._vehicle_bearing, self._abs_wind))
Ejemplo n.º 18
0
 def _deviation(self,requested_heading,heading):
     return angle_between(heading,requested_heading)
Ejemplo n.º 19
0
 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)
Ejemplo n.º 20
0
 def deviation(self, heading, jitter):
     return abs(
         abs(angle_between(self.sensors.compass_heading_smoothed, heading))
         - jitter)
Ejemplo n.º 21
0
 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)