class DepthSensor:
def __init__(self):
self.sensor = ms5837.MS5837_30BA(
) # Default I2C bus is 1 (Raspberry Pi 3)
self.client = Client(ports.DEPTH_DRIVER_PORT)
self.logger = Logger(filename='depth')
def run(self):
# We must initialize the sensor before reading it
if not self.sensor.init():
print("Sensor could not be initialized")
exit(1)
# We have to read values from sensor to update pressure and temperature
if not self.sensor.read():
print("Sensor read failed!")
exit(1)
loop_condition = True
while loop_condition:
if self.sensor.read():
self.client.send_data(self.sensor.pressure() * SCALETOCM)
#print(self.sensor.pressure())
msg = str(self.sensor.depth())
self.logger.log(msg)
else:
loop_condition = False
def __del__(self):
pass
class DepthSensor:
def __init__(self):
self.sensor = ms5837.MS5837_02BA(
) # Default I2C bus is 1 (Raspberry Pi 3)
self.client = Client(DEPTH_DRIVER_PORT)
self.logger = DEFLOG.DEPTH_LOCAL_LOG
if self.logger:
self.logger = Logger(filename='depth_sensor',
directory=DEFLOG.LOG_DIRECTORY)
def run(self):
# We must initialize the sensor before reading it
if not self.sensor.init():
error = "Sensor could not be initialized"
self.log(error, 'error')
print(error)
exit(1)
# We have to read values from sensor to update pressure and temperature
if not self.sensor.read():
error = "Sensor read failed!"
self.log(error, 'error')
print(error)
exit(1)
loop_condition = True
while loop_condition:
if self.sensor.read():
self.client.send_data(self.sensor.depth())
msg = str(self.sensor.depth())
if self.logger:
self.logger.log(msg)
else:
loop_condition = False
def __del__(self):
self.logger.exit()
def log(self, msg, logtype='info'):
if self.logger:
self.logger.log(msg, logtype=logtype)
class Sonar:
def __init__(self, USB_device_adress=USB.SONAR):
self.logger = DEFLOG.DISTANCE_LOCAL_LOG
if DEFLOG.DISTANCE_LOCAL_LOG:
self.logger = Logger(filename='front_distance',
directory=DEFLOG.LOG_DIRECTORY)
self.sensor = Ping1D(USB_device_adress, 115200)
self.log("connected to the device")
self.client = Client(DISTANCE_DRIVER_PORT)
self.log("connected to the server")
def run(self):
# We must initialize the sensor before reading it
if not self.sensor.initialize():
error = "Sensor could not be initialized"
print(error)
self.log(error, 'error')
exit(1)
loop_condition = True
while loop_condition:
data = self.sensor.get_distance()
if data:
self.client.send_data(data["distance"] / 10)
msg = str(data["distance"])
self.log(msg)
else:
loop_condition = False
time.sleep(0.035)
def log(self, msg, logtype='info'):
if self.logger:
self.logger.log(msg, logtype=logtype)
def __del__(self):
pass
if self.logger:
self.logger.exit()
class LightHandling:
def __init__(self):
self.pi = pigpio.pi()
self.logger = Logger(filename='light_handling', directory='logs/electro/')
if not self.pi.connected:
self.logger.log("pi not connected", logtype='error')
exit()
self.logger.log("setup")
self.setup()
def setup(self):
self.pi.set_PWM_frequency(PWM_lights, 400)
self.pi.set_PWM_range(PWM_lights, OFF) # 550 - OFF, 950 - ON_FULL
self.pi.set_PWM_dutycycle(PWM_lights, OFF)
def lights_set_brightness(self,x):
if x < 0: x = 0
if x > 100: x = 100
x=100-x
self.pi.set_PWM_dutycycle(PWM_lights, -8*x+1400)
self.logger.log("lights' brightness: " + str(x) + "%")
class AHRS_Separate():
yaw = 0
pitch = 0
roll = 0
free_acc = [0, 0, 0]
rate_of_turn = [0, 0, 0]
def __init__(self, log_direcory=""):
self.serial = serial.Serial(IMU_PORT, 115200, stopbits=2, parity=serial.PARITY_NONE)
self.lock_rate_of_turn = threading.Lock()
self.lock_free_acc = threading.Lock()
self.lock_angular_pos = threading.Lock()
self.logger = Logger(filename='ahrs_separate', directory=log_direcory, logtype='info',
logformat='[{timestamp}] {logtype}: {message}',
title='AHRS logger', logexists='append', console=False)
self.close_order = False
self.yaw_correction = 0.0
self.previous_yaw = 0.0
def get_message(self):
MID = 0
data = 0
ser = self.serial
while True:
pre = ser.read()
if pre.hex() == 'fa': # check preamble
bid = ser.read()
checksum = bid[0]
if bid.hex() == 'ff': # check bus ID
MID = ser.read(1)[0]
checksum += MID
len = ser.read()
len_value = len[0]
checksum += len_value
if len.hex() == 'ff': # extended message
len_ext = ser.read(2)
len_value = int.from_bytes(len_ext, 'big')
checksum += len_ext[0] + len_ext[1]
data = ser.read(len_value)
for byte in data:
checksum += byte
checksum += ser.read()[0]
if checksum & 0xff == 0: # Checksum OK!
break
self._interpret_message(MID, data)
self.should_fix()
def _interpret_euler(self, data: BytesQueue):
length = data.pop()
if length != 12:
print("Unexpected euler angles data length")
roll_bytes = data.pop(4)
pitch_bytes = data.pop(4)
yaw_bytes = data.pop(4)
with self.lock_angular_pos:
self.roll = struct.unpack(">f", roll_bytes)[0]
self.pitch = struct.unpack(">f", pitch_bytes)[0]
self.yaw = struct.unpack(">f", yaw_bytes)[0]
def _interpret_free_acceleration(self, data: BytesQueue):
length = data.pop()
if length != 12:
print("Unexpected free acceleration data length")
accX_bytes = data.pop(4)
accY_bytes = data.pop(4)
accZ_bytes = data.pop(4)
with self.lock_free_acc:
self.free_acc[0] = struct.unpack(">f", accX_bytes)[0]
self.free_acc[1] = struct.unpack(">f", accY_bytes)[0]
self.free_acc[2] = struct.unpack(">f", accZ_bytes)[0]
def _interpret_rate_of_turn(self, data: BytesQueue):
length = data.pop()
if length != 12:
print("Unexpected rate of turn data length")
rotX_bytes = data.pop(4)
rotY_bytes = data.pop(4)
rotZ_bytes = data.pop(4)
with self.lock_rate_of_turn:
self.rate_of_turn[0] = struct.unpack(">f", rotX_bytes)[0]
self.rate_of_turn[1] = struct.unpack(">f", rotY_bytes)[0]
self.rate_of_turn[2] = struct.unpack(">f", rotZ_bytes)[0]
def _interpret_mtdata2(self, data: BytesQueue):
while data.length() > 0:
data_type = data.pop(2)
if data_type.hex() == "2030":
self._interpret_euler(data)
elif data_type.hex() == "4030":
self._interpret_free_acceleration(data)
elif data_type.hex() == "8020":
self._interpret_rate_of_turn(data)
else:
# print("Unexpected mtdata2 information type: {hex}".format(hex=data_type.hex()))
return
def _interpret_message(self, mid: int, data: bytes):
data_queue = BytesQueue(data)
if mid == 0x36:
self._interpret_mtdata2(data_queue)
else:
print("WARNING: Unknown message type")
def get_data(self):
"""
To call from oudstide the function
:return: {'lineA_x':0,'lineA_y':0,'lineA_z':0, 'angularA_x':0,'angularA_y':0,'angularA_z':0, 'yaw':0,'pitch':0,'roll':0}
"""
data = {}
with self.lock_free_acc:
data['lineA_x'] = (self.free_acc[0])
data['lineA_y'] = (self.free_acc[1])
data['lineA_z'] = (self.free_acc[2])
with self.lock_angular_pos:
data['yaw'] = (self.yaw)
data['pitch'] = (self.pitch)
data['roll'] = (self.roll)
with self.lock_rate_of_turn:
data['angularA_x'] = (self.rate_of_turn[0])
data['angularA_y'] = (self.rate_of_turn[1])
data['angularA_z'] = (self.rate_of_turn[2])
return data
def run(self):
while not self.close_order:
self.get_message()
def close(self):
self.close_order = True
@staticmethod
def isAHRSconected():
'''
:return: True if you can use AHRS or False if you can't
'''
return os.path.exists(IMU_PORT)
def should_fix(self):
LIMIT = 175.0
if self.yaw > LIMIT and self.previous_yaw < -LIMIT:
self.yaw_correction -= 360.0
elif self.yaw < -LIMIT and self.previous_yaw > LIMIT:
self.yaw_correction += 360.0
self.logger.log("prev: "+str(self.previous_yaw)+" curr: "+str(self.yaw))
self.previous_yaw = self.yaw
class CameraClient:
def __init__(self, host=IP_ADDRESS, port=CAMERA_SERVER_PORT, retry_no=5, name_modifer = "", logs=True):
"""
Initialize Camera Client Class
:param host: [String] Server host
:param port: [Int] Server port
:param retry_no: [Int] Number of retries
"""
self.host = host
self.port = port
self.retryNo = retry_no
# set logger file
self.logs = logs
if self.logs:
self.logger = Logger(filename='camera_client'+name_modifer, title="CameraClient", directory=LOG_DIRECOTRY, logexists='append')
self.logger.start()
if not self.__auto_retry(self.__create_socket()):
if self.logs:
self.logger.log(f"ERROR: Create socket failure")
return
if not self.__auto_retry(self.__connect_to_server()):
if self.logs:
self.logger.log(f"ERROR: Connect to server failure")
return
def __create_socket(self):
"""
Create socket for making connection possible
:return: [Bool] True if successful
"""
try:
self.socket = socket.socket()
return True
except socket.error as msg:
if self.logs:
self.logger.log(f'WARNING: Socket creation error: {msg}.')
return False
def __connect_to_server(self):
"""
Establish connection to server
:return: True if successful
"""
try:
if self.logs:
self.logger.log(f"Connecting the port {self.host}:{self.port}")
self.socket.connect((self.host, self.port))
return True
except socket.error as msg:
if self.logs:
self.logger.log(f"WARNING: Socket binding error: {msg}.")
return False
def __auto_retry(self, function):
"""
Auto-retry function
:param function: [Function] Function to try. Require False/0 if unsuccessful
:return: True if finished with retry number of tries
"""
for i in range(self.retryNo):
if function:
return True
elif i == self.retryNo:
if self.logs:
self.logger.log("ERROR: Initialize error. Check logs for more info.")
return False
if self.logs:
self.logger.log(f"Retrying. Try: {i} of {self.retryNo}.")
@property
def frame(self):
"""
Get frame from server
:return: Frame of image
"""
limit = struct.calcsize(">L")
data = b""
self.socket.send("get_frame".encode())
while len(data) < limit:
data += self.socket.recv(4096)
packed_msg_size = data[:limit]
msg_size = struct.unpack(">L", packed_msg_size)[0]
data = data[limit:]
while len(data) < msg_size:
data += self.socket.recv(4096)
frame_data = data[:msg_size]
# data = data[msg_size:]
frame = pickle.loads(frame_data, fix_imports=True, encoding="bytes")
return frame
def change_camera(self,id):
self.socket.send(("change_to:{}".format(id)).encode())
confirmation = self.socket.recv(4096).decode()
if confirmation == 'true':
return 'true'
else:
return 'false'
class Main():
'''
Creates object of all sensor types, packs their references into
a list. Creates Communication thread.
'''
def __init__(self):
'''
Creates and stores references of all slave objects.
'''
self.logger = Logger(filename='main_xavier',
title="Main Xavier",
directory=LOG_DIRECOTRY)
self.logger.start()
# cameras
self.camera_client = CameraClient(IP_ADDRESS, name_modifer="_main")
self.logger.log("Cameras created")
#communication
self.communication = Communication()
self.rpi_reference = self.communication.rpi_reference
self.logger.log("communication was established")
# sensors
self.ahrs = AHRS(self.rpi_reference)
self.depth_sensor = DepthSensor(self.rpi_reference)
self.distance_sensor = DistanceSensor(self.rpi_reference)
self.hydrophones = Hydrophones(self.rpi_reference)
self.logger.log("sensors created")
self.sensors = {
'ahrs': self.ahrs,
'depth': self.depth_sensor,
'distance': self.distance_sensor,
'hydrophones': self.hydrophones
}
#control
self.movements = Movements(self.rpi_reference, self.logger)
#self.torpedoes = Torpedoes(self.rpi_reference)
#self.manipulator = Manipulator(self.rpi_reference)
self.dropper = Dropper(self.rpi_reference)
self.logger.log("control objects created")
self.control = {
'movements': self.movements,
#torpedoes': self.torpedoes,
#'manipulator': self.manipulator,
'dropper': self.dropper
}
# task sheduler
self.task_scheduler = TaskSchedululer(self.control, self.sensors,
self.logger)
self.logger.log("Task scheduler created")
def run(self):
self.logger.log("main thread is running")
self.task_scheduler.run()
'--threshold',
default=0.5,
type=float,
help="Detection threshold (from 0 to 1)")
args = parser.parse_args()
server = Flask(__name__)
cam_client = CameraClient(host=str(os.system('hostname -I')))
model = DarknetYoloModel(
model_path=f"{PROJECT_ROOT}/neural_networks/models",
threshold=args.threshold)
model.load(model_name='gate')
logger.log("Model loaded to server")
predict_time = 0
@server.route("/predict_with_image", methods=["GET"])
def predict_with_image():
start = time.time()
img = cam_client.frame
getting_frame = time.time() - start
start = time.time()
result = model.predict(img)
global predict_time
predict_time = time.time() - start
start = time.time()
result.append(img)
result = pickle.dumps(result)
class ServerXavier:
def __init__(self, host=str(os.system('hostname -I')), port=CAMERA_SERVER_PORT, black_and_white=False, retry_no=5):
"""
Initialize server
:param host: [String] host address
:param port: [Int] port
:param black_and_white: [Bool] Is white and white camera image?
:param retry_no: [Int] Number of retries
"""
self.host = host
self.port = port
self.bw = black_and_white
self.retryNo = retry_no
# set logger file
self.logger = Logger(filename='server_xavier', title="ServerXavier", directory=LOG_DIRECOTRY, logexists='append')
self.logger.start()
# start up camera
front_camera_connected = False
bottom_camera_connected = False
try:
front_camera = cv2.VideoCapture(CAMERAS.FRONT_CAMERA_DEVNAME)
front_camera_connected = True
except:
self.logger.log("Front camera not connected")
try:
bottom_camera = cv2.VideoCapture(CAMERAS.BOTTOM_CAMERA_DEVNAME)
bottom_camera_connected = True
except:
self.logger.log("Bottom camera not connected")
if front_camera_connected and bottom_camera_connected:
self.camerasDict = {"front": front_camera,"bottom": bottom_camera}
self.cameraCapture = self.camerasDict["front"]
elif front_cammera_connected:
self.camerasDict = {"front": front_camera}
self.cameraCapture = self.camerasDict["front"]
elif bottom_camera_connected:
self.camerasDict = {"bottom": bottom_camera}
self.cameraCapture = self.camerasDict["front"]
else:
self.print("No camera connected")
self.logger.log("No camera connected")
if not self.__auto_retry(self.__create_socket()):
self.logger.log(f"ERROR: Create socket failure")
return
if not self.__auto_retry(self.__bind_socket()):
self.logger.log(f"ERROR: Bind socket failure")
return
self.logger.log(f"Init complete")
# variables for videoClient use
self.VIDEO_PATH = ''
self.VIDEO = None
self.VIDEO_FRAME_COUNT = 0
def __create_socket(self):
"""
Create socket for making connection possible
:return: [Bool] True if successful
"""
try:
self.socket = socket.socket()
return True
except socket.error as msg:
self.logger.log(f'WARNING: Socket creation error: {msg}.')
return False
def __bind_socket(self):
"""
Bind selected socket and listen for connections
:return: [Bool] True if successful
"""
try:
self.logger.log(f"Binding the Port {self.port}")
self.socket.bind((self.host, self.port))
self.socket.listen(5)
return True
except socket.error as msg:
self.logger.log(f"WARNING: Socket binding error: {msg}.")
return False
def __auto_retry(self, function):
"""
Auto-retry function
:param function: [Function] Function to try. Require False/0 if unsuccessful
:return: True if finished with retry number of tries
"""
for i in range(self.retryNo):
if function:
return True
elif i == self.retryNo:
self.logger.log("ERROR: Initialize error. Check logs for more info.")
return False
self.logger.log(f"Retrying. Try: {i} of {self.retryNo}.")
def socket_accept(self):
"""
Accept connection from client
:return: None
"""
conn, address = self.socket.accept()
self.logger.log(f"Connection has been established! | {address[0]}:{address[1]}")
threading.Thread(target=self.__handle_client, args=(conn,)).start()
def change_camera(self, id):
if id in self.camerasDict.keys():
self.cameraCapture = self.camerasDict[id]
return True
else:
return False
def __handle_client(self, conn):
"""
Handle client in separate function
:param conn: Client connection data
:return: None
"""
counter = 1
while True:
data = conn.recv(1024).decode()
if not data:
break
elif "change" in data:
if self.change_camera(data.split(':')[1]):
conn.send('true'.encode())
else:
conn.send('false'.decode())
elif "get_frame" in data:
conn.send(self.__frame(source=self.cameraCapture, h_flip=True))
elif "get_video" in data:
# set video variables
if counter == 1:
self.VIDEO_PATH = data.split()[1]
self.VIDEO = cv2.VideoCapture(self.VIDEO_PATH)
self.VIDEO_FRAME_COUNT = self.VIDEO.get(7)
if counter < self.VIDEO_FRAME_COUNT:
conn.send(self.__frame(source=self.VIDEO))
counter += 1
else:
self.VIDEO.release()
self.VIDEO = cv2.VideoCapture(self.VIDEO_PATH)
conn.send(self.__frame(source=self.VIDEO))
counter = 2
conn.close()
def __frame(self, source, v_flip=False, h_flip=False):
"""
Get picture frame
:param v_flip: [Bool] Is image flipped vertical
:param h_flip: [Bool] Is image flipped horizontal
:return: frame
"""
# Capture frame
ret, frame = source.read()
# Handles the mirroring of the current frame
frame = cv2.flip(frame, self.__flip(v_flip, h_flip))
if self.bw:
# Change color to black and white if decided
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Code image
result, frame = cv2.imencode('.jpg', frame)
data = pickle.dumps(frame, 0)
size = len(data)
return struct.pack(">L", size) + data
def __flip(self, v_flip, h_flip):
"""
Get flip parameter
:param v_flip: [Bool] Is image flipped vertical
:param h_flip: [Bool] Is image flipped horizontal
:return: [Int] value for cv2 flip method
"""
if h_flip and v_flip:
return -1
elif v_flip:
return 0
else:
return 1
directory=DEFLOG.LOG_DIRECTORY)
engine_slave = rov_comm.Client(ports.ENGINE_MASTER_PORT)
if MODE == 'ROV4':
spi = spidev.SpiDev()
spi.open(0, 0)
spi.max_speed_hz = 15600000
else:
engine_driver = EngineDriver()
print("WIP")
while True:
try:
movements = engine_slave.get_data() #robimy z byte na movements
if logger:
logger.log("movements: " + str(movements))
if MODE == 'ROV4':
powers = compute_power_rov4(movements['front'],
movements['right'],
movements['up'], movements['yaw'])
else:
powers = compute_power_rov3(movements['front'],
movements['right'], 0,
movements['yaw'])
if logger:
logger.log("engines: " + str(powers))
time.sleep(0.05)
if MODE == 'ROV4':
set_engines(powers)
class DarknetClient():
"""
Class for interacting witch python darknet server.
"""
def __init__(self, url= "http://192.168.0.103"):#url=f"http://localhost"):
"""
:param port: Port of running darknet server
:param url: Url of running darknet server external eg: "http://192.168.0.104"
"""
with open('ports.txt','r') as f:
self.port = str(int(f.read())+2)
print("Darknet client port",self.port)
self.url = url
self.logger = Logger(filename='darknet_client', title="Darknet_Client", directory=LOG_DIRECOTRY, logexists='append')
self.logger.log(url)
def load_model(self, model_name, retries=3) -> bool:
"""
Sends a post request to darknet server, and load new model from memory
:param model_name: string with dir name that contains trained model
:param retries: max retries before giving up
:return: Response from darknet server. True if operation succeeded
"""
data = {'model_name': str(model_name)}
for i in range(retries):
try:
server_url = self.url + ":" + self.port + "/load_model"
req = requests.post(url=server_url,data = data)
result = req.content
self.logger.log("Model loaded correctly")
break
except Exception as e:
time.sleep(0.1)
result = False
self.logger.log(str(e))
return result
def change_camera(self,camera,retries = 3) -> bool:
"""
Sends a post requests to darknet server, and change source camera
from with server takes frames.
:param camera: name of camera in string that specifies source eg: "bottom"
:param retries: max retries before giving up
:return: Response from darknet server. True if operation succeeded
"""
data = {'cam_name': str(camera)}
for i in range(retries):
try:
server_url = self.url + ":" + self.port + "/change_camera"
req = requests.post(url=server_url,data = data)
result = req.content
break
except Exception:
time.sleep(0.1)
result = False
return result
def predict(self):
"""
Request prediction from darknet server, and get BoudingBox
:return: Response from darknet server contains bbox structure
"""
server_url = self.url + ":" + self.port + "/predict"
req = requests.get(url=server_url)
result = req.content
result = pickle.loads(result)
self.logger.log("Img predict")
if result:
self.logger.log(str(result[0].normalize(480,480)))
return result
def predict_with_image(self):
"""
Request prediction from darknet server. When server get BoudingBox with image
:return: Response from darknet server contains bbox structure and image
"""
server_url = self.url + ":" + self.port + "/predict_with_image"
req = requests.get(url=server_url)
result = req.content
result = pickle.loads(result)
return result
def prediction_time(self):
"""
Request prediction time from darknet server
:return: Response from darknet server time of prediction
"""
server_url = self.url + ":" + self.port + "/prediction_time"
req = requests.get(url=server_url)
result = req.content
return float(result)
def change_threshold(self, threshold = 0.5, retries = 3) -> bool:
"""
Change detection threshold from 0 to 1.
:return: Response from darknet server. True if operation succeeded
"""
data = {'threshold': str(threshold)}
for i in range(retries):
try:
server_url = self.url + ":" + self.port + "/change_threshold"
req = requests.post(url=server_url,params = data)
result = req.content
break
except Exception:
time.sleep(0.1)
result = False
return result
class TorpedoHandling:
def __init__(self):
self.pi = pigpio.pi()
self.logger = Logger(filename='torpedoes_handling')
if not self.pi.connected:
self.logger.log("pi not connected", logtype='error')
exit()
self.logger.log("setup")
self.setup()
def setup(self):
self.pi.set_mode(reed_switch, pigpio.INPUT)
self.pi.set_mode(trigger, pigpio.OUTPUT)
self.pi.set_mode(en, pigpio.OUTPUT)
self.pi.write(en, 1)
self.pi.set_PWM_frequency(PWM_serwo, 330)
self.pi.set_PWM_range(PWM_serwo,
1000) # 270 max_cw, 730 max_ccw, 500 neutral
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL)
def initial_rotation(self):
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL + FAST_ROTATION)
time.sleep(TIME_OF_INITIAL_ROTATION)
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL)
def position(self):
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL + FAST_ROTATION)
while self.pi.read(reed_switch) == 0:
pass
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL + SLOW_ROTATION)
while self.pi.read(reed_switch) == 1:
pass
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL - SLOW_ROTATION)
while self.pi.read(reed_switch) == 0:
pass
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL)
def fire(self):
self.pi.write(trigger, 1)
time.sleep(DELAY_AFTER_TRIGGER)
self.pi.write(trigger, 0)
def stop(self):
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL)
self.pi.write(en, 0)
def sequence(self):
#self.pi.set_PWM_dutycycle(PWM_serwo, 300)
self.logger.log("initial rotation")
self.initial_rotation() # TODO move to __init__
self.logger.log("position")
self.position()
self.logger.log("fire")
self.fire()
self.logger.log("stop")
self.stop()
self.logger.log("koniec")
self.pi.set_PWM_dutycycle(PWM_serwo, 0)
self.pi.stop()