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, }, )