diff --git a/.idea/misc.xml b/.idea/misc.xml
index e158a33..116cb80 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -3,5 +3,5 @@
   <component name="Black">
     <option name="sdkName" value="Python 3.11 (pid-balancer)" />
   </component>
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.11 (pid-balancer)" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.11 (pid-balancer) (2)" project-jdk-type="Python SDK" />
 </project>
\ No newline at end of file
diff --git a/control_functions.py b/control_functions.py
index 26f6ef3..2239d47 100644
--- a/control_functions.py
+++ b/control_functions.py
@@ -1,6 +1,6 @@
-from adafruit_hcsr04 import HCSR04 as hcsr04        # PWM driver board for servo
-import board                                        # PWM driver board for servo
-from adafruit_servokit import ServoKit              # Servo libraries
+from adafruit_hcsr04 import HCSR04 as hcsr04        # Ultrasound sensor
+import board                                        # General board pin mapper
+from adafruit_servokit import ServoKit              # Servo libraries for PWM driver board
 import adafruit_pcf8591.pcf8591 as PCF              # AD/DA converter board for potentiometer
 from adafruit_pcf8591.analog_in import AnalogIn     # Analogue in pin library
 from adafruit_pcf8591.analog_out import AnalogOut   # Analogue out pin library
@@ -35,6 +35,12 @@ DEBUG: bool = False         # More data to display
 # Control the number of samples for single measurement
 MAX_SAMPLES = 10
 
+# Control the number of samples for the potentiometer
+PCF_VALUE = 65535
+POT_MAX = 65280
+POT_MIN = 256
+POT_INTERVAL = 0.1
+
 # Variables to assist PID calculations
 current_time: float = 0
 integral: float = 0
@@ -69,7 +75,7 @@ def read_distance_sensor(file_stamp):
 
     # Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long distance)
     # Return the mean timestamp and median distance.
-    with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):
+    with hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar:
         samples: int = 0
         max_samples: int = MAX_SAMPLES
         timestamp_last: float = 0.0
@@ -108,25 +114,26 @@ def read_distance_sensor(file_stamp):
 
 def read_setpoint():
 
-    # Setup the internal DAC to output a voltage and then measure it with the first ADC channel.
-    # Wiring: Connect the DAC output to the first ADC channel, in addition to the normal power and I2C connections
-
     i2c = board.I2C()
     pcf = PCF.PCF8591(i2c)
-
     pcf_in_0 = AnalogIn(pcf, PCF.A0)
     pcf_out = AnalogOut(pcf, PCF.OUT)
+    pcf_out.value = PCF_VALUE
 
     while True:
-        print("Setting out to ", 65535)
-        pcf_out.value = 65535
-        raw_value = pcf_in_0.value
-        scaled_value = (raw_value / 65535) * pcf_in_0.reference_voltage
+        raw_value: float = pcf_in_0.value
+        scaled_value: float = (raw_value / PCF_VALUE) * pcf_in_0.reference_voltage
+        # Calculate angle in reference to raw pot values
+        angle: float = ((180 - 0) / (POT_MAX - POT_MIN)) * (raw_value - POT_MIN)
 
-        print("Pin 0: %0.2fV" % (scaled_value))
-        print("")
-        time.sleep(1)
+        if SCREEN:
+            print('pin 0 ', pcf.read(0))
+            print('raw_value ',raw_value)
+            print("pin 0: %0.2fV" % scaled_value)
+            print(angle)
 
+        time.sleep(POT_INTERVAL)
+        send_data_to_servo(set_angle=angle)
 
 def calculate_velocity():
     ...
@@ -160,7 +167,9 @@ def calculate_new_servo_position():
     ...
 
 
-def send_data_to_servo():
+def send_data_to_servo(set_angle):
 
-    KIT.servo[0].angle = 180 # Set angle
+    KIT.servo[0].angle = set_angle # Set angle
 
+read_distance_sensor(file_stamp=123)
+read_setpoint()
\ No newline at end of file
diff --git a/main.py b/main.py
index 97337b3..5cca24a 100644
--- a/main.py
+++ b/main.py
@@ -1,7 +1,5 @@
-import control_functions as cf
-import plotter_functions as pf
-import twin_functions as tw
 from datetime import datetime
+import control_functions as cf
 
 file_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')