diff --git a/camera/raspberry/client.py b/camera/raspberry/client.py
new file mode 100644
index 0000000..1c84723
--- /dev/null
+++ b/camera/raspberry/client.py
@@ -0,0 +1,36 @@
+import socket
+import subprocess
+#from picamera2 import Picamera2
+import numpy
+import time
+
+TCP_IP = '192.168.1.2'
+TCP_PORT = 5001
+
+sock = socket.socket()
+sock.connect((TCP_IP, TCP_PORT))
+
+#picam2 = PiCamera()
+
+#config = picam2.create_preview_configuration({"size": (640, 480)})
+#picam2.configure(config)
+#picam2.start()
+#array = picam2.capture_array("main")
+while(True):
+    #array = picam2.capture_array("main")
+
+    #ock.send( str(len(array)).ljust(16))
+    #sock.send( array )
+    #time.sleep(0.070)
+    command = sock.recv(1)
+    command = command.decode()
+    if command == "n":
+        p = subprocess.Popen("./sensors/sensors", stdout=subprocess.PIPE, shell=True)
+        (output, err) = p.communicate()
+        print(output)
+        print(len(output))
+        sock.send(str(len(output)).encode())
+        sock.send(output)
+
+
+sock.close()
diff --git a/camera/raspberry/focus.sh b/camera/raspberry/focus.sh
new file mode 100644
index 0000000..879d5be
--- /dev/null
+++ b/camera/raspberry/focus.sh
@@ -0,0 +1 @@
+python3 ~/Arducam-Pivariety-V4L2-Driver/focus/FocuserExample.py -d /dev/v4l-subdev1 --focus-step 10
diff --git a/camera/raspberry/lights/Makefile b/camera/raspberry/lights/Makefile
new file mode 100644
index 0000000..4c082b0
--- /dev/null
+++ b/camera/raspberry/lights/Makefile
@@ -0,0 +1,40 @@
+CC = g++
+
+OPT = -O3
+FLAGS = -std=c++20 -Wall -Wextra -pedantic
+LIBS = -lwiringPi
+
+TARGET = bbx-test
+SOURCEDIR = src
+BUILDDIR = build
+
+SOURCES = $(wildcard $(SOURCEDIR)/*.cpp)
+OBJECTS = $(patsubst $(SOURCEDIR)/%.cpp,$(BUILDDIR)/%.o,$(SOURCES))
+
+.PHONY: all clean depend run
+
+all: $(TARGET)
+
+$(BUILDDIR):
+	@mkdir -p $(BUILDDIR)
+
+$(TARGET): $(OBJECTS)
+	$(CC) $(FLAGS) $(OPT) $^ -o $@ $(LIBS)
+
+$(OBJECTS): $(BUILDDIR)/%.o :  $(SOURCEDIR)/%.cpp $(SOURCEDIR)/%.hpp | $(BUILDDIR)
+	$(CC) $(FLAGS) $(OPT) -c $< -o $@ $(LIBS)
+
+depend: .depend
+
+.depend: $(SOURCES)
+	rm -rf $(BUILDDIR)/.depend
+	$(CXX) $(FLAGS) -MM $^ -MF $(BUILDDIR)/.depend
+
+clean:
+	rm -rf $(BUILDDIR)
+	rm -f $(TARGET)
+
+run: $(TARGET)
+	sudo ./$(TARGET)
+
+-include $(BUILDDIR)/.depend
diff --git a/camera/raspberry/lights/build/LEDs.o b/camera/raspberry/lights/build/LEDs.o
new file mode 100644
index 0000000..3dc0eba
Binary files /dev/null and b/camera/raspberry/lights/build/LEDs.o differ
diff --git a/camera/raspberry/lights/build/adxl345.o b/camera/raspberry/lights/build/adxl345.o
new file mode 100644
index 0000000..248fede
Binary files /dev/null and b/camera/raspberry/lights/build/adxl345.o differ
diff --git a/camera/raspberry/lights/build/demo.o b/camera/raspberry/lights/build/demo.o
new file mode 100644
index 0000000..a95b1af
Binary files /dev/null and b/camera/raspberry/lights/build/demo.o differ
diff --git a/camera/raspberry/lights/build/fan.o b/camera/raspberry/lights/build/fan.o
new file mode 100644
index 0000000..0ce7f2d
Binary files /dev/null and b/camera/raspberry/lights/build/fan.o differ
diff --git a/camera/raspberry/lights/build/sick_front.o b/camera/raspberry/lights/build/sick_front.o
new file mode 100644
index 0000000..e05040d
Binary files /dev/null and b/camera/raspberry/lights/build/sick_front.o differ
diff --git a/camera/raspberry/lights/build/sick_side.o b/camera/raspberry/lights/build/sick_side.o
new file mode 100644
index 0000000..df4ba6d
Binary files /dev/null and b/camera/raspberry/lights/build/sick_side.o differ
diff --git a/camera/raspberry/lights/lights b/camera/raspberry/lights/lights
new file mode 100644
index 0000000..65c5e16
Binary files /dev/null and b/camera/raspberry/lights/lights differ
diff --git a/camera/raspberry/lights/src/LEDs.cpp b/camera/raspberry/lights/src/LEDs.cpp
new file mode 100644
index 0000000..de803b1
--- /dev/null
+++ b/camera/raspberry/lights/src/LEDs.cpp
@@ -0,0 +1,16 @@
+#include "LEDs.hpp"
+
+LEDs::LEDs(){
+    wiringPiSetup();
+    pinMode(GPIO_LED_left, PWM_OUTPUT);
+    pinMode(GPIO_LED_right, PWM_OUTPUT);
+    pwmSetClock(4000);      // Clock prescaller for PWM generator
+}
+
+void LEDs::Left(float power){
+    pwmWrite(GPIO_LED_left, power * 1024);
+}
+
+void LEDs::Right(float power){
+    pwmWrite(GPIO_LED_right, power * 1024);
+}
diff --git a/camera/raspberry/lights/src/LEDs.hpp b/camera/raspberry/lights/src/LEDs.hpp
new file mode 100644
index 0000000..cc9693e
--- /dev/null
+++ b/camera/raspberry/lights/src/LEDs.hpp
@@ -0,0 +1,45 @@
+#pragma once
+
+#include <wiringPi.h>
+
+using namespace std;
+
+/**
+ * @brief LEDs strips controlled by AL8861Y
+ *          Dimmable by PWM on channel PWM0 and PWM1
+ */
+class LEDs{
+    /**
+     * @brief WiringPi GPIO number of pin which controls left channel
+     */
+    int GPIO_LED_left = 23;
+
+    /**
+     * @brief WiringPi GPIO number of pin which controls left channel
+     */
+    int GPIO_LED_right = 26;
+
+public:
+    /**
+     * @brief   Construct a new LEDs object
+     *          Configure GPIo and sets up frequency of PWM
+     *          Closes possible frequency reachable by Rpi is around 7 kHz
+     */
+    LEDs();
+
+    /**
+     * @brief Set power for right LED strip
+     *        Right channel is connected to pin 33 on header, GPIO13 (wiring pin 26)
+     *
+     * @param power Power of LED strip in range from 0.0 to 1.0
+     */
+    void Right(float power = 0);
+
+    /**
+     * @brief Set power for left LED strip
+     *        Left channel is connected to pin 32 on header, GPIO12 (wiring pin 23)
+     *
+     * @param power Power of LED strip in range from 0.0 to 1.0
+     */
+    void Left(float power = 0);
+};
diff --git a/camera/raspberry/lights/src/adxl345.cpp b/camera/raspberry/lights/src/adxl345.cpp
new file mode 100644
index 0000000..1d7cf6d
--- /dev/null
+++ b/camera/raspberry/lights/src/adxl345.cpp
@@ -0,0 +1,104 @@
+#include "adxl345.hpp"
+
+ADXL345::ADXL345(bool channel):channel(channel){
+    // SPI Mode 3 - CPOL = 1, CPHA = 1
+    wiringPiSPISetupMode(channel, 500000, 3);
+    // Power-up
+    Write(registers::POWER_CTL, 0b00001000);
+    // Maximal data output rate
+    Write(registers::BW_RATE, 0b00001010);
+
+    // Check connection by reading constant value of chip ID
+    if(ID() != 0xe5){
+        cout << "Cannot establish connection to accelerometer" << endl;
+    }
+}
+
+uint8_t ADXL345::ID(){
+    return Read(registers::DEVID);
+}
+
+array<int, 3> ADXL345::Trim_offsets(){
+    // Zero offset
+    Write(registers::OFSX, 0);
+    Write(registers::OFSY, 0);
+    Write(registers::OFSZ, 0);
+
+    // Acquire current values
+    usleep(100*1000);
+    auto axis_values = Raw_values();
+
+    // Calculate trims, Only acceleration of 1g should be on Z axis
+    int X_trim = -(round(axis_values[0]/4.0));
+    int Y_trim = -(round(axis_values[1]/4.0));
+    int Z_trim = -(round((axis_values[2]-255)/4.0));
+
+    // Write new trim values to registers
+    Write(registers::OFSX, X_trim);
+    Write(registers::OFSY, Y_trim);
+    Write(registers::OFSZ, Z_trim);
+
+    // Wait for propagation
+    usleep(100*1000);
+
+    return {X_trim, Y_trim, Z_trim};
+}
+
+void ADXL345::Trim_offsets(array<int8_t, 3> axis_offsets){
+    Write(registers::OFSX, axis_offsets[0]);
+    Write(registers::OFSY, axis_offsets[1]);
+    Write(registers::OFSZ, axis_offsets[2]);
+
+    // Wait for propagation
+    usleep(100*1000);
+}
+
+uint8_t ADXL345::Read(registers address){
+    // 0x80 is read command
+    vector<uint8_t> transfer_data = {static_cast<uint8_t>(static_cast<uint8_t>(address) | 0x80), dummy_packet};
+    wiringPiSPIDataRW(channel, transfer_data.data(), 2);
+    return transfer_data[1];
+}
+
+vector<uint8_t> ADXL345::Read_sequence(registers address, short length){
+    vector<uint8_t> transfer_data( length + 1, dummy_packet);
+    // Insert address to first position in data transfer, rest if filled with dummy bytes
+    transfer_data[0] = static_cast<uint8_t>(address) | 0xc0;
+    wiringPiSPIDataRW(channel, transfer_data.data(), length + 1);
+    // Remove first byte (address) from data
+    transfer_data.erase(transfer_data.begin());
+    return transfer_data;
+}
+
+void ADXL345::Write(registers address, uint8_t data){
+    vector<uint8_t> transfer_data = {static_cast<uint8_t>(address), data};
+    wiringPiSPIDataRW(channel, transfer_data.data(), 2);
+}
+
+array<int16_t, 3> ADXL345::Raw_values(){
+    vector<uint8_t> register_values = Read_sequence(registers::DATAX0, 6);
+    array<int16_t, 3> axis_values;
+    // Lower register represents LSB of value
+    axis_values[0] = register_values[0] | (register_values[1] << 8);
+    axis_values[1] = register_values[2] | (register_values[3] << 8);
+    axis_values[2] = register_values[4] | (register_values[5] << 8);
+    return axis_values;
+}
+
+double ADXL345::Inclination(){
+    auto axis_values = Raw_values();
+    // Resulting value is angle of IC package to horizontal plane
+    double angle = atan(static_cast<double>(axis_values[1])/axis_values[2]) * (180 / numbers::pi);
+    // Calculate absolute tilt based on quadrants from sensor values
+    if((axis_values[1] < 0) and (axis_values[2] >= 0)){
+        angle *= -1;
+    } else if ((axis_values[1] < 0) and (axis_values[2] < 0)){
+        angle = 180 - angle;
+    } else if ((axis_values[1] >= 0) and (axis_values[2] < 0)){
+        angle = -angle + 180;
+    } else if ((axis_values[1] > 0) and (axis_values[2] >= 0)){
+        angle = 360 - angle;
+    }
+    // Calculate inverse angle to corespond with clockwise rotation
+    return - angle + 360;
+}
diff --git a/camera/raspberry/lights/src/adxl345.hpp b/camera/raspberry/lights/src/adxl345.hpp
new file mode 100644
index 0000000..de862cf
--- /dev/null
+++ b/camera/raspberry/lights/src/adxl345.hpp
@@ -0,0 +1,124 @@
+#pragma once
+
+#include <iostream>
+#include <vector>
+#include <array>
+#include <cmath>
+#include <numbers>
+
+#include <unistd.h>
+
+#include <wiringPiSPI.h>
+
+using namespace std;
+
+/**
+ * @brief   Accelerometer AXDL345
+ *          Datasheet: https://www.analog.com/media/en/technical-documentation/data-sheets/ADXL345.pdf
+ */
+class ADXL345{
+    /**
+     * @brief  Channel on which is accelerometer connected
+     */
+    bool channel;
+
+    /**
+     * @brief Some of ADXL345 regsisters
+     */
+    enum class registers: uint8_t{
+        DEVID       = 0x00,
+        OFSX        = 0x1e,
+        OFSY        = 0x1f,
+        OFSZ        = 0x20,
+        BW_RATE     = 0x2c,
+        POWER_CTL   = 0x2d,
+        DATA_FORMAT = 0x31,
+        DATAX0      = 0x32,
+        DATAX1      = 0x33,
+        DATAY0      = 0x34,
+        DATAY1      = 0x35,
+        DATAZ0      = 0x36,
+        DATAZ1      = 0x37
+    };
+
+    /**
+     * @brief   Dummy packet, is not interpreted as any command by ADXL
+                Is used as padding for SPI transfer
+     */
+    uint8_t dummy_packet = 0xaa;
+
+public:
+
+    /**
+     * @brief Construct a new ADXL345 object
+     *
+     * @param channel SPI channel of Rpi, has only two channels, 0 or 1
+     */
+    ADXL345(bool channel = 0);
+
+    /**
+     * @brief   Reads ID from accelerometer
+     *          Value is read only
+     *          Suitable for testing of connection
+     *
+     * @return uint8_t  Always should returns 0xe5 if communications is working
+     */
+    uint8_t ID();
+
+    /**
+     * @brief   Calculates roll angle of sensor based on value from X and Z values of acceleration
+     *
+     * @return double Roll angle of sensor (around X axis of IC), From 0 to 360°, clockwise rotation
+     */
+    double Inclination();
+
+    /**
+     * @brief   Trims offsets of accelerometer based on current values !!
+     *          During this calibration process accelerometer should be position in base position (0 degrees)
+
+     * @return array<int8_t, 3> Values which was used as trimming values for coresponding axis [X,Y,Z]
+     */
+    array<int, 3> Trim_offsets();
+
+    /**
+     * @brief   Trims offsets of accelerometer with defined values
+     *
+     * @param axis_offsets  Trim values for all accelerometer axis [X,Y,Z]
+     *                      Value is multiplied by 4 and added to resulting acceleration of respective axis
+     */
+    void Trim_offsets(array<int8_t, 3> axis_offsets);
+
+
+private:
+    /**
+     * @brief   Reads raw values of acceleration for all axis
+     *
+     * @return array<int16_t, 3>    Acceleration values from sensor for each axis [X,Y,Z]
+     */
+    array<int16_t, 3> Raw_values();
+
+    /**
+     * @brief   Read one byte of data from ADXL memory
+     *
+     * @param address   Address in ADXL memory from which are data read
+     * @return uint8_t  Data from memory
+     */
+    uint8_t Read(registers address);
+
+    /**
+     * @brief       Reads sequence of data from ADXL memory
+     *
+     * @param address   Address in ADXL memory from which reading starts
+     * @param length    Amount of bytes to read
+     * @return vector<uint8_t>  Readout data
+     */
+    vector<uint8_t> Read_sequence(registers address, short length);
+
+    /**
+     * @brief   Write one byte of data into ADXL memory
+     *
+     * @param address   Address to which will be writing performed
+     * @param data      Data to write
+     */
+    void Write(registers address, uint8_t data);
+};
diff --git a/camera/raspberry/lights/src/demo.cpp b/camera/raspberry/lights/src/demo.cpp
new file mode 100644
index 0000000..9fca721
--- /dev/null
+++ b/camera/raspberry/lights/src/demo.cpp
@@ -0,0 +1,21 @@
+#include "demo.hpp"
+
+using namespace std;
+
+int main(){
+
+    // ----- Fan -----
+    auto fan = Fan();
+    fan.On();
+
+    // ----- LEDS -----
+    auto led = LEDs();
+    for(int i = 0; i < 100000; i++){
+        led.Left(1.0);
+        led.Right(1.0);
+        usleep(10000);
+        led.Left(0.0);
+        led.Right(0.0);
+    }
+    fan.Off();
+}
diff --git a/camera/raspberry/lights/src/demo.hpp b/camera/raspberry/lights/src/demo.hpp
new file mode 100644
index 0000000..5b8c9ba
--- /dev/null
+++ b/camera/raspberry/lights/src/demo.hpp
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <iostream>
+#include <iomanip>
+
+#include <unistd.h>
+
+#include "adxl345.hpp"
+#include "fan.hpp"
+#include "LEDs.hpp"
+#include "sick_side.hpp"
+#include "sick_front.hpp"
diff --git a/camera/raspberry/lights/src/fan.cpp b/camera/raspberry/lights/src/fan.cpp
new file mode 100644
index 0000000..9a11b78
--- /dev/null
+++ b/camera/raspberry/lights/src/fan.cpp
@@ -0,0 +1,14 @@
+#include "fan.hpp"
+
+Fan::Fan(){
+    wiringPiSetup();
+    pinMode(GPIO_fan, OUTPUT);
+}
+
+void Fan::On(){
+    digitalWrite(GPIO_fan, 1);
+}
+
+void Fan::Off(){
+    digitalWrite(GPIO_fan, 0);
+}
diff --git a/camera/raspberry/lights/src/fan.hpp b/camera/raspberry/lights/src/fan.hpp
new file mode 100644
index 0000000..0e64d35
--- /dev/null
+++ b/camera/raspberry/lights/src/fan.hpp
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <wiringPi.h>
+#include <softPwm.h>
+
+using namespace std;
+
+/**
+ * @brief PWM controlled fan without RPM detection
+ */
+class Fan{
+    /**
+     * @brief WiringPi GPIO number of pin which controls fan
+     */
+    int GPIO_fan = 25;
+
+public:
+    /**
+     * @brief   Construct a new Fan object
+     *          Create soft-PWM thread for fan control signal
+     */
+    Fan();
+
+    /**
+     * @brief Enable cooling fan
+     */
+    void On();
+
+    /**
+     * @brief Disable cooling fan
+     */
+    void Off();
+
+};
diff --git a/camera/raspberry/lights/src/sick_front.cpp b/camera/raspberry/lights/src/sick_front.cpp
new file mode 100644
index 0000000..ae0e0f0
--- /dev/null
+++ b/camera/raspberry/lights/src/sick_front.cpp
@@ -0,0 +1,61 @@
+#include "sick_front.hpp"
+
+Sick_front::Sick_front(bool channel):channel(channel){
+    wiringPiSPISetupMode(channel, 500000, 3);
+    wiringPiSetup();
+    pinMode(GPIO_conversion_start, OUTPUT);
+    pinMode(GPIO_laser_enable, OUTPUT);
+}
+
+optional<unsigned int> Sick_front::Distance_avg(unsigned int measurement_count){
+    unsigned int sum = 0;
+    unsigned int successful_measurements = 0;
+    for(unsigned int i = 0; i < measurement_count; i++){
+        auto distance = Distance();
+        if(distance){
+            sum += *distance;
+            successful_measurements++;
+        }
+    }
+    if(sum == 0){
+        return {};
+    }
+    return sum/successful_measurements;
+}
+
+optional<unsigned int> Sick_front::Distance(){
+    auto raw = Conversion_raw();
+    float differential_raw = static_cast<float>(static_cast<uint16_t>(raw));
+    // Calculation of true voltage from reference voltage and ADC value
+    double voltage = differential_raw / (1 << 16) * reference_voltage;
+    //cout << "Voltage: " << hex << raw << dec << " : " <<differential_raw << " : " << setprecision(4) << voltage << " V" << setprecision(0);
+    // Out of Range
+    if((voltage < voltage_close) | (voltage > voltage_far)){
+        return {};
+    }
+
+    double range_percentage = (voltage-voltage_min)/(voltage_max-voltage_min);
+    double distance = round((range_max-range_min)*range_percentage+range_min);
+    return distance;
+}
+
+uint16_t Sick_front::Conversion_raw(){
+    // Start conversion
+    digitalWrite(GPIO_conversion_start, 1);
+    array<uint8_t, 2> received_data = {0x00, 0x00};
+    wiringPiSPIDataRW(channel, received_data.data(), 2);
+    // Start conversion
+    digitalWrite(GPIO_conversion_start, 0);
+    return ((received_data[0]<<8) | received_data[1]);
+}
+
+void Sick_front::Laser(bool state){
+    if(state){
+        // ON
+        pinMode(GPIO_laser_enable, INPUT);
+    } else {
+        // OFF
+        pinMode(GPIO_laser_enable, OUTPUT);
+        digitalWrite(GPIO_laser_enable, 0);
+    }
+}
diff --git a/camera/raspberry/lights/src/sick_front.hpp b/camera/raspberry/lights/src/sick_front.hpp
new file mode 100644
index 0000000..18b6f95
--- /dev/null
+++ b/camera/raspberry/lights/src/sick_front.hpp
@@ -0,0 +1,134 @@
+#pragma once
+
+#include <iostream>
+#include <array>
+#include <string>
+#include <optional>
+#include <cmath>
+
+#include <iomanip>
+
+#include <wiringPi.h>
+#include <wiringPiSPI.h>
+
+using namespace std;
+
+/**
+Type: DT50-N2113
+**/
+class Sick_front{
+    /**
+     * @brief  Channel on which is ADC for front distance sensor connected
+     */
+    bool channel;
+
+    /**
+     * @brief WiringPi GPIO number of pin which start conversion by change 0->1
+     */
+    const int GPIO_conversion_start = 5;
+
+    /**
+     * @brief WiringPi GPIO number of pin which turn ON/OFF laser of sensor
+     */
+    const int GPIO_laser_enable = 4;
+
+        /**
+     * @brief   Minimal configured distance of sensor
+     */
+    const int range_min = 200;
+
+    /**
+     * @brief   Maximal configured distance of sensor
+     */
+    const int range_max = 10000;
+
+    /**
+     * @brief   Value of sense resistor in ohms used for current loop
+     */
+    const int sense_resistor = 150;
+
+    /**
+     * @brief   Current coresponding to minimal distance, configured in "range_min"
+     */
+    const float current_loop_min = 0.004;
+
+    /**
+     * @brief   Current coresponding to maximal distance, configured in "range_max"
+     */
+    const float current_loop_max = 0.020;
+
+    /**
+     * @brief   Current coresponding to state when minimal distance is exceeded
+     */
+    const float current_loop_close = 0.0035;
+
+    /**
+     * @brief   Current coresponding to state when maximal distance is exceeded
+     */
+    const float current_loop_far = 0.0205;
+
+    /**
+     * @brief   Voltage coresponding to maximal distance
+     */
+    const float voltage_max = current_loop_max*sense_resistor;
+
+    /**
+     * @brief   Voltage coresponding to minimal distance
+     */
+    const float voltage_min = current_loop_min*sense_resistor;
+
+    /**
+     * @brief   Voltage coresponding to state when maximal distance is exceeded
+     */
+    const float voltage_close = current_loop_close*sense_resistor;
+
+    /**
+     * @brief   Voltage coresponding to state when minimal distance is exceeded
+     */
+    const float voltage_far = current_loop_far*sense_resistor;
+
+    /**
+     * @brief  Reference voltage of ADC
+     */
+    const float reference_voltage = 3.308f;
+
+public:
+    /**
+     * @brief   Construct a new Sick_front object
+     *          Configure GPIO and SPI interface
+     *
+     * @param channel   Channel on which is sensor connected
+     */
+    Sick_front(bool channel = 1);
+
+    /**
+     * @brief Calculate distance measurted by distance sensor
+     *
+     * @return optional<unsigned int>   True and value in mm if in range, False otherwise
+     */
+    optional<unsigned int> Distance();
+
+    /**
+     * @brief Average multiple measurements of distance sensor output
+     *
+     * @param measurement_count         Amount of measurents to average
+     * @return optional<unsigned int>   Same as Distance()
+     */
+    optional<unsigned int> Distance_avg(unsigned int measurement_count = 8);
+
+    /**
+     * @brief           Turn ON or OFF laser of sensor
+     *
+     * @param state     New state of laser
+     */
+    void Laser(bool state);
+
+private:
+    /**
+     * @brief           Read raw ADC data from ADC
+     *
+     * @return uint16_t raw ADc data in 2's complement format
+     */
+    uint16_t Conversion_raw();
+
+};
diff --git a/camera/raspberry/lights/src/sick_side.cpp b/camera/raspberry/lights/src/sick_side.cpp
new file mode 100644
index 0000000..c689c45
--- /dev/null
+++ b/camera/raspberry/lights/src/sick_side.cpp
@@ -0,0 +1,103 @@
+#include "sick_side.hpp"
+
+Sick_side::Sick_side(){
+    serial_descriptor = serialOpen("/dev/ttyS0", 9600);
+    if(serial_descriptor == -1){
+        cout << "Cannot open serial line for communication with side distance sensor" << endl;
+    }
+}
+
+uint8_t Sick_side::ID(){
+    Send_data({static_cast<uint8_t>(commands::Read), 0x01, 0x00});
+    //while(Data_available() < 12);
+    // Readout data which are in buffer from transmittion
+    //Receive();
+    // Read real received data from sensor
+    if(auto responce = Receive()){
+        return (*responce)[2];
+    } else {
+        return 0x00;
+    }
+}
+
+void Sick_side::Laser(bool state){
+    if(state){
+        // ON
+        Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x03});
+    } else {
+        // OFF
+        Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x02});
+    }
+
+    //Read responce to clear buffer
+    Receive();
+}
+
+optional<double> Sick_side::Distance(){
+    auto distance_data = Distance_data();
+    if(distance_data == 0x7fff){
+        // Out of measuring range
+        return {};
+    } else {
+        // Convert to 2'complement and then to mm
+        return static_cast<int16_t>(distance_data)/100.0;
+    }
+}
+
+uint16_t Sick_side::Distance_data(){
+    Send_data({static_cast<uint8_t>(commands::Command), 0xb0, 0x01});
+    if(auto responce = Receive()){
+        return (((*responce)[1]<<8) | (*responce)[2]);
+    } else {
+        return 0x7fff;
+    }
+}
+
+void Sick_side::Send_data(array<uint8_t, 3> data){
+    array<uint8_t, 6> packet = {static_cast<uint8_t>(signs::STX), data[0], data[1], data[2], static_cast<uint8_t>(signs::ETX), 0x00};
+    packet[5] = Calculate_BCC(packet);
+    Transmit(packet);
+}
+
+void Sick_side::Transmit(array<uint8_t, 6> data){
+    for(int i = 0; i < 6; i++){
+        serialPutchar(serial_descriptor, data[i]);
+    }
+}
+
+uint8_t Sick_side::Calculate_BCC(array<uint8_t, 6> data){
+    uint8_t BCC = 0;
+    // BCC is calculated only from bytes on positions 1,2,3
+    for(int byte_index = 1; byte_index < 4; byte_index++){
+        BCC ^= data[byte_index];
+    }
+    return BCC;
+}
+
+optional<array<uint8_t, 3>> Sick_side::Receive(){
+    while(Data_available() < 12);
+    // Readout data which are in buffer from transmittion
+    for(int i = 0; i < 6; i++){
+        serialGetchar(serial_descriptor);
+    }
+    // Receive true data
+    array<uint8_t, 6> received_data;
+    for(int i = 0; i < 6; i++){
+        received_data[i] = serialGetchar(serial_descriptor);
+    }
+    // Check for communication errors
+    if(static_cast<signs>(received_data[1]) == signs::NAK){
+        cout << error_codes[received_data[2]] << endl;
+        return {};
+    }
+    if(received_data[5] != Calculate_BCC(received_data)){
+        cout << "Received BCC is invalid" << endl;
+        return {};
+    }
+
+    return array<uint8_t, 3>({received_data[1], received_data[2], received_data[3]});
+}
+
+unsigned int Sick_side::Data_available(){
+    return serialDataAvail(serial_descriptor);
+}
diff --git a/camera/raspberry/lights/src/sick_side.hpp b/camera/raspberry/lights/src/sick_side.hpp
new file mode 100644
index 0000000..f77d016
--- /dev/null
+++ b/camera/raspberry/lights/src/sick_side.hpp
@@ -0,0 +1,119 @@
+#pragma once
+
+#include <iostream>
+#include <array>
+#include <optional>
+#include <map>
+#include <string>
+
+#include <wiringSerial.h>
+
+using namespace std;
+
+/**
+Type: OD1-B100C50A15
+**/
+class Sick_side{
+
+    int serial_descriptor = 0;
+
+    enum class signs: uint8_t{
+        STX       = 0x02,
+        ETX       = 0x03,
+        ACK       = 0x06,
+        NAK       = 0x15,
+    };
+
+    map<uint8_t, string> error_codes{
+        {0x02, "Address is invalid"},
+        {0x04, "BCC values is invalid"},
+        {0x05, "Invalid command"},
+        {0x06, "Invalid setting value (out of spec)"},
+        {0x07, "Invalid setting value (out of range)"}
+    };
+
+    enum class commands: uint8_t{
+        Command     = 0x43, // Also used for reading out measured values
+        Write       = 0x57,
+        Read        = 0x52,
+    };
+
+public:
+    /**
+     * @brief   Construct a new Sick_side object
+     *          Opens serial line and save file descriptor
+     */
+    Sick_side();
+
+    /**
+     * @brief           Read sensor ID from memory of sensor
+     *                  Used for connection check
+     *
+     * @return uint8_t  Value 0x64, 0x00 if communication fails
+     */
+    uint8_t ID();
+
+    /**
+     * @brief           Turn ON or OFF laser of sensor
+     *
+     * @param state     New state of laser
+     */
+    void Laser(bool state);
+
+    /**
+     * @brief   Read and calculate distance from distance sensor
+     *          Distance is relative to sensors zero point
+     *
+     * @return optional<double> And and relative distance from zero point, false otherwise (Out of Range)
+     */
+    optional<double> Distance();
+
+private:
+
+    /**
+     * @brief Reads distance data from sensor register
+     *
+     * @return uint16_t Distance in 2's complement, range -5000 to 5000, coresponds to -50 mm to +50 mm
+     */
+    uint16_t Distance_data();
+
+    /**
+     * @brief       Transmit given data over serial to RS485 transmitter
+     *
+     * @param data  Data to send via serial line, header and footer are added
+     */
+    void Send_data(array<uint8_t, 3> data);
+
+    /**
+     * @brief           Calculates BCC for given packet of data
+     *                  BCC is XOR of bytes on index 1,2,3, remaning bytes are ignored
+     *
+     * @param data      Packet used for calculation
+     * @return uint8_t  Values of BCC
+     */
+    uint8_t Calculate_BCC(array<uint8_t, 6> data);
+
+    /**
+     * @brief       Transmits data over UART to RS485 transmitter
+     *              Data are also copied to input buffer due to half-duplex connection
+     *
+     * @param data  Data to transmit
+     */
+    void Transmit(array<uint8_t, 6> data);
+
+    /**
+     * @brief       Data received from sensor
+     *              Only usefull data are returned, without STX, ACK/NACK, BCC
+     *
+     * @return optional<array<uint8_t, 3>>  True and value if communication is valid, False otherwise
+     */
+    optional<array<uint8_t, 3>> Receive();
+
+    /**
+     * @brief Reads amopunt of data in input buffer
+     *
+     * @return unsigned int Number of bytes in input buffer
+     */
+    unsigned int Data_available();
+
+};
diff --git a/camera/raspberry/makefile b/camera/raspberry/makefile
new file mode 100644
index 0000000..661a29d
--- /dev/null
+++ b/camera/raspberry/makefile
@@ -0,0 +1,6 @@
+all:
+	make -C lights/
+	mv lights/bbx-test lights/lights
+	make -C sensors/
+	mv sensors/bbx-test sensors/sensors
+	
diff --git a/camera/raspberry/sensors/Makefile b/camera/raspberry/sensors/Makefile
new file mode 100644
index 0000000..4c082b0
--- /dev/null
+++ b/camera/raspberry/sensors/Makefile
@@ -0,0 +1,40 @@
+CC = g++
+
+OPT = -O3
+FLAGS = -std=c++20 -Wall -Wextra -pedantic
+LIBS = -lwiringPi
+
+TARGET = bbx-test
+SOURCEDIR = src
+BUILDDIR = build
+
+SOURCES = $(wildcard $(SOURCEDIR)/*.cpp)
+OBJECTS = $(patsubst $(SOURCEDIR)/%.cpp,$(BUILDDIR)/%.o,$(SOURCES))
+
+.PHONY: all clean depend run
+
+all: $(TARGET)
+
+$(BUILDDIR):
+	@mkdir -p $(BUILDDIR)
+
+$(TARGET): $(OBJECTS)
+	$(CC) $(FLAGS) $(OPT) $^ -o $@ $(LIBS)
+
+$(OBJECTS): $(BUILDDIR)/%.o :  $(SOURCEDIR)/%.cpp $(SOURCEDIR)/%.hpp | $(BUILDDIR)
+	$(CC) $(FLAGS) $(OPT) -c $< -o $@ $(LIBS)
+
+depend: .depend
+
+.depend: $(SOURCES)
+	rm -rf $(BUILDDIR)/.depend
+	$(CXX) $(FLAGS) -MM $^ -MF $(BUILDDIR)/.depend
+
+clean:
+	rm -rf $(BUILDDIR)
+	rm -f $(TARGET)
+
+run: $(TARGET)
+	sudo ./$(TARGET)
+
+-include $(BUILDDIR)/.depend
diff --git a/camera/raspberry/sensors/build/LEDs.o b/camera/raspberry/sensors/build/LEDs.o
new file mode 100644
index 0000000..3dc0eba
Binary files /dev/null and b/camera/raspberry/sensors/build/LEDs.o differ
diff --git a/camera/raspberry/sensors/build/adxl345.o b/camera/raspberry/sensors/build/adxl345.o
new file mode 100644
index 0000000..248fede
Binary files /dev/null and b/camera/raspberry/sensors/build/adxl345.o differ
diff --git a/camera/raspberry/sensors/build/demo.o b/camera/raspberry/sensors/build/demo.o
new file mode 100644
index 0000000..326b58e
Binary files /dev/null and b/camera/raspberry/sensors/build/demo.o differ
diff --git a/camera/raspberry/sensors/build/fan.o b/camera/raspberry/sensors/build/fan.o
new file mode 100644
index 0000000..0ce7f2d
Binary files /dev/null and b/camera/raspberry/sensors/build/fan.o differ
diff --git a/camera/raspberry/sensors/build/sick_front.o b/camera/raspberry/sensors/build/sick_front.o
new file mode 100644
index 0000000..e05040d
Binary files /dev/null and b/camera/raspberry/sensors/build/sick_front.o differ
diff --git a/camera/raspberry/sensors/build/sick_side.o b/camera/raspberry/sensors/build/sick_side.o
new file mode 100644
index 0000000..df4ba6d
Binary files /dev/null and b/camera/raspberry/sensors/build/sick_side.o differ
diff --git a/camera/raspberry/sensors/sensors b/camera/raspberry/sensors/sensors
new file mode 100644
index 0000000..8eb780d
Binary files /dev/null and b/camera/raspberry/sensors/sensors differ
diff --git a/camera/raspberry/sensors/src/LEDs.cpp b/camera/raspberry/sensors/src/LEDs.cpp
new file mode 100644
index 0000000..de803b1
--- /dev/null
+++ b/camera/raspberry/sensors/src/LEDs.cpp
@@ -0,0 +1,16 @@
+#include "LEDs.hpp"
+
+LEDs::LEDs(){
+    wiringPiSetup();
+    pinMode(GPIO_LED_left, PWM_OUTPUT);
+    pinMode(GPIO_LED_right, PWM_OUTPUT);
+    pwmSetClock(4000);      // Clock prescaller for PWM generator
+}
+
+void LEDs::Left(float power){
+    pwmWrite(GPIO_LED_left, power * 1024);
+}
+
+void LEDs::Right(float power){
+    pwmWrite(GPIO_LED_right, power * 1024);
+}
diff --git a/camera/raspberry/sensors/src/LEDs.hpp b/camera/raspberry/sensors/src/LEDs.hpp
new file mode 100644
index 0000000..cc9693e
--- /dev/null
+++ b/camera/raspberry/sensors/src/LEDs.hpp
@@ -0,0 +1,45 @@
+#pragma once
+
+#include <wiringPi.h>
+
+using namespace std;
+
+/**
+ * @brief LEDs strips controlled by AL8861Y
+ *          Dimmable by PWM on channel PWM0 and PWM1
+ */
+class LEDs{
+    /**
+     * @brief WiringPi GPIO number of pin which controls left channel
+     */
+    int GPIO_LED_left = 23;
+
+    /**
+     * @brief WiringPi GPIO number of pin which controls left channel
+     */
+    int GPIO_LED_right = 26;
+
+public:
+    /**
+     * @brief   Construct a new LEDs object
+     *          Configure GPIo and sets up frequency of PWM
+     *          Closes possible frequency reachable by Rpi is around 7 kHz
+     */
+    LEDs();
+
+    /**
+     * @brief Set power for right LED strip
+     *        Right channel is connected to pin 33 on header, GPIO13 (wiring pin 26)
+     *
+     * @param power Power of LED strip in range from 0.0 to 1.0
+     */
+    void Right(float power = 0);
+
+    /**
+     * @brief Set power for left LED strip
+     *        Left channel is connected to pin 32 on header, GPIO12 (wiring pin 23)
+     *
+     * @param power Power of LED strip in range from 0.0 to 1.0
+     */
+    void Left(float power = 0);
+};
diff --git a/camera/raspberry/sensors/src/adxl345.cpp b/camera/raspberry/sensors/src/adxl345.cpp
new file mode 100644
index 0000000..1d7cf6d
--- /dev/null
+++ b/camera/raspberry/sensors/src/adxl345.cpp
@@ -0,0 +1,104 @@
+#include "adxl345.hpp"
+
+ADXL345::ADXL345(bool channel):channel(channel){
+    // SPI Mode 3 - CPOL = 1, CPHA = 1
+    wiringPiSPISetupMode(channel, 500000, 3);
+    // Power-up
+    Write(registers::POWER_CTL, 0b00001000);
+    // Maximal data output rate
+    Write(registers::BW_RATE, 0b00001010);
+
+    // Check connection by reading constant value of chip ID
+    if(ID() != 0xe5){
+        cout << "Cannot establish connection to accelerometer" << endl;
+    }
+}
+
+uint8_t ADXL345::ID(){
+    return Read(registers::DEVID);
+}
+
+array<int, 3> ADXL345::Trim_offsets(){
+    // Zero offset
+    Write(registers::OFSX, 0);
+    Write(registers::OFSY, 0);
+    Write(registers::OFSZ, 0);
+
+    // Acquire current values
+    usleep(100*1000);
+    auto axis_values = Raw_values();
+
+    // Calculate trims, Only acceleration of 1g should be on Z axis
+    int X_trim = -(round(axis_values[0]/4.0));
+    int Y_trim = -(round(axis_values[1]/4.0));
+    int Z_trim = -(round((axis_values[2]-255)/4.0));
+
+    // Write new trim values to registers
+    Write(registers::OFSX, X_trim);
+    Write(registers::OFSY, Y_trim);
+    Write(registers::OFSZ, Z_trim);
+
+    // Wait for propagation
+    usleep(100*1000);
+
+    return {X_trim, Y_trim, Z_trim};
+}
+
+void ADXL345::Trim_offsets(array<int8_t, 3> axis_offsets){
+    Write(registers::OFSX, axis_offsets[0]);
+    Write(registers::OFSY, axis_offsets[1]);
+    Write(registers::OFSZ, axis_offsets[2]);
+
+    // Wait for propagation
+    usleep(100*1000);
+}
+
+uint8_t ADXL345::Read(registers address){
+    // 0x80 is read command
+    vector<uint8_t> transfer_data = {static_cast<uint8_t>(static_cast<uint8_t>(address) | 0x80), dummy_packet};
+    wiringPiSPIDataRW(channel, transfer_data.data(), 2);
+    return transfer_data[1];
+}
+
+vector<uint8_t> ADXL345::Read_sequence(registers address, short length){
+    vector<uint8_t> transfer_data( length + 1, dummy_packet);
+    // Insert address to first position in data transfer, rest if filled with dummy bytes
+    transfer_data[0] = static_cast<uint8_t>(address) | 0xc0;
+    wiringPiSPIDataRW(channel, transfer_data.data(), length + 1);
+    // Remove first byte (address) from data
+    transfer_data.erase(transfer_data.begin());
+    return transfer_data;
+}
+
+void ADXL345::Write(registers address, uint8_t data){
+    vector<uint8_t> transfer_data = {static_cast<uint8_t>(address), data};
+    wiringPiSPIDataRW(channel, transfer_data.data(), 2);
+}
+
+array<int16_t, 3> ADXL345::Raw_values(){
+    vector<uint8_t> register_values = Read_sequence(registers::DATAX0, 6);
+    array<int16_t, 3> axis_values;
+    // Lower register represents LSB of value
+    axis_values[0] = register_values[0] | (register_values[1] << 8);
+    axis_values[1] = register_values[2] | (register_values[3] << 8);
+    axis_values[2] = register_values[4] | (register_values[5] << 8);
+    return axis_values;
+}
+
+double ADXL345::Inclination(){
+    auto axis_values = Raw_values();
+    // Resulting value is angle of IC package to horizontal plane
+    double angle = atan(static_cast<double>(axis_values[1])/axis_values[2]) * (180 / numbers::pi);
+    // Calculate absolute tilt based on quadrants from sensor values
+    if((axis_values[1] < 0) and (axis_values[2] >= 0)){
+        angle *= -1;
+    } else if ((axis_values[1] < 0) and (axis_values[2] < 0)){
+        angle = 180 - angle;
+    } else if ((axis_values[1] >= 0) and (axis_values[2] < 0)){
+        angle = -angle + 180;
+    } else if ((axis_values[1] > 0) and (axis_values[2] >= 0)){
+        angle = 360 - angle;
+    }
+    // Calculate inverse angle to corespond with clockwise rotation
+    return - angle + 360;
+}
diff --git a/camera/raspberry/sensors/src/adxl345.hpp b/camera/raspberry/sensors/src/adxl345.hpp
new file mode 100644
index 0000000..de862cf
--- /dev/null
+++ b/camera/raspberry/sensors/src/adxl345.hpp
@@ -0,0 +1,124 @@
+#pragma once
+
+#include <iostream>
+#include <vector>
+#include <array>
+#include <cmath>
+#include <numbers>
+
+#include <unistd.h>
+
+#include <wiringPiSPI.h>
+
+using namespace std;
+
+/**
+ * @brief   Accelerometer AXDL345
+ *          Datasheet: https://www.analog.com/media/en/technical-documentation/data-sheets/ADXL345.pdf
+ */
+class ADXL345{
+    /**
+     * @brief  Channel on which is accelerometer connected
+     */
+    bool channel;
+
+    /**
+     * @brief Some of ADXL345 regsisters
+     */
+    enum class registers: uint8_t{
+        DEVID       = 0x00,
+        OFSX        = 0x1e,
+        OFSY        = 0x1f,
+        OFSZ        = 0x20,
+        BW_RATE     = 0x2c,
+        POWER_CTL   = 0x2d,
+        DATA_FORMAT = 0x31,
+        DATAX0      = 0x32,
+        DATAX1      = 0x33,
+        DATAY0      = 0x34,
+        DATAY1      = 0x35,
+        DATAZ0      = 0x36,
+        DATAZ1      = 0x37
+    };
+
+    /**
+     * @brief   Dummy packet, is not interpreted as any command by ADXL
+                Is used as padding for SPI transfer
+     */
+    uint8_t dummy_packet = 0xaa;
+
+public:
+
+    /**
+     * @brief Construct a new ADXL345 object
+     *
+     * @param channel SPI channel of Rpi, has only two channels, 0 or 1
+     */
+    ADXL345(bool channel = 0);
+
+    /**
+     * @brief   Reads ID from accelerometer
+     *          Value is read only
+     *          Suitable for testing of connection
+     *
+     * @return uint8_t  Always should returns 0xe5 if communications is working
+     */
+    uint8_t ID();
+
+    /**
+     * @brief   Calculates roll angle of sensor based on value from X and Z values of acceleration
+     *
+     * @return double Roll angle of sensor (around X axis of IC), From 0 to 360°, clockwise rotation
+     */
+    double Inclination();
+
+    /**
+     * @brief   Trims offsets of accelerometer based on current values !!
+     *          During this calibration process accelerometer should be position in base position (0 degrees)
+
+     * @return array<int8_t, 3> Values which was used as trimming values for coresponding axis [X,Y,Z]
+     */
+    array<int, 3> Trim_offsets();
+
+    /**
+     * @brief   Trims offsets of accelerometer with defined values
+     *
+     * @param axis_offsets  Trim values for all accelerometer axis [X,Y,Z]
+     *                      Value is multiplied by 4 and added to resulting acceleration of respective axis
+     */
+    void Trim_offsets(array<int8_t, 3> axis_offsets);
+
+
+private:
+    /**
+     * @brief   Reads raw values of acceleration for all axis
+     *
+     * @return array<int16_t, 3>    Acceleration values from sensor for each axis [X,Y,Z]
+     */
+    array<int16_t, 3> Raw_values();
+
+    /**
+     * @brief   Read one byte of data from ADXL memory
+     *
+     * @param address   Address in ADXL memory from which are data read
+     * @return uint8_t  Data from memory
+     */
+    uint8_t Read(registers address);
+
+    /**
+     * @brief       Reads sequence of data from ADXL memory
+     *
+     * @param address   Address in ADXL memory from which reading starts
+     * @param length    Amount of bytes to read
+     * @return vector<uint8_t>  Readout data
+     */
+    vector<uint8_t> Read_sequence(registers address, short length);
+
+    /**
+     * @brief   Write one byte of data into ADXL memory
+     *
+     * @param address   Address to which will be writing performed
+     * @param data      Data to write
+     */
+    void Write(registers address, uint8_t data);
+};
diff --git a/camera/raspberry/sensors/src/demo.cpp b/camera/raspberry/sensors/src/demo.cpp
new file mode 100644
index 0000000..97a5b82
--- /dev/null
+++ b/camera/raspberry/sensors/src/demo.cpp
@@ -0,0 +1,43 @@
+#include "demo.hpp"
+
+using namespace std;
+
+int main(){
+
+    // ----- Accelerometer -----
+    auto accelerometer = ADXL345();
+    // "HOT" trimming with current values of tilt
+    // This emthod is not expected to be used in production
+    //accelerometer.Trim_offsets();
+    // Trimming with predefined values, based on previous measuments
+    // Data for command below can be taken from command above
+    accelerometer.Trim_offsets({0,0,0});
+
+    // ----- Side laser -----
+    auto side = Sick_side();
+    // Enable laser of for measuring
+    side.Laser(false);
+
+    // ----- Front laser -----
+    auto front = Sick_front();
+    front.Laser(true);
+
+    // ----- Test loop -----
+    // Read inclination from accelerometer and distance from both sensors in loop and print it
+    while(true){
+        
+        if(auto distance = front.Distance_avg()) {
+             setprecision(1);
+             cout << "{ \n \"depthPos\" :\n {\"x\" : " << accelerometer.Inclination() << ",\n \"y\" : " << *distance << "\n},\n \"rectangle\" : \n {\n \"h\" : 50.0, \n \"w\" : 60.0 \n}}                                                                                              " <<std::endl;
+	     break;
+        }
+        else
+        {
+            distance=0.0;
+        }
+        
+
+    }
+
+    return 0;
+}
diff --git a/camera/raspberry/sensors/src/demo.hpp b/camera/raspberry/sensors/src/demo.hpp
new file mode 100644
index 0000000..5b8c9ba
--- /dev/null
+++ b/camera/raspberry/sensors/src/demo.hpp
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <iostream>
+#include <iomanip>
+
+#include <unistd.h>
+
+#include "adxl345.hpp"
+#include "fan.hpp"
+#include "LEDs.hpp"
+#include "sick_side.hpp"
+#include "sick_front.hpp"
diff --git a/camera/raspberry/sensors/src/fan.cpp b/camera/raspberry/sensors/src/fan.cpp
new file mode 100644
index 0000000..9a11b78
--- /dev/null
+++ b/camera/raspberry/sensors/src/fan.cpp
@@ -0,0 +1,14 @@
+#include "fan.hpp"
+
+Fan::Fan(){
+    wiringPiSetup();
+    pinMode(GPIO_fan, OUTPUT);
+}
+
+void Fan::On(){
+    digitalWrite(GPIO_fan, 1);
+}
+
+void Fan::Off(){
+    digitalWrite(GPIO_fan, 0);
+}
diff --git a/camera/raspberry/sensors/src/fan.hpp b/camera/raspberry/sensors/src/fan.hpp
new file mode 100644
index 0000000..0e64d35
--- /dev/null
+++ b/camera/raspberry/sensors/src/fan.hpp
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <wiringPi.h>
+#include <softPwm.h>
+
+using namespace std;
+
+/**
+ * @brief PWM controlled fan without RPM detection
+ */
+class Fan{
+    /**
+     * @brief WiringPi GPIO number of pin which controls fan
+     */
+    int GPIO_fan = 25;
+
+public:
+    /**
+     * @brief   Construct a new Fan object
+     *          Create soft-PWM thread for fan control signal
+     */
+    Fan();
+
+    /**
+     * @brief Enable cooling fan
+     */
+    void On();
+
+    /**
+     * @brief Disable cooling fan
+     */
+    void Off();
+
+};
diff --git a/camera/raspberry/sensors/src/sick_front.cpp b/camera/raspberry/sensors/src/sick_front.cpp
new file mode 100644
index 0000000..ae0e0f0
--- /dev/null
+++ b/camera/raspberry/sensors/src/sick_front.cpp
@@ -0,0 +1,61 @@
+#include "sick_front.hpp"
+
+Sick_front::Sick_front(bool channel):channel(channel){
+    wiringPiSPISetupMode(channel, 500000, 3);
+    wiringPiSetup();
+    pinMode(GPIO_conversion_start, OUTPUT);
+    pinMode(GPIO_laser_enable, OUTPUT);
+}
+
+optional<unsigned int> Sick_front::Distance_avg(unsigned int measurement_count){
+    unsigned int sum = 0;
+    unsigned int successful_measurements = 0;
+    for(unsigned int i = 0; i < measurement_count; i++){
+        auto distance = Distance();
+        if(distance){
+            sum += *distance;
+            successful_measurements++;
+        }
+    }
+    if(sum == 0){
+        return {};
+    }
+    return sum/successful_measurements;
+}
+
+optional<unsigned int> Sick_front::Distance(){
+    auto raw = Conversion_raw();
+    float differential_raw = static_cast<float>(static_cast<uint16_t>(raw));
+    // Calculation of true voltage from reference voltage and ADC value
+    double voltage = differential_raw / (1 << 16) * reference_voltage;
+    //cout << "Voltage: " << hex << raw << dec << " : " <<differential_raw << " : " << setprecision(4) << voltage << " V" << setprecision(0);
+    // Out of Range
+    if((voltage < voltage_close) | (voltage > voltage_far)){
+        return {};
+    }
+
+    double range_percentage = (voltage-voltage_min)/(voltage_max-voltage_min);
+    double distance = round((range_max-range_min)*range_percentage+range_min);
+    return distance;
+}
+
+uint16_t Sick_front::Conversion_raw(){
+    // Start conversion
+    digitalWrite(GPIO_conversion_start, 1);
+    array<uint8_t, 2> received_data = {0x00, 0x00};
+    wiringPiSPIDataRW(channel, received_data.data(), 2);
+    // Start conversion
+    digitalWrite(GPIO_conversion_start, 0);
+    return ((received_data[0]<<8) | received_data[1]);
+}
+
+void Sick_front::Laser(bool state){
+    if(state){
+        // ON
+        pinMode(GPIO_laser_enable, INPUT);
+    } else {
+        // OFF
+        pinMode(GPIO_laser_enable, OUTPUT);
+        digitalWrite(GPIO_laser_enable, 0);
+    }
+}
diff --git a/camera/raspberry/sensors/src/sick_front.hpp b/camera/raspberry/sensors/src/sick_front.hpp
new file mode 100644
index 0000000..18b6f95
--- /dev/null
+++ b/camera/raspberry/sensors/src/sick_front.hpp
@@ -0,0 +1,134 @@
+#pragma once
+
+#include <iostream>
+#include <array>
+#include <string>
+#include <optional>
+#include <cmath>
+
+#include <iomanip>
+
+#include <wiringPi.h>
+#include <wiringPiSPI.h>
+
+using namespace std;
+
+/**
+Type: DT50-N2113
+**/
+class Sick_front{
+    /**
+     * @brief  Channel on which is ADC for front distance sensor connected
+     */
+    bool channel;
+
+    /**
+     * @brief WiringPi GPIO number of pin which start conversion by change 0->1
+     */
+    const int GPIO_conversion_start = 5;
+
+    /**
+     * @brief WiringPi GPIO number of pin which turn ON/OFF laser of sensor
+     */
+    const int GPIO_laser_enable = 4;
+
+        /**
+     * @brief   Minimal configured distance of sensor
+     */
+    const int range_min = 200;
+
+    /**
+     * @brief   Maximal configured distance of sensor
+     */
+    const int range_max = 10000;
+
+    /**
+     * @brief   Value of sense resistor in ohms used for current loop
+     */
+    const int sense_resistor = 150;
+
+    /**
+     * @brief   Current coresponding to minimal distance, configured in "range_min"
+     */
+    const float current_loop_min = 0.004;
+
+    /**
+     * @brief   Current coresponding to maximal distance, configured in "range_max"
+     */
+    const float current_loop_max = 0.020;
+
+    /**
+     * @brief   Current coresponding to state when minimal distance is exceeded
+     */
+    const float current_loop_close = 0.0035;
+
+    /**
+     * @brief   Current coresponding to state when maximal distance is exceeded
+     */
+    const float current_loop_far = 0.0205;
+
+    /**
+     * @brief   Voltage coresponding to maximal distance
+     */
+    const float voltage_max = current_loop_max*sense_resistor;
+
+    /**
+     * @brief   Voltage coresponding to minimal distance
+     */
+    const float voltage_min = current_loop_min*sense_resistor;
+
+    /**
+     * @brief   Voltage coresponding to state when maximal distance is exceeded
+     */
+    const float voltage_close = current_loop_close*sense_resistor;
+
+    /**
+     * @brief   Voltage coresponding to state when minimal distance is exceeded
+     */
+    const float voltage_far = current_loop_far*sense_resistor;
+
+    /**
+     * @brief  Reference voltage of ADC
+     */
+    const float reference_voltage = 3.308f;
+
+public:
+    /**
+     * @brief   Construct a new Sick_front object
+     *          Configure GPIO and SPI interface
+     *
+     * @param channel   Channel on which is sensor connected
+     */
+    Sick_front(bool channel = 1);
+
+    /**
+     * @brief Calculate distance measurted by distance sensor
+     *
+     * @return optional<unsigned int>   True and value in mm if in range, False otherwise
+     */
+    optional<unsigned int> Distance();
+
+    /**
+     * @brief Average multiple measurements of distance sensor output
+     *
+     * @param measurement_count         Amount of measurents to average
+     * @return optional<unsigned int>   Same as Distance()
+     */
+    optional<unsigned int> Distance_avg(unsigned int measurement_count = 8);
+
+    /**
+     * @brief           Turn ON or OFF laser of sensor
+     *
+     * @param state     New state of laser
+     */
+    void Laser(bool state);
+
+private:
+    /**
+     * @brief           Read raw ADC data from ADC
+     *
+     * @return uint16_t raw ADc data in 2's complement format
+     */
+    uint16_t Conversion_raw();
+
+};
diff --git a/camera/raspberry/sensors/src/sick_side.cpp b/camera/raspberry/sensors/src/sick_side.cpp
new file mode 100644
index 0000000..c689c45
--- /dev/null
+++ b/camera/raspberry/sensors/src/sick_side.cpp
@@ -0,0 +1,103 @@
+#include "sick_side.hpp"
+
+Sick_side::Sick_side(){
+    serial_descriptor = serialOpen("/dev/ttyS0", 9600);
+    if(serial_descriptor == -1){
+        cout << "Cannot open serial line for communication with side distance sensor" << endl;
+    }
+}
+
+uint8_t Sick_side::ID(){
+    Send_data({static_cast<uint8_t>(commands::Read), 0x01, 0x00});
+    //while(Data_available() < 12);
+    // Readout data which are in buffer from transmittion
+    //Receive();
+    // Read real received data from sensor
+    if(auto responce = Receive()){
+        return (*responce)[2];
+    } else {
+        return 0x00;
+    }
+}
+
+void Sick_side::Laser(bool state){
+    if(state){
+        // ON
+        Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x03});
+    } else {
+        // OFF
+        Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x02});
+    }
+
+    //Read responce to clear buffer
+    Receive();
+}
+
+optional<double> Sick_side::Distance(){
+    auto distance_data = Distance_data();
+    if(distance_data == 0x7fff){
+        // Out of measuring range
+        return {};
+    } else {
+        // Convert to 2'complement and then to mm
+        return static_cast<int16_t>(distance_data)/100.0;
+    }
+}
+
+uint16_t Sick_side::Distance_data(){
+    Send_data({static_cast<uint8_t>(commands::Command), 0xb0, 0x01});
+    if(auto responce = Receive()){
+        return (((*responce)[1]<<8) | (*responce)[2]);
+    } else {
+        return 0x7fff;
+    }
+}
+
+void Sick_side::Send_data(array<uint8_t, 3> data){
+    array<uint8_t, 6> packet = {static_cast<uint8_t>(signs::STX), data[0], data[1], data[2], static_cast<uint8_t>(signs::ETX), 0x00};
+    packet[5] = Calculate_BCC(packet);
+    Transmit(packet);
+}
+
+void Sick_side::Transmit(array<uint8_t, 6> data){
+    for(int i = 0; i < 6; i++){
+        serialPutchar(serial_descriptor, data[i]);
+    }
+}
+
+uint8_t Sick_side::Calculate_BCC(array<uint8_t, 6> data){
+    uint8_t BCC = 0;
+    // BCC is calculated only from bytes on positions 1,2,3
+    for(int byte_index = 1; byte_index < 4; byte_index++){
+        BCC ^= data[byte_index];
+    }
+    return BCC;
+}
+
+optional<array<uint8_t, 3>> Sick_side::Receive(){
+    while(Data_available() < 12);
+    // Readout data which are in buffer from transmittion
+    for(int i = 0; i < 6; i++){
+        serialGetchar(serial_descriptor);
+    }
+    // Receive true data
+    array<uint8_t, 6> received_data;
+    for(int i = 0; i < 6; i++){
+        received_data[i] = serialGetchar(serial_descriptor);
+    }
+    // Check for communication errors
+    if(static_cast<signs>(received_data[1]) == signs::NAK){
+        cout << error_codes[received_data[2]] << endl;
+        return {};
+    }
+    if(received_data[5] != Calculate_BCC(received_data)){
+        cout << "Received BCC is invalid" << endl;
+        return {};
+    }
+
+    return array<uint8_t, 3>({received_data[1], received_data[2], received_data[3]});
+}
+
+unsigned int Sick_side::Data_available(){
+    return serialDataAvail(serial_descriptor);
+}
diff --git a/camera/raspberry/sensors/src/sick_side.hpp b/camera/raspberry/sensors/src/sick_side.hpp
new file mode 100644
index 0000000..f77d016
--- /dev/null
+++ b/camera/raspberry/sensors/src/sick_side.hpp
@@ -0,0 +1,119 @@
+#pragma once
+
+#include <iostream>
+#include <array>
+#include <optional>
+#include <map>
+#include <string>
+
+#include <wiringSerial.h>
+
+using namespace std;
+
+/**
+Type: OD1-B100C50A15
+**/
+class Sick_side{
+
+    int serial_descriptor = 0;
+
+    enum class signs: uint8_t{
+        STX       = 0x02,
+        ETX       = 0x03,
+        ACK       = 0x06,
+        NAK       = 0x15,
+    };
+
+    map<uint8_t, string> error_codes{
+        {0x02, "Address is invalid"},
+        {0x04, "BCC values is invalid"},
+        {0x05, "Invalid command"},
+        {0x06, "Invalid setting value (out of spec)"},
+        {0x07, "Invalid setting value (out of range)"}
+    };
+
+    enum class commands: uint8_t{
+        Command     = 0x43, // Also used for reading out measured values
+        Write       = 0x57,
+        Read        = 0x52,
+    };
+
+public:
+    /**
+     * @brief   Construct a new Sick_side object
+     *          Opens serial line and save file descriptor
+     */
+    Sick_side();
+
+    /**
+     * @brief           Read sensor ID from memory of sensor
+     *                  Used for connection check
+     *
+     * @return uint8_t  Value 0x64, 0x00 if communication fails
+     */
+    uint8_t ID();
+
+    /**
+     * @brief           Turn ON or OFF laser of sensor
+     *
+     * @param state     New state of laser
+     */
+    void Laser(bool state);
+
+    /**
+     * @brief   Read and calculate distance from distance sensor
+     *          Distance is relative to sensors zero point
+     *
+     * @return optional<double> And and relative distance from zero point, false otherwise (Out of Range)
+     */
+    optional<double> Distance();
+
+private:
+
+    /**
+     * @brief Reads distance data from sensor register
+     *
+     * @return uint16_t Distance in 2's complement, range -5000 to 5000, coresponds to -50 mm to +50 mm
+     */
+    uint16_t Distance_data();
+
+    /**
+     * @brief       Transmit given data over serial to RS485 transmitter
+     *
+     * @param data  Data to send via serial line, header and footer are added
+     */
+    void Send_data(array<uint8_t, 3> data);
+
+    /**
+     * @brief           Calculates BCC for given packet of data
+     *                  BCC is XOR of bytes on index 1,2,3, remaning bytes are ignored
+     *
+     * @param data      Packet used for calculation
+     * @return uint8_t  Values of BCC
+     */
+    uint8_t Calculate_BCC(array<uint8_t, 6> data);
+
+    /**
+     * @brief       Transmits data over UART to RS485 transmitter
+     *              Data are also copied to input buffer due to half-duplex connection
+     *
+     * @param data  Data to transmit
+     */
+    void Transmit(array<uint8_t, 6> data);
+
+    /**
+     * @brief       Data received from sensor
+     *              Only usefull data are returned, without STX, ACK/NACK, BCC
+     *
+     * @return optional<array<uint8_t, 3>>  True and value if communication is valid, False otherwise
+     */
+    optional<array<uint8_t, 3>> Receive();
+
+    /**
+     * @brief Reads amopunt of data in input buffer
+     *
+     * @return unsigned int Number of bytes in input buffer
+     */
+    unsigned int Data_available();
+
+};
diff --git a/camera/runThisOnRPI.txt b/camera/runThisOnRPI.txt
new file mode 100644
index 0000000..c3cf388
--- /dev/null
+++ b/camera/runThisOnRPI.txt
@@ -0,0 +1,3 @@
+
+gst-launch-1.0 libcamerasrc ! videoscale method=0 add-borders=false ! video/x-raw, colorimetry=bt709, format=NV12, width=720, height=1280, framerate=30/1 ! videoflip method=clockwise ! jpegenc ! tcpserversink host=0.0.0.0 port=5000
+
diff --git a/camera/server.py b/camera/server.py
new file mode 100644
index 0000000..4606330
--- /dev/null
+++ b/camera/server.py
@@ -0,0 +1,93 @@
+import socket
+import cv2
+import numpy as np
+import os
+
+
+def variance_of_laplacian(image):
+	return cv2.Laplacian(image, cv2.CV_64F).var()
+
+
+
+os.environ["OPENCV_FFMPEG_CAPTURE_OPTIONS"]="rtsp_transport;tcp"
+
+gstreamerstr = "tcpclientsrc host=192.168.1.110 port=5000 ! jpegdec ! videoconvert ! appsink"
+
+def recvall(sock, count):
+    buf = b''
+    while count:
+        newbuf = sock.recv(count)
+        if not newbuf: return None
+        buf += newbuf
+        count -= len(newbuf)
+    return buf
+
+TCP_IP = '192.168.1.2'
+TCP_PORT = 5001
+
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+s.bind(('192.168.1.2', TCP_PORT))
+s.listen(True)
+conn, addr = s.accept()
+
+
+#if not capture.isOpened() : print("CANNOT OPEN STREAM")
+#capture = cv2.VideoCapture("vid.mp4")
+keypressNo = 1
+while(True):
+    capture = cv2.VideoCapture(gstreamerstr,cv2.CAP_GSTREAMER)
+    ret, frame = capture.read()
+    if not ret:
+        print('fail')
+        break
+
+    frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    variance = variance_of_laplacian(frame)
+
+    cv2.putText(frame, "{}: {:.2f}".format("Variance:", variance), (10, 30),
+		cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
+    cv2.imshow('frame',frame)
+    #cv2.waitKey(0)
+    
+    if cv2.waitKey(70) == ord('v'):
+        conn.send("n".encode())
+
+        length = conn.recv(3)
+        stringData = recvall(conn, int(length))
+        stringData=stringData.decode()
+        print(stringData)
+
+        frames=[]
+        maxVarIdx = 0
+        maxVar = 0
+        for i in range(30):
+            ret, frame = capture.read()
+            frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+            frames.append(frame)
+            var = variance_of_laplacian(frame)
+            if var > maxVar:
+                maxVar = var
+                maxVarIdx = i
+
+                conn.send("n".encode())
+                length = conn.recv(3)
+                stringData = recvall(conn, int(length))
+                stringData=stringData.decode()
+                print(stringData)
+
+            cv2.waitKey(40)
+            #test to see if the image with the highest variance gets picked
+            #cv2.putText(frame, "{}: {:.2f}".format("Variance:", var), (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
+            #writeString="imgtest"+str(keypressNo)+"-"+str(i)+".jpg"
+            #cv2.imwrite(writeString, frames[i])
+        writeString="/home/palko/Documents/Documents/Diplomka/src/online/img"+str(keypressNo)+".png"
+        fileString = "/home/palko/Documents/Documents/Diplomka/src/online/img" + str(keypressNo) + ".json"
+        myfile = open(fileString, "w")
+        a=myfile.write(stringData)
+        myfile.close()
+        cv2.imwrite(writeString, frames[maxVarIdx])
+        keypressNo=keypressNo+1
+        
+
+capture.release()
+cv2.destroyAllWindows()
diff --git a/camera/server2.py b/camera/server2.py
new file mode 100644
index 0000000..459f51f
--- /dev/null
+++ b/camera/server2.py
@@ -0,0 +1,96 @@
+import socket
+import cv2
+import numpy as np
+import os
+
+
+def variance_of_laplacian(image):
+	return cv2.Laplacian(image, cv2.CV_64F).var()
+
+
+
+os.environ["OPENCV_FFMPEG_CAPTURE_OPTIONS"]="rtsp_transport;tcp"
+
+gstreamerstr = "tcpclientsrc host=192.168.1.110 port=5000 ! jpegdec ! videoconvert ! appsink"
+
+def recvall(sock, count):
+    buf = b''
+    while count:
+        newbuf = sock.recv(count)
+        if not newbuf: return None
+        buf += newbuf
+        count -= len(newbuf)
+    return buf
+
+TCP_IP = '192.168.1.2'
+TCP_PORT = 5001
+
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+s.bind(('192.168.1.2', TCP_PORT))
+s.listen(True)
+conn, addr = s.accept()
+
+
+#if not capture.isOpened() : print("CANNOT OPEN STREAM")
+#capture = cv2.VideoCapture("vid.mp4")
+keypressNo = 1
+while(True):
+    capture = cv2.VideoCapture(gstreamerstr,cv2.CAP_GSTREAMER)
+    ret, frame = capture.read()
+    if not ret:
+        print('fail')
+        break
+
+    frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    variance = variance_of_laplacian(frame)
+
+    cv2.putText(frame, "{}: {:.2f}".format("Variance:", variance), (10, 30),
+		cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
+    cv2.imshow('frame',frame)
+    #cv2.waitKey(0)
+    
+   
+    
+    while(True):
+        frames=[]
+        maxVarIdx = 0
+        maxVar = 0
+        conn.send("n".encode())
+    	length = conn.recv(3)
+    	stringData = recvall(conn, int(length))
+    	stringData=stringData.decode()
+    	print(stringData)
+    	maxVar=0
+    	var=0
+        for i in range(30):
+            ret, frame = capture.read()
+            frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+            frames.append(frame)
+            cv2.imshow("frame",frame)
+            var = variance_of_laplacian(frame)
+            if var > maxVar:
+                maxVar = var
+                maxVarIdx = i
+
+                conn.send("n".encode())
+                length = conn.recv(3)
+                stringData = recvall(conn, int(length))
+                stringData=stringData.decode()
+                print(stringData)
+
+            cv2.waitKey(40)
+            #test to see if the image with the highest variance gets picked
+            #cv2.putText(frame, "{}: {:.2f}".format("Variance:", var), (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
+            #writeString="imgtest"+str(keypressNo)+"-"+str(i)+".jpg"
+            #cv2.imwrite(writeString, frames[i])
+        writeString="/home/palko/Documents/Documents/Diplomka/src/online/img"+str(keypressNo)+".png"
+        fileString = "/home/palko/Documents/Documents/Diplomka/src/online/img" + str(keypressNo) + ".json"
+        myfile = open(fileString, "w")
+        a=myfile.write(stringData)
+        myfile.close()
+        cv2.imwrite(writeString, frames[maxVarIdx])
+        keypressNo=keypressNo+1
+        
+
+capture.release()
+cv2.destroyAllWindows()