diff --git a/CleanIt-SC/Headers/proximity.h b/CleanIt-SC/Headers/proximity.h
index 2a24a19..09a4e3c 100644
--- a/CleanIt-SC/Headers/proximity.h
+++ b/CleanIt-SC/Headers/proximity.h
@@ -1,11 +1,13 @@
 #ifndef PROXIMITY_H
 #define PROXIMITY_H
 #include <MKL25Z4.h>
+#include <cstdio>
+#include<math.h>
 #include "delays.h"
 
 void adc_init(void);
 void proximity_init(void);
-int proximity_read(int sensorValue);
+int proximity_read_average(int sensorValue, int meassurments);
 
 //adc_init()
 // Initialized clock for ADC0 and configures necessary register.
@@ -20,21 +22,28 @@ void adc_init(void){
 void proximity_init(void){
     adc_init();
     // Initialize clock to PORTB
-    SIM->SCGC4 |= 0x400;
+    SIM->SCGC5 |= 0x400;
     // Set PORT B0 & PORT B1 as analogue input
     PORTB->PCR[0] = 0;
     PORTB->PCR[1] = 1;
     
 }
 //proximity_read()
-// @param  integer corresponding to ADC channel. Should be 8 or 9.
+// @param  sensorChannel: int corresponding to ADC channel. Should be 8 or 9.
+// @param  meassurments: measurments to average
 // @returns int value corresponding to the sensor value
-int proximity_read(int sensorChannel){
-    int readValue;
-    ADC0->SC1[0] = sensorChannel;
-    while(!(ADC0->SC1[0] & 0x80)) { } /* wait for COCO */
-    readValue = ADC0->R[0]; 
-    return readValue;
+int proximity_read_average(int sensorChannel, int meassurments){
+    int rawValue,sum;
+    int valueCm;
+    sum = 0;
+    for (int i = 0; i < meassurments; i++){
+        ADC0->SC1[0] = sensorChannel;
+        while(!(ADC0->SC1[0] & 0x80)) { } /* wait for COCO */
+        sum = sum + ADC0->R[0];
+        delayMs(15);
+    }
+    rawValue = sum/meassurments;
+    return rawValue;
 }
 
 #endif
\ No newline at end of file
diff --git a/CleanIt-SC/main.cpp b/CleanIt-SC/main.cpp
index 98c6a21..c546d0b 100644
--- a/CleanIt-SC/main.cpp
+++ b/CleanIt-SC/main.cpp
@@ -7,12 +7,13 @@
 // main() runs in its own thread in the OS
 int main(){ 
     printf("Program Begin\n");
+    tpm_init();
     proximity_init();
     int leftSensor, rightSensor;
     while (true) {
-        //leftSensor = proximity_read(8);
-        printf("Loop\n");
-        //printf("%d\n", leftSensor);
+        leftSensor = proximity_read_average(8,20);
+        printf("%d\n", leftSensor);
+        tpm_delayMs(1000);
         
     }
 }