diff --git a/firmware/src/dccd/brakes.cpp b/firmware/src/dccd/brakes.cpp
new file mode 100644
index 0000000..63c0f38
--- /dev/null
+++ b/firmware/src/dccd/brakes.cpp
@@ -0,0 +1,71 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "brakes.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::Brakes::Brakes(void)
+{
+	return;
+}
+
+dccd::Brakes::~Brakes(void)
+{
+	return;
+}
+
+void dccd::Brakes::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	this->mode = OPEN;
+	this->is_active = 0;
+	this->is_new_mode = 0;
+}
+
+void dccd::Brakes::cfg_debounce(uint16_t dbnc_time)
+{
+	this->hardware->brakes.dbnc_lim = dbnc_time;
+}
+
+uint8_t dccd::Brakes::process(void)
+{
+	if(this->hardware->brakes.state > 0)
+	{
+		this->is_active = 1;
+	}
+	else
+	{
+		this->is_active = 0;
+	}
+	
+	return this->is_active;
+}
+
+Brakes::bmode_t dccd::Brakes::cycle_mode(void)
+{
+	switch(this->mode)
+	{
+		case OPEN:
+		this->mode = KEEP;
+		
+		case KEEP:
+		this->mode = LOCK;
+		
+		case LOCK:
+		this->mode = OPEN;
+		
+		default:
+		this->mode = OPEN;
+	}
+	
+	this->is_new_mode = 1;
+	
+	return this->mode;
+}
+
+/**** Private function definitions ***/
diff --git a/firmware/src/dccd/brakes.h b/firmware/src/dccd/brakes.h
new file mode 100644
index 0000000..6da5a7a
--- /dev/null
+++ b/firmware/src/dccd/brakes.h
@@ -0,0 +1,47 @@
+#ifndef DCCD_BRAKES_H_
+#define DCCD_BRAKES_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class Brakes
+{
+	public:
+	typedef enum
+	{
+		OPEN = 0,
+		KEEP = 1,
+		LOCK = 2
+	}bmode_t;
+	
+	Brakes(void);
+	~Brakes(void);
+	
+	void init(dccd::DccdHw* dccd_hw);	
+	
+	bmode_t mode;
+	uint8_t is_active;
+	uint8_t is_new_mode;
+	
+	void cfg_debounce(uint16_t dbnc_time);
+	bmode_t cycle_mode(void);
+	uint8_t process(void);
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_BRAKES_H_ */
\ No newline at end of file
diff --git a/firmware/src/dccd/coil_reg.cpp b/firmware/src/dccd/coil_reg.cpp
new file mode 100644
index 0000000..4326f60
--- /dev/null
+++ b/firmware/src/dccd/coil_reg.cpp
@@ -0,0 +1,112 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "coil_reg.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::CoilReg::CoilReg(void)
+{
+	return;
+}
+
+dccd::CoilReg::~CoilReg(void)
+{
+	return;
+}
+
+void dccd::CoilReg::init(dccd::DccdHw* dccd_hw)
+{	
+	this->lock_current = 4500;
+	this->ref_resistance = 1500;
+	this->cc_max_resistance = 2000;
+	this->cc_min_resistance = 1000;
+	this->target_force = 0;
+	this->set_force = 0;
+	this->disable_protection = 0;
+	
+	// Config output protection
+	this->hardware->out_voltage.under_treshold = 0;
+	this->hardware->out_voltage.over_treshold = 0xFFFF;
+	this->hardware->out_voltage.hold_time = 200;
+	this->hardware->out_voltage.cooldown_time = 1000;
+	this->hardware->out_voltage.auto_reset = 1;
+}
+
+void dccd::CoilReg::process(void)
+{	
+	// Fix target force
+	if(this->target_force > 100) this->target_force = 100;
+	
+	// Check protection
+	if((this->disable_protection==0)&&(this->hardware->out_voltage.fault!=0))
+	{
+		// HiZ
+		this->hardware->outreg.write_voltage(0);
+		this->hardware->outreg.write_current(0);
+		this->hardware->outreg.write_on(0);
+		this->hardware->out_voltage.under_treshold = 0;
+		this->set_force = -1;
+		return;
+	};
+
+	// Check for target changes
+	if(this->target_force == this->set_force) return;
+	
+	// Update set force
+	this->set_force = this->target_force;
+	if(this->set_force < 0)
+	{
+		// HiZ
+		this->hardware->outreg.write_voltage(0);
+		this->hardware->outreg.write_current(0);
+		this->hardware->outreg.write_on(0);
+		this->hardware->out_voltage.under_treshold = 0;
+	}
+	else if(this->set_force == 0)
+	{
+		// Open
+		this->hardware->outreg.write_voltage(0);
+		this->hardware->outreg.write_current(0);
+		this->hardware->outreg.write_on(1);
+		this->hardware->out_voltage.under_treshold = 0;
+	}
+	else
+	{
+		// Calculate current and voltage settings
+		this->hardware->outreg.write_current(util::percent_of((uint8_t)this->set_force, this->lock_current));
+		uint16_t resistance = this->ref_resistance;
+		if(this->hardware->outreg.cc_mode_en) resistance = this->cc_max_resistance;
+		this->hardware->outreg.write_voltage(util::sat_mul_kilo(this->hardware->outreg.read_current(), resistance));
+		this->hardware->outreg.write_on(1);
+		// Calculate min. voltage
+		if(this->disable_protection==0) this->hardware->out_voltage.under_treshold = util::sat_mul_kilo(this->hardware->outreg.read_current(), cc_min_resistance);
+	}
+}
+
+uint8_t dccd::CoilReg::is_fault(void)
+{
+	return this->hardware->out_voltage.fault;
+}
+
+uint8_t dccd::CoilReg::read_act_force(void)
+{
+	if(this->set_force < 0) return 0;
+	else return (uint8_t)this->set_force;
+}
+
+void dccd::CoilReg::cfg_set_cv_mode(void)
+{
+	this->hardware->outreg.cc_mode_en = 0;
+}
+
+void dccd::CoilReg::cfg_set_cc_mode(void)
+{
+	this->hardware->outreg.cc_mode_en = 1;
+}
+
+/**** Private function definitions ***/
diff --git a/firmware/src/dccd/coil_reg.h b/firmware/src/dccd/coil_reg.h
new file mode 100644
index 0000000..c7bc6df
--- /dev/null
+++ b/firmware/src/dccd/coil_reg.h
@@ -0,0 +1,47 @@
+#ifndef DCCD_COIL_REG_H_
+#define DCCD_COIL_REG_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class CoilReg
+{
+	public:	
+	CoilReg(void);
+	~CoilReg(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	
+	uint16_t lock_current;
+	uint16_t ref_resistance;
+	uint16_t cc_max_resistance;
+	uint16_t cc_min_resistance;
+	uint8_t disable_protection;
+	
+	int8_t target_force;
+	
+	void process(void);
+	uint8_t read_act_force(void);
+	void cfg_set_cv_mode(void);
+	void cfg_set_cc_mode(void);
+	uint8_t is_fault(void);
+
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	int8_t set_force;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_COIL_REG_H_ */
\ No newline at end of file
diff --git a/firmware/src/dccd/dccd.cpp b/firmware/src/dccd/dccd.cpp
index 88e7bc2..544205d 100644
--- a/firmware/src/dccd/dccd.cpp
+++ b/firmware/src/dccd/dccd.cpp
@@ -6,48 +6,8 @@ using namespace dccd;
 
 /**** Private definitions ****/
 /**** Private constants ****/
-static const uint16_t def_lock_current = 4500;
-static const uint16_t def_max_hbrake_time = 0;
-static const uint16_t def_btn_force_repeat_time = 300;
-static const uint16_t def_btn_mode_repeat_time = 700;
-static const uint8_t def_button_inputs = 1;
-
-static const uint8_t def_display_brigth = 100;
-static const uint8_t def_display_dimm = 50;
-
-static const uint16_t cv_ref_resistance = 1500;
-static const uint16_t cc_ref_resistance = 2000;
-static const uint16_t cc_min_resistance = 1000;
-
-static const uint8_t bmode_image_open = 0x07;
-static const uint8_t bmode_image_user = 0x1E;
-static const uint8_t bmode_image_lock = 0x38;
-
-static const uint16_t display_keep_bmode = 2000;
-static const uint16_t display_keep_userf = 1000;
-
-static const uint8_t user_force_step = 10;
-
-static const uint8_t def_btn_force = 0;
-static const uint8_t def_brake_mode = 0;
-
-static const uint16_t def_chasis_inp_debounce = 100;
-static const uint16_t def_user_inp_debounce = 20;
-
-static const uint16_t mem_addr_inp_mode = 0;
-static const uint16_t mem_addr_force = 1;
-static const uint16_t mem_addr_bmode = 2;
-static const uint16_t mem_addr_dsp_brigth = 3;
-static const uint16_t mem_addr_dsp_dimm = 4;
-static const uint16_t mem_addr_lock_current = 5;
-static const uint16_t mem_addr_hbrake_time = 7;
-
 /**** Private variables ****/
 /**** Private function declarations ****/
-static uint8_t img_gen_dot10(uint8_t percent);
-static uint8_t img_gen_dot20(uint8_t percent);
-static uint8_t img_gen_bar(uint8_t percent);
-
 /**** Public function definitions ****/
 dccd::DccdApp::DccdApp(void)
 {
@@ -63,33 +23,61 @@ void dccd::DccdApp::init(DccdHw* dccd_hw)
 {
 	this->hardware = dccd_hw;
 	
-	#define OVERRIDEDEDBNC
-	#ifdef OVERRIDEDEDBNC
-	this->hardware->btn_mode.dbnc_lim = def_user_inp_debounce;
-	this->hardware->btn_up.dbnc_lim = def_user_inp_debounce;
-	this->hardware->btn_down.dbnc_lim = def_user_inp_debounce;
-	this->hardware->handbrake.dbnc_lim = def_chasis_inp_debounce;
-	this->hardware->brakes.dbnc_lim = def_chasis_inp_debounce;
-	this->hardware->dimm.dbnc_lim = def_chasis_inp_debounce;
-	#endif
+	// Set config	
+	this->user_force.init(dccd_hw);
+	this->user_force.btn_repeat_time = 300;
+	this->user_force.pot_mode = 0;
+	this->user_force.btn_step = 10;
+	this->user_force.cfg_debounce(10);
 	
-	// Load saved config from memory
-	this->loadMemCfg();
+	this->mode_btn.init(dccd_hw);
+	this->mode_btn.btn_repeat_time = 700;
+	this->mode_btn.cfg_debounce(10);
 	
-	this->btn_force_repeat_time = def_btn_force_repeat_time;
-	this->btn_mode_repeat_time = def_btn_mode_repeat_time;
-	this->pot_force = 0;
+	this->tps.init(dccd_hw);
+	this->tps.treshold_on = 65;
+	this->tps.treshold_off = 55;
 	
+	this->hbrake.init(dccd_hw);
+	this->hbrake.latch_time = 1000;
+	this->hbrake.cfg_debounce(100);
+	
+	this->brakes.init(dccd_hw);
+	this->brakes.cfg_debounce(100);
+	
+	this->coil_reg.init(dccd_hw);
+	this->coil_reg.lock_current = 4500;
+	this->coil_reg.ref_resistance = 1500;
+	this->coil_reg.cc_max_resistance = 2000;
+	this->coil_reg.cc_min_resistance = 1000;
+	this->coil_reg.target_force = 0;
+	this->coil_reg.disable_protection = 1;
+	this->coil_reg.cfg_set_cc_mode();
+	
+	this->dsp.init(dccd_hw);
+	this->dsp.brigth_pwm = 100;
+	this->dsp.dimm_pwm = 25;
+	this->dsp.next_image = 0x01;
+	this->dsp.next_lock_lvl = 0;
+	this->dsp.next_lock_time = 0;
+	
+	// Set variables
+	this->hbrake_force = -1;
+	this->brakes_open_force = -1;
+	this->brakes_lock_force = 100;
+	this->tps_lock_force = 100;
+	
+	// Variable config
+	this->tps_enabled = 1;
+	if(this->user_force.pot_mode!=0) this->tps_enabled = 0;
+	
+	// Initialize state
 	this->hardware->read();
-	this->hardware->dimm.force_read();
-
-	this->hardware->outreg.write_voltage(0);
-	this->hardware->outreg.write_current(0);
-	this->hardware->outreg.write_on(1);
 	
-	this->hardware->display.write(0x01);
-	if(this->hardware->dimm.state) this->hardware->display.write_backlight(this->display_dimm);
-	else this->hardware->display.write_backlight(this->display_brigth);
+	this->coil_reg.process();
+	
+	this->dsp.force_backlight(this->dsp.brigth_pwm);
+	this->dsp.process();
 	
 	this->hardware->write();
 }
@@ -99,334 +87,118 @@ void dccd::DccdApp::process(void)
 	// Update all inputs
 	this->hardware->read();
 	
-	uint8_t is_new_mode = 0;
-	uint8_t is_new_btn_force = 0;
-	
-	// Process mode button
-	if((this->hardware->btn_mode.state==1)&&((this->hardware->btn_mode.is_new)||(this->hardware->btn_mode.time_read() >= this->btn_mode_repeat_time)))
+	// Process mode
+	this->mode_btn.process();
+	if(this->mode_btn.is_new)
 	{
-		this->hardware->btn_mode.time_reset();
-		this->hardware->btn_mode.is_new = 0;
-		// Change mode
-		switch(this->brake_mode)
-		{
-			case 0:
-			this->brake_mode = 1;
-			break;
-			
-			case 1:
-			this->brake_mode = 2;
-			break;
-			
-			default:
-			this->brake_mode = 0;
-			break;
-		}
-		is_new_mode = 1;
-		this->hardware->board_hw.nvmem.write_8b(mem_addr_bmode, this->brake_mode);
+		this->brakes.cycle_mode();
+		// Save to EEPROM
+		this->mode_btn.is_new = 0;
 	};
 	
-	// Process user force inputs
-	if((this->hardware->btn_up.state==1)&&((this->hardware->btn_up.is_new)||(this->hardware->btn_up.time_read() >= this->btn_force_repeat_time)))
-	{
-		this->hardware->btn_up.time_reset();
-		this->hardware->btn_up.is_new = 0;
-		// Increase user force
-		this->btn_force += user_force_step;
-		if(this->btn_force > 100) this->btn_force = 100;
-		is_new_btn_force = 1;
-	};
+	// Process user force
+	this->user_force.process();
 	
-	if((this->hardware->btn_down.state==1)&&((this->hardware->btn_down.is_new)||(this->hardware->btn_down.time_read() >= this->btn_force_repeat_time)))
-	{
-		this->hardware->btn_down.time_reset();
-		this->hardware->btn_down.is_new = 0;
-		// Decrease user force
-		this->btn_force -= user_force_step;
-		if(this->btn_force > 100) this->btn_force = 0;
-		is_new_btn_force = 1;
-	};
+	//if(this->user_force.is_new_btn_force)
+	//{
+	//	// Save to EEPROM
+	//}
 	
-	if(is_new_btn_force)
-	{
-		this->hardware->board_hw.nvmem.write_8b(mem_addr_force, this->btn_force);
-	};
+	// Process pedals
+	this->tps.process();
+	this->hbrake.process();
+	this->brakes.process();
 	
-	this->pot_force = this->hardware->pot.last_percent;
+	// Calculate new output force
+	this->coil_reg.target_force =  this->calc_next_force();
 	
-	// Determine user force
-	int8_t user_force;
-	if(this->button_inputs) user_force = (int8_t)this->btn_force;
-	else user_force = (int8_t)this->pot_force;
-	
-	// Determine next settable force
-	int8_t next_force;
-	
-	uint8_t hbrake_timeout = 0;
-	if((this->max_hbrake_time!=0)&&(this->hardware->handbrake.time_read() >= this->max_hbrake_time))
-	{
-		hbrake_timeout = 1;
-	};
-	
-	if((this->hardware->handbrake.state == 1)&&(hbrake_timeout==0))
-	{
-		// Handbrake override
-		next_force = -1;
-	}
-	else if(this->hardware->brakes.state == 1)
-	{
-		// Brakes override
-		switch(this->brake_mode)
-		{
-			case 0:
-			next_force = -1;
-			break;
-			
-			case 1:
-			next_force = user_force;
-			break;
-			
-			case 2:
-			next_force = 100;
-			break;
-			
-			default:
-			next_force = -1;
-			this->brake_mode = 0;
-			break;
-		}
-	}
-	else
-	{
-		// User force
-		next_force = user_force;
-	}
-	
-	// Apply next force
-	if(next_force < 0)
-	{
-		// HiZ
-		this->hardware->outreg.write_voltage(0);
-		this->hardware->outreg.write_current(0);
-		this->hardware->outreg.write_on(0);
-		// For display
-		next_force = 0;
-	}
-	else if(next_force == 0)
-	{
-		// Open
-		this->hardware->outreg.write_voltage(0);
-		this->hardware->outreg.write_current(0);
-		this->hardware->outreg.write_on(1);
-	}
-	else
-	{
-		// Calculate current and voltage settings
-		this->hardware->outreg.write_current(util::percent_of((uint8_t)next_force, this->lock_current));
-		uint16_t ref_resistance = cv_ref_resistance;
-		if(this->hardware->outreg.cc_mode_en) ref_resistance = cc_ref_resistance;
-		this->hardware->outreg.write_voltage(util::sat_mul_kilo(this->hardware->outreg.read_current(), ref_resistance));
-		this->hardware->outreg.write_on(1);
-	}
-	
-	// Display image
-	if(is_new_mode)
-	{
-		uint8_t bmode_image;
-		switch(this->brake_mode)
-		{
-			case 0:
-			bmode_image = bmode_image_open;
-			break;
-			
-			case 1:
-			bmode_image = bmode_image_user;
-			break;
-			
-			case 2:
-			bmode_image = bmode_image_lock;
-			break;
-			
-			default:
-			bmode_image = bmode_image_open;
-			this->brake_mode = 0;
-			break;
-		}
-		this->hardware->display.write(bmode_image, display_keep_bmode, display_keep_bmode, 1);
-		is_new_mode = 0;
-	}
-	else if((is_new_btn_force)&&(this->button_inputs))
-	{
-		this->hardware->display.write(img_gen_dot10(this->btn_force), display_keep_userf, display_keep_userf, 1);
-		is_new_btn_force = 0;
-	}
-	else if(this->hardware->display.is_cycle_end())
-	{
-		this->hardware->display.write(img_gen_dot10((uint8_t)next_force));	
-	};
-	
-	// Display backlight
-	if(this->hardware->dimm.is_new)
-	{
-		this->hardware->dimm.is_new = 0;
-		if(this->hardware->dimm.state) this->hardware->display.write_backlight(this->display_dimm);
-		else this->hardware->display.write_backlight(this->display_brigth);
-	};
+	// Process coil driver
+	this->coil_reg.process();
 	
+	// Process display logic
+	this->dsp_logic();
+
 	// Execute outputs
 	this->hardware->write();
 }
 
-uint8_t dccd::DccdApp::loadMemCfg(void)
+int8_t dccd::DccdApp::calc_next_force(void)
 {
-	// Load saved config from memory
-	uint8_t t1;
-	uint16_t t2;
-	uint8_t def_applied = 0;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_inp_mode);
-	if(t1 > 1){this->button_inputs = def_button_inputs; def_applied=1; }
-	else this->button_inputs = t1;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_force);
-	if(t1 > 100){this->btn_force = def_btn_force; def_applied=1; }
-	else this->btn_force = t1;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_bmode);
-	if(t1 > 2){this->brake_mode = def_brake_mode; def_applied=1; }
-	else this->brake_mode = t1;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_dsp_brigth);
-	if((t1 > 100)||(t1 == 0)){this->display_brigth = def_brake_mode; def_applied=1; }
-	else this->display_brigth = t1;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_dsp_dimm);
-	if((t1 > 100)||(t1 == 0)){this->display_dimm = def_brake_mode; def_applied=1; }
-	else this->display_dimm = t1;
-	
-	t2 = this->hardware->board_hw.nvmem.read_16b(mem_addr_lock_current);
-	if((t2 > 5000)||(t2 < 1000)){this->lock_current = def_lock_current; def_applied=1; }
-	else this->lock_current = t2;
-	
-	t2 = this->hardware->board_hw.nvmem.read_16b(mem_addr_hbrake_time);
-	if((t2 > 30000)||(t2 == 0)){this->max_hbrake_time = def_max_hbrake_time; def_applied=1; }
-	else this->max_hbrake_time = t2;
-	
-	return def_applied;
+	if(this->hbrake.is_active)
+	{
+		return this->hbrake_force;
+	}
+	else if(this->brakes.is_active)
+	{
+		switch(this->brakes.mode)
+		{
+			case Brakes::OPEN:
+			return this->brakes_open_force;
+			
+			case Brakes::KEEP:
+			return (int8_t)(this->user_force.force);
+			
+			case Brakes::LOCK:
+			return this->brakes_lock_force;
+			
+			default:
+			return 0;
+		}
+	}
+	else if((this->tps_enabled)&&(this->tps.is_active()))
+	{
+		return this->tps_lock_force;
+	}
+	else
+	{
+		return (int8_t)(this->user_force.force);
+	}
 }
 
-void dccd::DccdApp::saveMemCfg(void)
+void dccd::DccdApp::dsp_logic(void)
 {
-	// Save config to memory
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_inp_mode, this->button_inputs);
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_force, this->btn_force);
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_bmode, this->brake_mode);
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_dsp_brigth, this->display_brigth);
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_dsp_dimm, this->display_dimm);
-	
-	this->hardware->board_hw.nvmem.write_16b(mem_addr_lock_current, this->lock_current);
-	
-	this->hardware->board_hw.nvmem.write_16b(mem_addr_hbrake_time, this->max_hbrake_time);
+	// Display image
+	if(this->brakes.is_new_mode)
+	{
+		this->brakes.is_new_mode = 0;
+		uint8_t bmode_image;
+		switch(this->brakes.mode)
+		{
+			case Brakes::OPEN:
+			bmode_image = 0x07;
+			break;
+				
+			case Brakes::KEEP:
+			bmode_image = 0x1E;
+			break;
+				
+			case Brakes::LOCK:
+			bmode_image = 0x38;
+			break;
+				
+			default:
+			bmode_image = 0x07;
+			this->brakes.mode = Brakes::OPEN;
+			break;
+		}
+		this->dsp.write(bmode_image, 3, 2000);
+	}
+	else if(this->user_force.is_new_btn_force)
+	{
+		this->user_force.is_new_btn_force = 0;
+		this->dsp.write_percent(this->user_force.force, 0, 2, 1000);
+	}
+	else if(this->coil_reg.is_fault())
+	{
+		this->dsp.write(0x21, 1, 0);
+	}
+	else
+	{
+		this->dsp.write_percent(this->coil_reg.read_act_force(), 0, 0, 0);
+	};
+		
+	// Process display
+	this->dsp.process();
 }
 
 /**** Private function definitions ***/
-static uint8_t img_gen_dot10(uint8_t percent)
-{
-	switch(percent)
-	{
-		case 0 ... 5:
-		return 0x01;
-		
-		case 6 ... 15:
-		return 0x03;
-		
-		case 16 ... 25:
-		return 0x02;
-		
-		case 26 ... 35:
-		return 0x06;
-		
-		case 36 ... 45:
-		return 0x04;
-		
-		case 46 ... 55:
-		return 0x0C;
-		
-		case 56 ... 65:
-		return 0x08;
-		
-		case 66 ... 75:
-		return 0x18;
-		
-		case 76 ... 85:
-		return 0x10;
-		
-		case 86 ... 95:
-		return 0x30;
-		
-		case 96 ... 100:
-		return 0x20;
-		
-		default:
-		return 0x20;
-	}
-}
-
-static uint8_t img_gen_dot20(uint8_t percent)
-{
-	switch(percent)
-	{
-		case 0 ... 10:
-		return 0x01;
-		
-		case 11 ... 30:
-		return 0x02;
-		
-		case 31 ... 50:
-		return 0x04;
-		
-		case 51 ... 70:
-		return 0x08;
-		
-		case 71 ... 90:
-		return 0x10;
-		
-		case 91 ... 100:
-		return 0x20;
-		
-		default:
-		return 0x20;
-	}
-}
-
-static uint8_t img_gen_bar(uint8_t percent)
-{
-	switch(percent)
-	{
-		case 0 ... 10:
-		return 0x01;
-		
-		case 11 ... 30:
-		return 0x03;
-		
-		case 31 ... 50:
-		return 0x07;
-		
-		case 51 ... 70:
-		return 0x0F;
-		
-		case 71 ... 90:
-		return 0x1F;
-		
-		case 91 ... 100:
-		return 0x3F;
-		
-		default:
-		return 0x3F;
-	}
-}
diff --git a/firmware/src/dccd/dccd.h b/firmware/src/dccd/dccd.h
index bacd4db..e6e2525 100644
--- a/firmware/src/dccd/dccd.h
+++ b/firmware/src/dccd/dccd.h
@@ -4,6 +4,13 @@
 /**** Includes ****/
 #include <stdint.h>
 #include "dccd_hw.h"
+#include "user_force.h"
+#include "mode.h"
+#include "handbrake.h"
+#include "brakes.h"
+#include "coil_reg.h"
+#include "display.h"
+#include "tps.h"
 
 namespace dccd {
 
@@ -16,26 +23,28 @@ class DccdApp
 	
 	void init(DccdHw* dccd_hw);
 	void process(void);
+
+	UserForce user_force;
+	ModeBtn mode_btn;
+	Handbrake hbrake;
+	Brakes brakes;
+	CoilReg coil_reg;
+	DccdDisplay dsp;
+	Thtrottle tps;
 	
-	uint16_t lock_current;
-	uint16_t max_hbrake_time;
-	uint16_t btn_force_repeat_time;
-	uint16_t btn_mode_repeat_time;
-	uint8_t button_inputs;
-	uint8_t display_brigth;
-	uint8_t display_dimm;
-	
-	uint8_t btn_force;
-	uint8_t pot_force;
-	uint8_t brake_mode;
-	
-	uint8_t loadMemCfg(void);
-	void saveMemCfg(void);
+	int8_t hbrake_force;
+	int8_t brakes_open_force;
+	int8_t brakes_lock_force;
+	uint8_t tps_enabled;
+	int8_t tps_lock_force;
 	
 	#ifdef TESTING
 	protected:
 	#endif
 	DccdHw* hardware;
+	
+	int8_t calc_next_force(void);
+	void dsp_logic(void);
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/dccd/display.cpp b/firmware/src/dccd/display.cpp
new file mode 100644
index 0000000..40a337c
--- /dev/null
+++ b/firmware/src/dccd/display.cpp
@@ -0,0 +1,208 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "display.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+static uint8_t img_gen_dot10(uint8_t percent);
+static uint8_t img_gen_dot20(uint8_t percent);
+static uint8_t img_gen_bar(uint8_t percent);
+
+/**** Public function definitions ****/
+dccd::DccdDisplay::DccdDisplay(void)
+{
+	return;
+}
+
+dccd::DccdDisplay::~DccdDisplay(void)
+{
+	return;
+}
+
+void dccd::DccdDisplay::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	this->brigth_pwm = 50;
+	this->dimm_pwm = 25;
+	
+	this->next_image = 0x00;
+	this->next_lock_lvl = 0;
+	this->next_lock_time = 0;
+	this->act_image = 0x00;
+	this->act_lock_lvl = 0;
+}
+
+void dccd::DccdDisplay::cfg_debounce(uint16_t dbnc_time)
+{
+	this->hardware->dimm.dbnc_lim = dbnc_time;
+}
+
+void dccd::DccdDisplay::write(uint8_t image, uint8_t lock_lvl, uint16_t lock_time)
+{
+	if(lock_lvl < this->next_lock_lvl) return;
+	
+	this->next_image = image;
+	this->next_lock_lvl = lock_lvl;
+	this->next_lock_time = lock_time;
+}
+
+void dccd::DccdDisplay::write_percent(uint8_t percent, uint8_t style, uint8_t lock_lvl, uint16_t lock_time)
+{
+	uint8_t img = 0x01;
+	
+	switch(style)
+	{
+		case 1:
+		img = img_gen_dot20(percent);
+		break;
+		
+		case 2:
+		img = img_gen_bar(percent);
+		break;
+		
+		default:
+		img = img_gen_dot10(percent);
+		break;
+	}
+	
+	this->write(img, lock_lvl, lock_time);
+}
+
+void dccd::DccdDisplay::force_backlight(uint8_t percent)
+{
+	if(percent > this->brigth_pwm) percent = this->brigth_pwm;
+	this->hardware->display.write_backlight(percent);
+}
+
+void dccd::DccdDisplay::process(void)
+{	
+	// Process DIMM switch
+	if(this->hardware->dimm.is_new)
+	{
+		this->hardware->dimm.is_new = 0;
+		if(this->hardware->dimm.state) this->hardware->display.write_backlight(this->dimm_pwm);
+		else this->hardware->display.write_backlight(this->brigth_pwm);
+	};
+	
+	// Image processor
+	uint8_t update_img = 0;
+	if(this->next_lock_lvl >= this->act_lock_lvl) update_img = 1;
+	else if(this->hardware->display.is_cycle_end()) update_img = 1;
+	
+	if(update_img)
+	{
+		this->act_image = this->next_image;
+		this->act_lock_lvl = this->next_lock_lvl;
+		
+		if(this->next_lock_time > 0)
+		{
+			this->hardware->display.write(this->act_image, this->next_lock_time, this->next_lock_time, 1);
+		}
+		else
+		{
+			this->hardware->display.write(this->act_image);
+			this->act_lock_lvl = 0;
+		}
+	};
+}
+
+/**** Private function definitions ***/
+static uint8_t img_gen_dot10(uint8_t percent)
+{
+	switch(percent)
+	{
+		case 0 ... 5:
+		return 0x01;
+		
+		case 6 ... 15:
+		return 0x03;
+		
+		case 16 ... 25:
+		return 0x02;
+		
+		case 26 ... 35:
+		return 0x06;
+		
+		case 36 ... 45:
+		return 0x04;
+		
+		case 46 ... 55:
+		return 0x0C;
+		
+		case 56 ... 65:
+		return 0x08;
+		
+		case 66 ... 75:
+		return 0x18;
+		
+		case 76 ... 85:
+		return 0x10;
+		
+		case 86 ... 95:
+		return 0x30;
+		
+		case 96 ... 100:
+		return 0x20;
+		
+		default:
+		return 0x20;
+	}
+}
+
+static uint8_t img_gen_dot20(uint8_t percent)
+{
+	switch(percent)
+	{
+		case 0 ... 10:
+		return 0x01;
+		
+		case 11 ... 30:
+		return 0x02;
+		
+		case 31 ... 50:
+		return 0x04;
+		
+		case 51 ... 70:
+		return 0x08;
+		
+		case 71 ... 90:
+		return 0x10;
+		
+		case 91 ... 100:
+		return 0x20;
+		
+		default:
+		return 0x20;
+	}
+}
+
+static uint8_t img_gen_bar(uint8_t percent)
+{
+	switch(percent)
+	{
+		case 0 ... 10:
+		return 0x01;
+		
+		case 11 ... 30:
+		return 0x03;
+		
+		case 31 ... 50:
+		return 0x07;
+		
+		case 51 ... 70:
+		return 0x0F;
+		
+		case 71 ... 90:
+		return 0x1F;
+		
+		case 91 ... 100:
+		return 0x3F;
+		
+		default:
+		return 0x3F;
+	}
+}
\ No newline at end of file
diff --git a/firmware/src/dccd/display.h b/firmware/src/dccd/display.h
new file mode 100644
index 0000000..daefb77
--- /dev/null
+++ b/firmware/src/dccd/display.h
@@ -0,0 +1,48 @@
+#ifndef DCCD_DISPLAY_H_
+#define DCCD_DISPLAY_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class DccdDisplay
+{
+	public:	
+	DccdDisplay(void);
+	~DccdDisplay(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	void cfg_debounce(uint16_t dbnc_time);
+	
+	uint8_t brigth_pwm;
+	uint8_t dimm_pwm;
+	
+	uint8_t next_image;
+	uint8_t next_lock_lvl;
+	uint8_t next_lock_time;
+	
+	void write(uint8_t image, uint8_t lock_lvl, uint16_t lock_time);
+	void write_percent(uint8_t percent, uint8_t style, uint8_t lock_lvl, uint16_t lock_time);
+	void force_backlight(uint8_t percent);
+	void process(void);
+
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	
+	uint8_t act_image;
+	uint8_t act_lock_lvl;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_MODE_BTN_H_ */
\ No newline at end of file
diff --git a/firmware/src/dccd/handbrake.cpp b/firmware/src/dccd/handbrake.cpp
new file mode 100644
index 0000000..aa9015c
--- /dev/null
+++ b/firmware/src/dccd/handbrake.cpp
@@ -0,0 +1,70 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "handbrake.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::Handbrake::Handbrake(void)
+{
+	return;
+}
+
+dccd::Handbrake::~Handbrake(void)
+{
+	return;
+}
+
+void dccd::Handbrake::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	this->latch_time = 0;
+	this->hbrake_latch = 0;
+	this->start_ts = 0;
+	this->is_active = 0;
+}
+
+void dccd::Handbrake::cfg_debounce(uint16_t dbnc_time)
+{
+	this->hardware->handbrake.dbnc_lim = dbnc_time;
+}
+
+uint8_t dccd::Handbrake::process(void)
+{
+	uint16_t ts_now = this->hardware->counter.read();
+	
+	if(this->hardware->handbrake.state > 0)
+	{
+		if(this->hardware->handbrake.is_new)
+		{
+			this->hardware->handbrake.is_new = 0;
+			// Note start time
+			this->start_ts = ts_now;
+			this->hbrake_latch = 1;
+		};
+		this->is_active = 1;
+	}
+	else if((this->hbrake_latch != 0)&&(this->latch_time != 0))
+	{
+		uint16_t td = util::time_delta(this->start_ts, ts_now);
+		uint32_t td_ms = this->hardware->counter.convert_ms(td);
+		if(td_ms >= this->latch_time)
+		{
+			this->hbrake_latch = 0;
+			if(this->hardware->handbrake.state > 0) this->is_active = 1;
+			else this->is_active = 0;
+		};
+	}
+	else
+	{
+		this->is_active = 0;
+	}
+	
+	return this->is_active;
+}
+
+/**** Private function definitions ***/
diff --git a/firmware/src/dccd/handbrake.h b/firmware/src/dccd/handbrake.h
new file mode 100644
index 0000000..4728c1e
--- /dev/null
+++ b/firmware/src/dccd/handbrake.h
@@ -0,0 +1,40 @@
+#ifndef DCCD_HANDBRAKE_H_
+#define DCCD_HANDBRAKE_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class Handbrake
+{
+	public:		
+	Handbrake(void);
+	~Handbrake(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	
+	uint16_t latch_time;
+	uint8_t is_active;
+		
+	uint8_t process(void);
+	void cfg_debounce(uint16_t dbnc_time);
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	uint8_t hbrake_latch;
+	uint16_t start_ts;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_HANDBRAKE_H_ */
\ No newline at end of file
diff --git a/firmware/src/dccd/mode.cpp b/firmware/src/dccd/mode.cpp
new file mode 100644
index 0000000..a34b262
--- /dev/null
+++ b/firmware/src/dccd/mode.cpp
@@ -0,0 +1,44 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mode.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::ModeBtn::ModeBtn(void)
+{
+	return;
+}
+
+dccd::ModeBtn::~ModeBtn(void)
+{
+	return;
+}
+
+void dccd::ModeBtn::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	this->btn_repeat_time = 0;
+	this->is_new = 0;
+}
+
+void dccd::ModeBtn::cfg_debounce(uint16_t dbnc_time)
+{
+	this->hardware->btn_mode.dbnc_lim = dbnc_time;
+}
+
+void dccd::ModeBtn::process(void)
+{	
+	if((this->hardware->btn_mode.state==1)&&((this->hardware->btn_mode.is_new)||((this->hardware->btn_mode.time_read() >= this->btn_repeat_time)&&(this->btn_repeat_time!=0))))
+	{
+		this->hardware->btn_mode.time_reset();
+		this->hardware->btn_mode.is_new = 0;
+		this->is_new = 1;
+	};
+}
+
+/**** Private function definitions ***/
diff --git a/firmware/src/dccd/mode.h b/firmware/src/dccd/mode.h
new file mode 100644
index 0000000..65b7b3c
--- /dev/null
+++ b/firmware/src/dccd/mode.h
@@ -0,0 +1,39 @@
+#ifndef DCCD_MODE_BTN_H_
+#define DCCD_MODE_BTN_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class ModeBtn
+{
+	public:	
+	ModeBtn(void);
+	~ModeBtn(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	void cfg_debounce(uint16_t dbnc_time);
+	
+	uint16_t btn_repeat_time;
+	
+	uint8_t is_new;
+	
+	void process(void);
+
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_MODE_BTN_H_ */
\ No newline at end of file
diff --git a/firmware/src/dccd/tps.cpp b/firmware/src/dccd/tps.cpp
new file mode 100644
index 0000000..ab0c910
--- /dev/null
+++ b/firmware/src/dccd/tps.cpp
@@ -0,0 +1,59 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "tps.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::Thtrottle::Thtrottle(void)
+{
+	return;
+}
+
+dccd::Thtrottle::~Thtrottle(void)
+{
+	return;
+}
+
+void dccd::Thtrottle::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	this->is_new = 0;
+	this->active = 0;
+	
+	this->treshold_on = 55;
+	this->treshold_off = 45;
+}
+
+void dccd::Thtrottle::process(void)
+{
+	if(this->active)
+	{
+		if(this->hardware->pot.last_percent <= this->treshold_off)
+		{
+			this->active = 0;
+			this->is_new = 1;
+		}
+		else this->active = 1;
+	}
+	else
+	{
+		if(this->hardware->pot.last_percent >= this->treshold_on)
+		{
+			this->active = 1;
+			this->is_new = 1;
+		}
+		else this->active = 0;
+	}
+}
+
+uint8_t dccd::Thtrottle::is_active(void)
+{
+	return this->active;
+}
+
+/**** Private function definitions ***/
diff --git a/firmware/src/dccd/tps.h b/firmware/src/dccd/tps.h
new file mode 100644
index 0000000..a5ad9ca
--- /dev/null
+++ b/firmware/src/dccd/tps.h
@@ -0,0 +1,41 @@
+#ifndef DCCD_TPS_H_
+#define DCCD_TPS_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class Thtrottle
+{
+	public:	
+	Thtrottle(void);
+	~Thtrottle(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	
+	uint8_t treshold_on;
+	uint8_t treshold_off;
+	
+	uint8_t is_new;
+	uint8_t is_active(void);
+	
+	void process(void);
+
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	uint8_t active;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_TPS_H_ */
\ No newline at end of file
diff --git a/firmware/src/dccd/user_force.cpp b/firmware/src/dccd/user_force.cpp
new file mode 100644
index 0000000..8691d6a
--- /dev/null
+++ b/firmware/src/dccd/user_force.cpp
@@ -0,0 +1,85 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "user_force.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::UserForce::UserForce(void)
+{
+	return;
+}
+
+dccd::UserForce::~UserForce(void)
+{
+	return;
+}
+
+void dccd::UserForce::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	this->is_new_btn_force = 0;
+	this->btn_repeat_time = 0;
+	this->force = 0;
+	this->pot_mode = 0;
+	this->btn_force = 0;
+	this->btn_step = 20;
+}
+
+void dccd::UserForce::cfg_debounce(uint16_t dbnc_time)
+{
+	this->hardware->btn_up.dbnc_lim = dbnc_time;
+	this->hardware->btn_down.dbnc_lim = dbnc_time;
+}
+
+void dccd::UserForce::cfg_pot(uint16_t dead_bot, uint16_t dead_top)
+{
+	if(this->pot_mode==0) return;
+	this->hardware->pot.high_deadzone = dead_top;
+	this->hardware->pot.low_deadzone = dead_bot;
+}
+
+void dccd::UserForce::write_force(uint8_t force)
+{
+	if(force > 100) force = 100;
+	this->btn_force = force;
+	this->force = force;
+}
+
+uint8_t dccd::UserForce::process(void)
+{
+	if(this->pot_mode)
+	{
+		this->force = this->hardware->pot.last_percent;
+		return this->force;
+	};
+	
+	if((this->hardware->btn_up.state==1)&&((this->hardware->btn_up.is_new)||((this->hardware->btn_up.time_read() >= this->btn_repeat_time)&&(this->btn_repeat_time!=0))))
+	{
+		this->hardware->btn_up.time_reset();
+		this->hardware->btn_up.is_new = 0;
+		// Increase user force
+		this->btn_force += this->btn_step;
+		if(this->btn_force > 100) this->btn_force = 100;
+		is_new_btn_force = 1;
+	};
+	
+	if((this->hardware->btn_down.state==1)&&((this->hardware->btn_down.is_new)||((this->hardware->btn_down.time_read() >= this->btn_repeat_time)&&(this->btn_repeat_time!=0))))
+	{
+		this->hardware->btn_down.time_reset();
+		this->hardware->btn_down.is_new = 0;
+		// Decrease user force
+		this->btn_force -= this->btn_step;
+		if(this->btn_force > 100) this->btn_force = 0;
+		is_new_btn_force = 1;
+	};
+
+	this->force = this->btn_force;
+	return this->force;
+}
+
+/**** Private function definitions ***/
diff --git a/firmware/src/dccd/user_force.h b/firmware/src/dccd/user_force.h
new file mode 100644
index 0000000..28f1b43
--- /dev/null
+++ b/firmware/src/dccd/user_force.h
@@ -0,0 +1,45 @@
+#ifndef DCCD_USER_FORCE_H_
+#define DCCD_USER_FORCE_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class UserForce
+{
+	public:	
+	UserForce(void);
+	~UserForce(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	void cfg_debounce(uint16_t dbnc_time);
+	void cfg_pot(uint16_t dead_bot, uint16_t dead_top);
+	
+	uint16_t btn_repeat_time;
+	uint8_t pot_mode;
+	uint8_t btn_step;
+	
+	uint8_t force;
+	uint8_t is_new_btn_force;
+	
+	uint8_t process(void);
+	void write_force(uint8_t force);
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	uint8_t btn_force;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_USER_FORCE_H_ */
\ No newline at end of file
diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index f1f6d4c..27c4bb1 100644
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -24,26 +24,6 @@ int main(void)
 	
 	app.init(&dccd_hw);
 	
-	//#define OVERRIDECFG
-	#ifdef OVERRIDECFG
-	// Configuration
-	app.lock_current = 4500;
-	app.max_hbrake_time = 2000;
-	app.button_inputs = 1;
-	app.display_brigth = 100;
-	app.display_dimm = 25;
-	
-	// Initial values
-	app.btn_force = 0;
-	app.brake_mode = 0;
-	#endif
-	
-	// Save config to memory
-	//#define SAVECFG
-	#ifdef SAVECFG
-	app.saveMemCfg();
-	#endif
-	
 	// Super loop
 	while(1)
 	{	
diff --git a/firmware/src/uDCCD.cppproj b/firmware/src/uDCCD.cppproj
index 983558e..d5d697d 100644
--- a/firmware/src/uDCCD.cppproj
+++ b/firmware/src/uDCCD.cppproj
@@ -217,6 +217,12 @@
     <Compile Include="bsp\mcu\mcu_hal_r8.cpp">
       <SubType>compile</SubType>
     </Compile>
+    <Compile Include="dccd\brakes.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\brakes.h">
+      <SubType>compile</SubType>
+    </Compile>
     <Compile Include="dccd\dccd.cpp">
       <SubType>compile</SubType>
     </Compile>
@@ -229,6 +235,42 @@
     <Compile Include="dccd\dccd_hw.h">
       <SubType>compile</SubType>
     </Compile>
+    <Compile Include="dccd\display.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\display.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\coil_reg.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\coil_reg.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\handbrake.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\handbrake.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\mode.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\mode.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\tps.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\tps.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\user_force.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\user_force.h">
+      <SubType>compile</SubType>
+    </Compile>
     <Compile Include="hw\button.cpp">
       <SubType>compile</SubType>
     </Compile>