diff --git a/Examples/Adafruit_PCA9685_servo_driver.py b/Examples/Adafruit_PCA9685_servo_driver.py
new file mode 100644
index 0000000..60e55ba
--- /dev/null
+++ b/Examples/Adafruit_PCA9685_servo_driver.py
@@ -0,0 +1,28 @@
+import pwmio
+from Adafruit_PCA9685 import PCA9685
+from time import sleep
+
+def angle_to_pulse(angle):
+    # Calculate pulse length as derivative of the angle.
+    pulse = int(round(41 / 9 * angle / 2 + 100, 0))
+    return pulse
+
+# Define driver object
+pwm = PCA9685()
+
+# Set frequency
+pwm.set_pwm_freq(50)
+
+# Channel of the driver board (0-15)
+channel = 0
+
+# Minimum and maximum pulse lengths. 100-510 translates to 0-180 degree.
+# The formula for angel to pulse length is: 41/9 * <angle> /2 +100. MUST BE ROUNDED en set to INT()
+min_pulse = 100  # Min pulse length = 0deg
+max_pulse = 510  # Max pulse length = 180deg
+
+# Example with <set_angle> parameter
+set_angle = 180
+print("Angle:", set_angle, "> Pulse:", angle_to_pulse(set_angle))
+pwm.set_pwm(channel, 0,  angle_to_pulse(set_angle))
+
diff --git a/Examples/Adafruit_Servokit_servo_driver.py b/Examples/Adafruit_Servokit_servo_driver.py
new file mode 100644
index 0000000..c38db1d
--- /dev/null
+++ b/Examples/Adafruit_Servokit_servo_driver.py
@@ -0,0 +1,38 @@
+from time import sleep
+from adafruit_servokit import ServoKit
+
+kit = ServoKit(channels=16)
+
+# Control the minimum and maximum range of the servo.
+# min_pulse (int) – The minimum pulse width of the servo in microseconds.
+# max_pulse (int) – The maximum pulse width of the servo in microseconds.
+kit.servo[0].set_pulse_width_range(500, 2500)
+
+# Pulse width expressed as fraction between 0.0 (`min_pulse`) and 1.0 (`max_pulse`).
+# For conventional servos, corresponds to the servo position as a fraction
+# of the actuation range. Is None when servo is disabled (pulsewidth of 0ms)
+# kit.servo[0].fraction = 0.5
+
+# property angle: float | None
+# The servo angle in degrees. Must be in the range 0 to actuation_range.
+# Is None when servo is disabled.
+kit.servo[0].angle = 180
+
+# property throttle: float
+# How much power is being delivered to the motor.
+# Values range from -1.0 (full throttle reverse) to 1.0 (full throttle forwards.)
+# 0 will stop the motor from spinning.
+# kit.continuous_servo[0].throttle = 1
+
+# property actuation range: float | None
+# The servo angle in degrees. Must be in the range 0 to actuation_range.
+# Is None when servo is disabled
+#kit.servo[0].actuation_range = 120
+
+
+
+
+
+
+
+
diff --git a/calibrate.py b/Old/calibrate.py
similarity index 100%
rename from calibrate.py
rename to Old/calibrate.py
diff --git a/min-max-demo.py b/Old/min-max-demo.py
similarity index 74%
rename from min-max-demo.py
rename to Old/min-max-demo.py
index 81d383f..df266ac 100644
--- a/min-max-demo.py
+++ b/Old/min-max-demo.py
@@ -5,9 +5,11 @@
 from gpiozero import Servo
 from time import sleep
 
-myGPIO = 17
+from gpiozero.pins.pigpio import PiGPIOFactory
 
-servo = Servo(myGPIO)
+my_factory = PiGPIOFactory()
+myGPIO = 12
+servo = Servo(myGPIO, pin_factory=my_factory)
 
 while True:
     servo.mid()
diff --git a/precision_positioning.py b/Old/precision_positioning.py
similarity index 100%
rename from precision_positioning.py
rename to Old/precision_positioning.py
diff --git a/control_functions.py b/control_functions.py
new file mode 100644
index 0000000..271a64f
--- /dev/null
+++ b/control_functions.py
@@ -0,0 +1,85 @@
+from adafruit_hcsr04 import HCSR04 as hcsr04
+import board
+import data_transfer_functions as dt
+from adafruit_servokit import ServoKit
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.integrate import odeint
+import numpy as np
+import matplotlib.pyplot as plt
+
+# Variables to control sensor
+TRIGGER_PIN = board.D4
+ECHO_PIN = board.D17
+TIMEOUT = 0.1
+MIN_DISTANCE = 4
+MAX_DISTANCE = 40
+
+# Variables to control logging.
+LOG = True
+SCREEN = True
+
+# PID measurements
+time = 0
+integral = 0
+time_prev = -1e-6
+e_prev = 0
+
+
+def initial():
+    ...
+
+
+def read_distance_sensor(fixed_file_stamp):
+
+    with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):
+
+        while True:
+            try:
+                distance = sonar.distance
+                if MIN_DISTANCE < distance < MAX_DISTANCE:
+
+                    dt.log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None
+                    print("Distance: ", sonar.distance) if SCREEN else None
+                else:
+                    dt.log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG else None
+                    print("Distance: ", distance) if SCREEN else None
+
+            except RuntimeError:
+                dt.log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG else None
+                print("Timeout") if SCREEN else None
+
+def read_setpoint():
+    ...
+
+
+def calculate_velocity():
+
+
+
+def PID_calculations(Kp, Ki, Kd, setpoint, measurement):
+
+    global cur_time, integral, prev_time, prev_error
+    offset_value = 320
+    # PID calculations
+    error = setpoint - measurement
+    P = Kp * error
+    integral = integral + Ki * error * (cur_time - prev_time)
+    D = Kd * (error - prev_error) / (cur_time - prev_time)
+    PID_result = offset_value + P + integral + D
+    prev_error = error
+    prev_time = cur_time
+    return PID_result
+
+
+def calculate_new_servo_pos():
+    ...
+
+
+def send_data_to_servo():
+
+    kit = ServoKit(channels=16)
+    kit.servo[0].set_pulse_width_range(500, 2500)
+
+    kit.servo[0].angle = 180
+
diff --git a/data writing.py b/data writing.py
deleted file mode 100644
index 6246699..0000000
--- a/data writing.py	
+++ /dev/null
@@ -1,10 +0,0 @@
-def sensor_raw_data():
-    pass
-
-
-def pid_data():
-    pass
-
-
-def servo_raw_data():
-    pass
\ No newline at end of file
diff --git a/data_transfer_functions.py b/data_transfer_functions.py
new file mode 100644
index 0000000..b30820a
--- /dev/null
+++ b/data_transfer_functions.py
@@ -0,0 +1,16 @@
+import csv
+from datetime import datetime
+
+# Create timestamp strings for logs and screen
+def timestamper():
+    log_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
+    file_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')
+    return log_timestamp, file_timestamp
+
+# Write data to any of the logfiles
+def log_data(fixed_file_stamp: str, data_file: str, data_line: float, remark: str|None):
+    log_stamp, _ = timestamper()
+
+    with open("pid-balancer_" + data_file + "_data_" + fixed_file_stamp + ".csv", "a") as data_file:
+        data_writer = csv.writer(data_file)
+        data_writer.writerow([log_stamp,data_line, remark])
\ No newline at end of file
diff --git a/functions.py b/functions.py
deleted file mode 100644
index 97fb9f1..0000000
--- a/functions.py
+++ /dev/null
@@ -1,39 +0,0 @@
-import gpiozero as gp
-from time import sleep
-
-
-def system_test():
-    ...
-# todo Test niet mogelijk. Verwijderen?
-
-
-def initial():
-    ...
-
-
-def calibrate():
-    ...
-
-
-def read_distance_sensor():
-    ...
-
-
-def read_setpoint():
-    ...
-
-
-def calculate_position():
-    ...
-
-
-def process_pid():
-    ...
-
-
-def calculate_new_servo_pos():
-    ...
-
-
-def send_data_to_servo():
-    ...
\ No newline at end of file
diff --git a/main.py b/main.py
index 967ab7b..b199b4d 100644
--- a/main.py
+++ b/main.py
@@ -1,9 +1,18 @@
-import functions
-from functions import initial
+import control_functions as cf
+import data_transfer_functions as dt
+import plotter_functions as pf
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.integrate import odeint
+import numpy as np
+import matplotlib.pyplot as plt
+import data_transfer_functions as dt
 
-aaa = initial()
+_, fixed_file_stamp = dt.timestamper()
+
+
+cf.read_distance_sensor(fixed_file_stamp)
 
-# todo Moet nog formule in
 
 
 
diff --git a/plotter.py b/plotter_functions.py
similarity index 88%
rename from plotter.py
rename to plotter_functions.py
index 342acae..df4dc39 100644
--- a/plotter.py
+++ b/plotter_functions.py
@@ -4,3 +4,4 @@ def read_data_file():
 
 def plot_graphs():
     pass
+