diff --git a/.project b/.project
new file mode 100644
index 0000000..d18e839
--- /dev/null
+++ b/.project
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>efs-can-sensor-clustor</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+	</buildSpec>
+	<natures>
+	</natures>
+</projectDescription>
diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 9bc884e..90b21d0 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="146207565932111761" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1256082460173696897" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -16,7 +16,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="146207565932111761" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1256082460173696897" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/Drivers/EPC611 Driver/epc611.cpp b/Drivers/EPC611 Driver/epc611.cpp
new file mode 100644
index 0000000..35efd5e
--- /dev/null
+++ b/Drivers/EPC611 Driver/epc611.cpp	
@@ -0,0 +1,142 @@
+/*
+ * epc611.cpp
+ *
+ *  Created on: Sep 29, 2024
+ *      Author: Playdream
+ */
+
+#include "epc611.hpp"
+#include "stm32l4xx_hal_conf.h"
+
+EPC611::EPC611(SPI_HandleTypeDef *spi, SPI_HandleTypeDef *nss_gpio_type, uint16_t nss_pin_out, SPI_HandleTypeDef *data_rdy_gpio_type, uint16_t data_rdy_pin_out) {
+	epc_spi = spi;
+	gpio_nss = nss_gpio_type;
+	nss_pin = nss_pin_out;
+	gpio_data_rdy = data_rdy_gpio_type;
+	data_rdy_pin = data_rdy_pin_out;
+}
+
+uint16_t EPC611::sendRecv(uint16_t data) {
+	HAL_GPIO_WritePin(gpio_nss,nss_pin,GPIO_PIN_RESET);
+	uint8_t send[2] = {data>>8, data&0x00FF}; // MSB first
+	uint8_t received[2] = {0};
+	HAL_SPI_TransmitReceive(&hspi1, send, received, 2, SPI_TIMEOUT);
+	HAL_GPIO_WritePin(gpio_nss,nss_pin,GPIO_PIN_SET);
+	return received[0] <<8 | (received[1]);
+}
+
+uint16_t EPC611::write(uint8_t data,uint8_t address) {
+	uint16_t write = EPC_WRITE | data | ((address&0x1F) << 8);
+	return poll(write);
+}
+uint16_t EPC611::read(uint8_t address) {
+	uint16_t write = EPC_READ | ((address&0x1F) << 8);
+	return poll(write);
+}
+uint16_t EPC611::pageSelect(uint8_t page) {
+	uint16_t write = EPC_PAGE_SELECT | ((page&0x07) << 8);
+	return poll(write);
+}
+uint16_t EPC611::reset() {
+	return poll(EPC_RESET);
+}
+uint16_t EPC611::quit() {
+	return poll(EPC_QUIT);
+}
+uint16_t EPC611::nop() {
+	return poll(EPC_NOP);
+}
+
+// For polling read and write (Done after an epcRead or epcWrite)
+/*uint16_t EPC611::poll(uint16_t last_response, int attempts) {
+	uint16_t new_response = last_response;
+	if(last_response == EPC_WRITE_NOT_DONE || last_response == EPC_READ_NOT_DONE) {
+		for(int i=0;i<attempts && (new_response == EPC_WRITE_NOT_DONE || new_response == EPC_READ_NOT_DONE);i++) {
+			new_response = EPC611::epcNop();
+		}
+	}
+	return new_response;
+}*/
+uint16_t EPC611::poll(uint16_t data)
+{
+	uint16_t status = sendRecv(data);
+	while(status == EPC_WRITE_NOT_DONE || status == EPC_READ_NOT_DONE)
+	{
+		status = sendRecv(data);
+	}
+	return status;
+}
+
+void EPC611::startTIM() {
+	// Startup
+	while(nop() != EPC_IDLE);
+
+}
+
+void EPC611:startUHD()
+{
+	// Set mode
+	pageSelect(4);
+	write(0x38, 0x12);
+	write(0x2F, 0x15); // Set 4 DCS Ultra High-Dynamic (UHD) mode
+
+	// Set frequency
+	write(0x01, 0x05); // Set frequency to 10 MHz
+
+	// Set integration time
+	pageSelect(5);
+	write(0x01, 0x00);
+	write(0xFF, 0x01);
+	write(0x03, 0x02);
+	write(0xFF, 0x03);
+	write(0x07, 0x04);
+	write(0xFF, 0x05);
+	write(0x0F, 0x06);
+	write(0xFF, 0x07);
+	write(0x1F, 0x08);
+	write(0xFF, 0x09);
+	write(0x3F, 0x0A);
+	write(0xFF, 0x0B);
+	write(0x7F, 0x0C);
+	write(0xFF, 0x0D);
+	write(0xFF, 0x0E);
+	write(0xFF, 0x0F);
+
+	nop();
+
+	// Start measurement
+	pageSelect(2);
+	write(0x01, 0x18); // Set trigger
+	nop();
+}
+
+// Returns 8 rows of sums for the 8x8 TOF sensor
+void EPC611::getFrameUHD(uint16_t DCS_frames[4][8]) // Gets 4 frames because that's what the chip does for some reason
+{
+	// Start frame measurement
+	pageSelect(2);
+	for(int j=0; j<4; j++) { // repeat for DCS0 - DCS3
+		for(int i=0; i<4;i++) { // repeat for 4 double-rows
+			uint16_t status = read(0x15);
+
+			// Wait for data
+			while(!dataReady())
+			{
+				status = read(0x15);
+				nop();
+			}
+			uint16_t data1 = read(0x14);
+			uint16_t data2 = read(0x14);
+			uint16_t data3 = read(0x14);
+			uint16_t data4 = read(0x14);
+			nop();
+			DCS_frames[j][UHD_READOUT[i*2]] = data1 << 8 | data2;
+			DCS_frames[j][UHD_READOUT[i*2+1]] = data3 << 8 | data4;
+		}
+	}
+}
+
+bool EPC611::dataReady()
+{
+	return (HAL_GPIO_ReadPin(gpio_data_rdy,data_rdy_pin) == GPIO_PIN_SET);
+}
diff --git a/Drivers/EPC611 Driver/epc611.hpp b/Drivers/EPC611 Driver/epc611.hpp
new file mode 100644
index 0000000..5da3427
--- /dev/null
+++ b/Drivers/EPC611 Driver/epc611.hpp	
@@ -0,0 +1,76 @@
+/*
+ * epc611.h
+ *
+ *  Created on: Sep 29, 2024
+ *      Author: Playdream
+ */
+
+#ifndef INC_EPC611_HPP_
+#define INC_EPC611_HPP_
+
+const uint16_t EPC_NOP = 0x0;
+const uint16_t EPC_QUIT = 0x6000;
+const uint16_t EPC_RESET = 0xC000;
+
+// EPC Command Flags
+const uint16_t EPC_READ = 0x2000;
+const uint16_t EPC_WRITE = 0x4000;
+const uint16_t EPC_PAGE_SELECT = 0x8000;
+
+// EPC Response Codes
+const uint16_t EPC_IDLE = 0x0;
+const uint16_t EPC_SPI_NOT_READY = 0xFFFF;  // Poll with NOP until you get NOP
+const uint16_t EPC_SYS_NOT_READY = 0xEBFF;  // Poll with NOP until you get NOP
+const uint16_t EPC_ERROR = 0xF58E;          // You messed up something
+const uint16_t EPC_QUIT_RESPONSE = 0xE38E;
+const uint16_t EPC_WRITE_NOT_DONE = 0xCCCC; // Poll with NOP until you get WRITE_DONE
+const uint16_t EPC_READ_NOT_DONE = 0x7333;  // Poll with NOP until you get READ_DONE
+
+// EPC Response Flags
+const uint16_t EPC_FLAG = 0xE000; // 1110 0000 0000 0000
+const uint16_t EPC_READ_DONE = 0x2000;
+const uint16_t EPC_WRITE_DONE = 0x4000;
+const uint16_t EPC_PAGE_RESPONSE = 0x8000;
+
+const uint32_t SPI_TIMEOUT = 10; // ms
+
+const int UHD_READOUT[8] = {3,4,2,5,1,6,0,7}; // UHD readout is in a weird order
+
+enum EPCMode { // [TODO] Implement a use for the mode so it can all be done by 1 function
+	NONE,
+	TIM,
+	UHD
+};
+
+class EPC611 {
+	public:
+
+		EPC611(SPI_HandleTypeDef *spi, SPI_HandleTypeDef *nss_gpio_type, uint16_t nss_pin_out, SPI_HandleTypeDef *data_rdy_gpio_type, uint16_t data_rdy_pin_out);
+
+		uint16_t sendRecv(uint16_t data);
+		uint16_t write(uint8_t data,uint8_t address);
+		uint16_t read(uint8_t address);
+		uint16_t pageSelect(uint8_t page);
+		uint16_t reset();
+		uint16_t quit();
+		uint16_t nop();
+		uint16_t poll(uint16_t data);
+		void startTIM();
+		void startUHD();
+		void getFrameUHD(uint16_t DCS_frames[][8]);
+
+		bool dataReady();
+
+	private:
+		enum EPCMode mode = NONE; // [TODO] Add a function that uses this to determine what type of frames to grab
+		SPI_HandleTypeDef *epc_spi;
+
+		GPIO_TypeDef *gpio_nss;
+		uint16_t nss_pin;
+
+		GPIO_TypeDef *gpio_data_rdy;
+		uint16_t data_rdy_pin;
+
+};
+
+#endif /* INC_EPC611_HPP_ */