def __init__(self,
servo_roll_port="S0",
servo_tilt_port="S3",
name="head"):
self.name = name
self._roll_servo = ServoMotor(servo_roll_port)
self._tilt_servo = ServoMotor(servo_tilt_port)
self._head_roll_pid = PID(Kp=3.0,
Ki=1.5,
Kd=0.25,
setpoint=0,
output_limits=(-100, 100))
self.timeout = 5 # s
self.__shake_thread_control = ThreadControl()
self.__nod_thread_control = ThreadControl()
Stateful.__init__(self, children=["_roll_servo", "_tilt_servo"])
Recreatable.__init__(
self,
config_dict={
"servo_roll_port": servo_roll_port,
"servo_tilt_port": servo_tilt_port,
"name": name,
},
)
def __init__(self, port_name, zero_point=0, name="servo"):
self._pma_port = port_name
self.name = name
self.__controller = ServoController(self._pma_port)
self.__target_angle = 0.0
self.__target_speed = self.__DEFAULT_SPEED
self.__min_angle = self.__HARDWARE_MIN_ANGLE
self.__max_angle = self.__HARDWARE_MAX_ANGLE
self.__has_set_angle = False
self.__zero_point = zero_point
atexit.register(self.__cleanup)
Stateful.__init__(self)
Recreatable.__init__(
self,
config_dict={
"port_name": port_name,
"name": name,
"zero_point": lambda: self.zero_point,
},
)
def __init__(self, port_name, name="button"):
self._pma_port = port_name
self.name = name
Stateful.__init__(self)
Recreatable.__init__(self, {"port_name": port_name, "name": self.name})
gpiozero_Button.__init__(self, get_pin_for_port(self._pma_port))
def __init__(
self,
port_name,
forward_direction,
braking_type=BrakingType.BRAKE,
wheel_diameter=0.075,
name=None,
):
self.name = name
self._pma_port = port_name
self.__motor_core = EncoderMotorController(self._pma_port,
braking_type.value)
self.__forward_direction = forward_direction
self.__wheel_diameter = wheel_diameter
self.__prev_time_dist_cnt = time.time()
self.__previous_reading_odometer = 0
atexit.register(self.stop)
Stateful.__init__(self)
Recreatable.__init__(
self,
config_dict={
"port_name": port_name,
"name": name,
"forward_direction": lambda: self.forward_direction,
"braking_type": lambda: self.braking_type,
"wheel_diameter": lambda: self.wheel_diameter,
},
)
def __init__(self,
left_motor_port="M3",
right_motor_port="M0",
name="drive"):
self.name = name
# motor and wheel setup
self.left_motor_port = left_motor_port
self.right_motor_port = right_motor_port
self.left_motor = EncoderMotor(
port_name=self.left_motor_port,
forward_direction=ForwardDirection.CLOCKWISE)
self.right_motor = EncoderMotor(
port_name=self.right_motor_port,
forward_direction=ForwardDirection.COUNTER_CLOCKWISE,
)
# chassis setup
self.wheel_separation = 0.163
# Round down to ensure no speed value ever goes above maximum due to rounding issues (resulting in error)
self.max_motor_speed = (floor(
min(self.left_motor.max_speed, self.right_motor.max_speed) * 1000)
/ 1000)
self.max_robot_angular_speed = self.max_motor_speed / (
self.wheel_separation / 2)
# Target lock drive angle
self._linear_speed_x_hold = 0
self.__target_lock_pid_controller = PID(
Kp=0.045,
Ki=0.002,
Kd=0.0035,
setpoint=0,
output_limits=(-self.max_robot_angular_speed,
self.max_robot_angular_speed),
)
# Motor syncing
self.__mcu_device = PlateInterface().get_device_mcu()
self._set_synchronous_motor_movement_mode()
Stateful.__init__(self, children=["left_motor", "right_motor"])
Recreatable.__init__(
self,
config_dict={
"left_motor_port": left_motor_port,
"right_motor_port": right_motor_port,
"name": self.name,
},
)
def __init__(self, port_name, pin_number=1, name="adcbase"):
self._pma_port = port_name
self.name = name
self.is_current = False
self.channel = get_pin_for_port(self._pma_port, pin_number)
self.__adc_device = PlateInterface().get_device_mcu()
Stateful.__init__(self)
Recreatable.__init__(self, {
"port_name": port_name,
"pin_number": pin_number,
"name": self.name
})
def __init__(
self,
port_name,
queue_len=5,
max_distance=3,
threshold_distance=0.3,
partial=False,
name="ultrasonic",
):
assert port_name in Port
self._pma_port = port_name
self.name = name
if port_name in valid_analog_ports:
self.__ultrasonic_device = UltrasonicSensorMCU(
port_name=port_name,
queue_len=queue_len,
max_distance=max_distance,
threshold_distance=threshold_distance,
partial=partial,
name=name,
)
else:
self.__ultrasonic_device = UltrasonicSensorRPI(
port_name=port_name,
queue_len=queue_len,
max_distance=max_distance,
threshold_distance=threshold_distance,
partial=partial,
name=name,
)
self._in_range_function = None
self._out_of_range_function = None
Stateful.__init__(self)
Recreatable.__init__(
self,
{
"port_name": port_name,
"queue_len": queue_len,
"partial": partial,
"name": self.name,
"max_distance": lambda: self.max_distance,
"threshold_distance": lambda: self.threshold_distance,
},
)
def __init__(self, left_pincer_port="S3", right_pincer_port="S0", name="pincers"):
self.name = name
self._left_pincer = ServoMotor(left_pincer_port)
self._right_pincer = ServoMotor(right_pincer_port)
self.__left_pincer_setting = ServoMotorSetting()
self.__right_pincer_setting = ServoMotorSetting()
Stateful.__init__(self, children=["_left_pincer", "_right_pincer"])
Recreatable.__init__(
self,
config_dict={
"left_pincer_port": left_pincer_port,
"right_pincer_port": right_pincer_port,
"name": self.name,
},
)
def __init__(self, measurement_frequency, wheel_separation):
self._kalman_filter = KalmanFilter(dim_x=len(State), dim_z=2, dim_u=2)
self._velocities = deque(maxlen=2)
self._velocities.append(np.zeros((len(VelocityType), 1),
dtype=float)) # [v, w].
# Starting covariance is small since we know with high confidence we are starting at [0, 0, 0] with 0 velocity
self._kalman_filter.P = np.eye(5) * 1e-6
# Motor encoder has 115 RPM resolution. This is 2.75 RPM after dividing by gear ratio.
# Converting to m/s this is 0.01 m/s resolution (from wheel diameter of 0.0718)
# Divide by two to give a max error of +/- 0.005 m/s
# This would usually be taken as the 3*sigma value but given all the other errors in the system (wheel diameter,
# slippage etc) we'll use this value directly for sigma
linear_velocity_sigma = 0.005
# the Q matrix is the covariance of the expected state change over the time interval dt
Q_sigma = linear_velocity_sigma / (self._sigma_default_dt *
measurement_frequency)
self._kalman_filter.Q = np.diag([
Q_sigma**2,
Q_sigma**2,
math.radians(Q_sigma),
Q_sigma**2,
Q_sigma**2,
])
# State transition function for x1 = Fx0 + Bu0
self._kalman_filter.F = np.diag([1, 1, 1, 0, 0])
self._odom_linear_velocity_variance = linear_velocity_sigma**2
# angular velocity is calculated from motor velocities, maximum error is 0.005 * 2 across both wheel speeds
# divide by wheel separation to get resulting standard deviation for angular velocity
self._odom_angular_velocity_variance = (linear_velocity_sigma * 2 /
wheel_separation)**2
# Gyroscope resolution from datasheet is 1/131 = 0.0076 degrees/second +/- 1.5%
# Measure variance from stationary IMU is sigma**2 = 0.0018 over approx 200 samples
# IMU is likely to have a bias term associated and is less trustworthy than the odometry, use a larger value
# for now.
self._imu_angular_velocity_variance = np.radians(0.5**2)
Stateful.__init__(self, children=[])
def __init__(self,
servo_pan_port="S0",
servo_tilt_port="S3",
name="pan_tilt"):
self.name = name
self._pan_servo = ServoMotor(servo_pan_port)
self._tilt_servo = ServoMotor(servo_tilt_port)
self._object_tracker = PanTiltObjectTracker(
pan_servo=self._pan_servo, tilt_servo=self._tilt_servo)
Stateful.__init__(self, children=["_pan_servo", "_tilt_servo"])
Recreatable.__init__(
self,
config_dict={
"servo_pan_port": servo_pan_port,
"servo_tilt_port": servo_tilt_port,
"name": name,
},
)
def __init__(
self,
index=None,
resolution=(640, 480),
camera_type=CameraTypes.USB_CAMERA,
path_to_images="",
format="PIL",
flip_top_bottom: bool = False,
flip_left_right: bool = False,
rotate_angle=0,
name="camera",
):
# Initialise private variables
self._resolution = resolution
self._format = None
# Set format using setter property function
self.format = format
# Frame callback
self.on_frame = None
# Internal
self._index = index
self._camera_type = CameraTypes(camera_type)
self._path_to_images = path_to_images
self._rotate_angle = rotate_angle
self._flip_top_bottom = flip_top_bottom
self._flip_left_right = flip_left_right
if self._camera_type == CameraTypes.USB_CAMERA:
self.__camera = UsbCamera(
index=self._index,
resolution=self._resolution,
flip_top_bottom=flip_top_bottom,
flip_left_right=flip_left_right,
rotate_angle=self._rotate_angle,
)
elif self._camera_type == CameraTypes.FILE_SYSTEM_CAMERA:
self.__camera = FileSystemCamera(self._path_to_images)
self.__continue_processing = True
self.__frame_handler = FrameHandler()
self.__new_frame_event = Event()
self.__process_image_thread = Thread(
target=self.__process_camera_output, daemon=True)
self.__process_image_thread.start()
self.name = name
Stateful.__init__(self)
Recreatable.__init__(
self,
config_dict={
"index":
index,
"resolution":
resolution,
"camera_type":
camera_type.value
if isinstance(camera_type, Enum) else camera_type,
"path_to_images":
path_to_images,
"format":
format,
"name":
self.name,
"flip_top_bottom":
self._flip_top_bottom,
"flip_left_right":
self._flip_left_right,
"rotate_angle":
self._rotate_angle,
},
)