diff --git a/firmware/src/dccd/dccd.cpp b/firmware/src/dccd/dccd.cpp
index a77e39f..88e7bc2 100644
--- a/firmware/src/dccd/dccd.cpp
+++ b/firmware/src/dccd/dccd.cpp
@@ -6,6 +6,42 @@ 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);
@@ -23,53 +59,54 @@ dccd::DccdApp::~DccdApp(void)
 	return;
 }
 
-void dccd::DccdApp::init(void)
+void dccd::DccdApp::init(DccdHw* dccd_hw)
 {
-	// Read settings from EEPROM
+	this->hardware = dccd_hw;
 	
-	DccdHw::dccdHwCfg_t cfg;
-	cfg.handbrake_pull_up = 1;
-	cfg.pwm_f_khz = 16;
-	cfg.speed_hall = 0;
+	#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
 	
-	this->hardware.init(&cfg);
+	// Load saved config from memory
+	this->loadMemCfg();
 	
-	this->lock_current = 4500;
-	this->max_hbrake_time = 1500;
-	this->btn_repeat_time = 500;
-	this->button_inputs = 1;
-	
-	this->display_brigth = 100;
-	this->display_dimm = 50;
-	
-	this->btn_force = 0;
+	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->brake_mode = 0;
 	
-	this->hardware.read();
+	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.outreg.voltage = 0;
-	this->hardware.outreg.current = 0;
-	this->hardware.outreg.out_on = 1;
-	this->hardware.display.write(0x01);
-	this->hardware.display.write_backlight(50);
+	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->hardware.write();
+	this->hardware->write();
 }
 
 void dccd::DccdApp::process(void)
 {
 	// Update all inputs
-	this->hardware.read();
+	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_repeat_time)))
+	if((this->hardware->btn_mode.state==1)&&((this->hardware->btn_mode.is_new)||(this->hardware->btn_mode.time_read() >= this->btn_mode_repeat_time)))
 	{
-		this->hardware.btn_mode.time_reset();
-		this->hardware.btn_mode.is_new = 0;
+		this->hardware->btn_mode.time_reset();
+		this->hardware->btn_mode.is_new = 0;
 		// Change mode
 		switch(this->brake_mode)
 		{
@@ -86,30 +123,36 @@ void dccd::DccdApp::process(void)
 			break;
 		}
 		is_new_mode = 1;
+		this->hardware->board_hw.nvmem.write_8b(mem_addr_bmode, this->brake_mode);
 	};
 	
 	// 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_repeat_time)))
+	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;
+		this->hardware->btn_up.time_reset();
+		this->hardware->btn_up.is_new = 0;
 		// Increase user force
-		this->btn_force += 10;
+		this->btn_force += user_force_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)))
+	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;
+		this->hardware->btn_down.time_reset();
+		this->hardware->btn_down.is_new = 0;
 		// Decrease user force
-		this->btn_force -= 10;
+		this->btn_force -= user_force_step;
 		if(this->btn_force > 100) this->btn_force = 0;
 		is_new_btn_force = 1;
 	};
 	
-	this->pot_force = this->hardware.pot.last_percent;
+	if(is_new_btn_force)
+	{
+		this->hardware->board_hw.nvmem.write_8b(mem_addr_force, this->btn_force);
+	};
+	
+	this->pot_force = this->hardware->pot.last_percent;
 	
 	// Determine user force
 	int8_t user_force;
@@ -119,12 +162,18 @@ void dccd::DccdApp::process(void)
 	// Determine next settable force
 	int8_t next_force;
 	
-	if((this->hardware.handbrake.state == 1)&&(this->hardware.handbrake.time_read() < this->max_hbrake_time))
+	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)
+	else if(this->hardware->brakes.state == 1)
 	{
 		// Brakes override
 		switch(this->brake_mode)
@@ -142,7 +191,8 @@ void dccd::DccdApp::process(void)
 			break;
 			
 			default:
-			this->brake_mode = -1;
+			next_force = -1;
+			this->brake_mode = 0;
 			break;
 		}
 	}
@@ -156,102 +206,227 @@ void dccd::DccdApp::process(void)
 	if(next_force < 0)
 	{
 		// HiZ
-		this->hardware.outreg.voltage = 0;
-		this->hardware.outreg.current = 0;
-		this->hardware.outreg.out_on = 0;
+		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.voltage = 0;
-		this->hardware.outreg.current = 0;
-		this->hardware.outreg.out_on = 1;
+		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.current = util::percent_of((uint8_t)next_force, this->lock_current);
-		this->hardware.outreg.voltage = util::sat_mul_kilo(this->hardware.outreg.current, 2500);
-		this->hardware.outreg.out_on = 1;
+		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 image;
+		uint8_t bmode_image;
 		switch(this->brake_mode)
 		{
 			case 0:
-			image = 0x07;
+			bmode_image = bmode_image_open;
 			break;
 			
 			case 1:
-			image = 0x1E;
+			bmode_image = bmode_image_user;
 			break;
 			
 			case 2:
-			image = 0x38;
+			bmode_image = bmode_image_lock;
 			break;
 			
 			default:
-			image = 0x07;
+			bmode_image = bmode_image_open;
+			this->brake_mode = 0;
 			break;
 		}
-		this->hardware.display.write(image, 1000, 1000, 1);
+		this->hardware->display.write(bmode_image, display_keep_bmode, display_keep_bmode, 1);
+		is_new_mode = 0;
 	}
-	else if(is_new_btn_force)
+	else if((is_new_btn_force)&&(this->button_inputs))
 	{
-		this->hardware.display.write(img_gen_dot10(this->btn_force), 500, 500, 1);
+		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())
+	else if(this->hardware->display.is_cycle_end())
 	{
-		this->hardware.display.write(img_gen_dot10(next_force));
+		this->hardware->display.write(img_gen_dot10((uint8_t)next_force));	
 	};
 	
 	// Display backlight
-	if(this->hardware.dimm.is_new)
+	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);
+		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);
 	};
 	
 	// Execute outputs
-	this->hardware.write();
+	this->hardware->write();
+}
+
+uint8_t dccd::DccdApp::loadMemCfg(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;
+}
+
+void dccd::DccdApp::saveMemCfg(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);
 }
 
 /**** Private function definitions ***/
 static uint8_t img_gen_dot10(uint8_t percent)
 {
-	if(percent<6) return 0x01;
-	else if(percent<16) return 0x03;
-	else if(percent<26) return 0x02;
-	else if(percent<36) return 0x06;
-	else if(percent<46) return 0x04;
-	else if(percent<56) return 0x0C;
-	else if(percent<66) return 0x08;
-	else if(percent<76) return 0x18;
-	else if(percent<86) return 0x10;
-	else if(percent<96) return 0x30;
-	else return 0x20;
+	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)
 {
-	if(percent<11) return 0x01;
-	else if(percent<31) return 0x02;
-	else if(percent<51) return 0x04;
-	else if(percent<71) return 0x08;
-	else if(percent<91) return 0x10;
-	else return 0x20;
+	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)
 {
-	if(percent<11) return 0x01;
-	else if(percent<31) return 0x03;
-	else if(percent<51) return 0x07;
-	else if(percent<71) return 0x0F;
-	else if(percent<91) return 0x1F;
-	else return 0x3F;
+	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 a723aa2..bacd4db 100644
--- a/firmware/src/dccd/dccd.h
+++ b/firmware/src/dccd/dccd.h
@@ -14,12 +14,13 @@ class DccdApp
 	DccdApp(void);
 	~DccdApp(void);
 	
-	void init(void);
+	void init(DccdHw* dccd_hw);
 	void process(void);
 	
 	uint16_t lock_current;
 	uint16_t max_hbrake_time;
-	uint16_t btn_repeat_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;
@@ -28,10 +29,13 @@ class DccdApp
 	uint8_t pot_force;
 	uint8_t brake_mode;
 	
+	uint8_t loadMemCfg(void);
+	void saveMemCfg(void);
+	
 	#ifdef TESTING
 	protected:
 	#endif
-	dccd::DccdHw hardware;
+	DccdHw* hardware;
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/dccd/dccd_hw.cpp b/firmware/src/dccd/dccd_hw.cpp
index 7bceec4..4270771 100644
--- a/firmware/src/dccd/dccd_hw.cpp
+++ b/firmware/src/dccd/dccd_hw.cpp
@@ -6,6 +6,37 @@ using namespace dccd;
 
 /**** Private definitions ****/
 /**** Private constants ****/
+static const uint8_t def_dbnc_time = 10;
+
+static const uint16_t def_pot_dead_bot = 500;
+static const uint16_t def_pot_dead_top = 4500;
+
+static const uint8_t def_cc_mode_en = 1;
+
+static const uint16_t def_cnter_us = 900;
+
+static const uint16_t def_out_voltage_under_treshold = 0;
+static const uint16_t def_out_voltage_over_treshold = 9000;
+static const uint16_t def_out_voltage_hold_time = 1000;
+static const uint16_t def_out_voltage_cooldown_time = 0;
+
+static const uint16_t def_out_current_under_treshold = 0;
+static const uint16_t def_out_current_over_treshold = 6000;
+static const uint16_t def_out_current_hold_time = 200;
+static const uint16_t def_out_current_cooldown_time = 1000;
+
+static const uint16_t def_battery_voltage_under_treshold = 9000;
+static const uint16_t def_battery_voltage_over_treshold = 18000;
+static const uint16_t def_battery_voltage_hold_time = 1000;
+static const uint16_t def_battery_voltage_cooldown_time = 0;
+
+static const uint16_t def_battery_current_under_treshold = 0;
+static const uint16_t def_battery_current_over_treshold = 8000;
+static const uint16_t def_battery_current_hold_time = 200;
+static const uint16_t def_battery_current_cooldown_time = 1000;
+
+static const uint16_t def_inital_bat_voltage = 12000;
+
 /**** Private variables ****/
 /**** Private function declarations ****/
 /**** Public function definitions ****/
@@ -20,66 +51,68 @@ dccd::DccdHw::~DccdHw(void)
 }
 
 void dccd::DccdHw::init(dccdHwCfg_t* cfg)
-{
+{	
+	// Apply config
 	bsp::Board::boardCfg_t board_cfg;
 	board_cfg.pwm_f_khz = cfg->pwm_f_khz;
 	board_cfg.od_common_is_pwm = 1;
 	this->board_hw.init(&board_cfg);
 	
-	this->counter.init(0xFFFF, 900);
+	this->counter.init(0xFFFF, cfg->counter_step_us);
+	this->counter.disabled = 0;
 	
 	this->out_voltage.init(&(this->board_hw.out_voltage), &(this->counter));
-	this->out_voltage.under_treshold = 0;
-	this->out_voltage.over_treshold = 9000;
-	this->out_voltage.hold_time = 1000;
-	this->out_voltage.cooldown_time = 0;
+	this->out_voltage.under_treshold = def_out_voltage_under_treshold;
+	this->out_voltage.over_treshold = def_out_voltage_over_treshold;
+	this->out_voltage.hold_time = def_out_voltage_hold_time;
+	this->out_voltage.cooldown_time = def_out_voltage_cooldown_time;
 	this->out_voltage.update_ain = 0;
 	this->out_voltage.auto_reset = 1;
 	
 	this->out_current.init(&(this->board_hw.out_current), &(this->counter));
-	this->out_current.under_treshold = 0;
-	this->out_current.over_treshold = 6000;
-	this->out_current.hold_time = 200;
-	this->out_current.cooldown_time = 1000;
+	this->out_current.under_treshold = def_out_current_under_treshold;
+	this->out_current.over_treshold = def_out_current_over_treshold;
+	this->out_current.hold_time = def_out_current_hold_time;
+	this->out_current.cooldown_time = def_out_current_cooldown_time;
 	this->out_current.update_ain = 0;
 	this->out_current.auto_reset = 1;
 	
 	this->battery_voltage.init(&(this->board_hw.battery_voltage), &(this->counter));
-	this->battery_voltage.under_treshold = 9000;
-	this->battery_voltage.over_treshold = 18000;
-	this->battery_voltage.hold_time = 1000;
-	this->battery_voltage.cooldown_time = 0;
+	this->battery_voltage.under_treshold = def_battery_voltage_under_treshold;
+	this->battery_voltage.over_treshold = def_battery_voltage_over_treshold;
+	this->battery_voltage.hold_time = def_battery_voltage_hold_time;
+	this->battery_voltage.cooldown_time = def_battery_voltage_cooldown_time;
 	this->battery_voltage.update_ain = 0;
 	this->battery_voltage.auto_reset = 1;
-	this->battery_voltage.last_read = 12000;
+	this->battery_voltage.last_read = def_inital_bat_voltage;
 	
 	this->battery_current.init(&(this->board_hw.battery_current), &(this->counter));
-	this->battery_current.under_treshold = 0;
-	this->battery_current.over_treshold = 8000;
-	this->battery_current.hold_time = 200;
-	this->battery_current.cooldown_time = 1000;
+	this->battery_current.under_treshold = def_battery_current_under_treshold;
+	this->battery_current.over_treshold = def_battery_current_over_treshold;
+	this->battery_current.hold_time = def_battery_current_hold_time;
+	this->battery_current.cooldown_time = def_battery_current_cooldown_time;
 	this->battery_current.update_ain = 0;
 	this->battery_current.auto_reset = 1;
 	
-	this->btn_up.init(&(this->board_hw.din4), 0, &(this->counter), 10);
+	this->btn_up.init(&(this->board_hw.din4), 0, &(this->counter), def_dbnc_time);
 	this->btn_up.update_din = 0;
 	
-	this->btn_down.init(&(this->board_hw.din3), 0, &(this->counter), 10);
+	this->btn_down.init(&(this->board_hw.din3), 0, &(this->counter), def_dbnc_time);
 	this->btn_down.update_din = 0;
 	
-	this->btn_mode.init(&(this->board_hw.din1), 0, &(this->counter), 10);
+	this->btn_mode.init(&(this->board_hw.din1), 0, &(this->counter), def_dbnc_time);
 	this->btn_mode.update_din = 0;
 	
-	this->handbrake.init(&(this->board_hw.hvdin3), 0, &(this->counter), 10);
+	this->handbrake.init(&(this->board_hw.hvdin3), 0, &(this->counter), def_dbnc_time);
 	this->handbrake.update_din = 0;
 	
-	this->brakes.init(&(this->board_hw.hvdin2), 1, &(this->counter), 10);
+	this->brakes.init(&(this->board_hw.hvdin2), 1, &(this->counter), def_dbnc_time);
 	this->brakes.update_din = 0;
 	
-	this->dimm.init(&(this->board_hw.hvdin1), 1, &(this->counter), 10);
+	this->dimm.init(&(this->board_hw.hvdin1), 1, &(this->counter), def_dbnc_time);
 	this->dimm.update_din = 0;
 	
-	this->pot.init(&(this->board_hw.ain2), 500, 4500);
+	this->pot.init(&(this->board_hw.ain2), def_pot_dead_bot, def_pot_dead_top);
 	this->pot.update_ain = 0;
 	
 	hw::OutReg::outRegCfg_t outreg_cfg;
@@ -89,6 +122,8 @@ void dccd::DccdHw::init(dccdHwCfg_t* cfg)
 	outreg_cfg.uout = &this->board_hw.out_voltage;
 	outreg_cfg.iout = &this->board_hw.out_current;
 	this->outreg.init(&outreg_cfg);
+	this->outreg.cc_mode_en = def_cc_mode_en;
+	this->outreg.update_ain = 0;
 	
 	hw::LedDisplay::doutCfg_t dsp_cfg;
 	dsp_cfg.led0_dout_ch = &(this->board_hw.od1);
@@ -114,12 +149,10 @@ void dccd::DccdHw::init(dccdHwCfg_t* cfg)
 	else this->board_hw.freq_pull.write(0);
 	
 	// Set initial output states
-	this->outreg.voltage = 0;
-	this->outreg.current = 0;
-	this->outreg.out_on = 0;
-	this->outreg.lock = 0;
-	this->outreg.cc_mode_en = 0;
-	this->outreg.update_ain = 0;
+	this->outreg.write_voltage(0);
+	this->outreg.write_current(0);
+	this->outreg.write_on(0);
+	this->outreg.write_lock(0);
 	this->outreg.process();
 	
 	this->display.write_backlight(100);
diff --git a/firmware/src/dccd/dccd_hw.h b/firmware/src/dccd/dccd_hw.h
index 6ed8897..3ccdd0c 100644
--- a/firmware/src/dccd/dccd_hw.h
+++ b/firmware/src/dccd/dccd_hw.h
@@ -22,6 +22,7 @@ class DccdHw
 		uint8_t pwm_f_khz;
 		uint8_t handbrake_pull_up;
 		uint8_t speed_hall;
+		uint16_t counter_step_us;
 	} dccdHwCfg_t;
 		
 	DccdHw(void);
diff --git a/firmware/src/hw/led_display.cpp b/firmware/src/hw/led_display.cpp
index 867e7f0..ac525f4 100644
--- a/firmware/src/hw/led_display.cpp
+++ b/firmware/src/hw/led_display.cpp
@@ -41,6 +41,7 @@ void hw::LedDisplay::init(doutCfg_t* dout_chs, uint8_t act_lvl, util::VCounter*
 	this->cycle_cnt = 0;
 	this->cycle_limit = 0;
 	this->timestamp_start = 0;
+	this->image = 0x00;
 	
 	this->force(0x00);
 	this->write_backlight(0);
@@ -83,9 +84,10 @@ void hw::LedDisplay::write(uint8_t image)
 	this->cycle_cnt = 0;
 	this->cycle_limit = 0;
 	this->timestamp_start = 0;
+	this->image = image;
 	
 	// Set initial state
-	this->force(image);
+	this->force(this->image);
 }
 
 void hw::LedDisplay::write(uint8_t image, uint16_t on_time, uint16_t period, uint8_t cycle_limit)
@@ -95,9 +97,10 @@ void hw::LedDisplay::write(uint8_t image, uint16_t on_time, uint16_t period, uin
 	this->period = period;
 	this->cycle_cnt = 0;
 	this->cycle_limit = cycle_limit;
+	this->image = image;
 	
 	// Set initial state
-	if(this->on_time > 0) this->force(image);
+	if(this->on_time > 0) this->force(this->image);
 	else this->force(0x00);
 	
 	// Cycle start time
diff --git a/firmware/src/hw/out_reg.cpp b/firmware/src/hw/out_reg.cpp
index 2e9f8ff..da62478 100644
--- a/firmware/src/hw/out_reg.cpp
+++ b/firmware/src/hw/out_reg.cpp
@@ -35,6 +35,39 @@ void hw::OutReg::init(outRegCfg_t* cfg)
 	
 	this->cc_mode_en = 0;
 	this->update_ain = 0;
+	this->cc_tolerance = 75;
+}
+
+void hw::OutReg::write_voltage(uint16_t voltage)
+{
+	this->voltage = voltage;
+}
+
+void hw::OutReg::write_current(uint16_t current)
+{
+	this->current = current;
+	this->current_bot = util::sat_subtract(current, this->cc_tolerance);
+	this->current_top = util::sat_add(current, this->cc_tolerance);
+}
+
+void hw::OutReg::write_on(uint8_t state)
+{
+	this->out_on = state;
+}
+
+void hw::OutReg::write_lock(uint8_t state)
+{
+	this->lock = state;
+}
+
+uint16_t hw::OutReg::read_voltage(void)
+{
+	return this->voltage;
+}
+
+uint16_t hw::OutReg::read_current(void)
+{
+	return this->current;
 }
 
 void hw::OutReg::process(void)
@@ -61,20 +94,23 @@ void hw::OutReg::process(void)
 	};
 
 	// Calculate next duty cycle setting
-	uint16_t next_duty;
+	uint16_t next_duty = this->pwm_high->get_set_duty();
 	
 	if((this->voltage==0)||(this->current==0))
 	{
 		// Off but not HiZ
 		next_duty = 0;
 	}
-	else if((this->cc_mode_en)&&(this->iout->last_read > this->current))
+	else if((this->cc_mode_en)&&(this->iout->last_read > this->current_bot))
 	{
-		// Constant current mode
-		// Reduce voltage to be within current limit
-		uint32_t temp = (uint32_t)this->pwm_high->get_set_duty() * (uint32_t)this->current;
-		temp /= this->iout->last_read;
-		next_duty = util::sat_cast(temp);
+		// Constant current mode - Change voltage to be within current limit
+		if(util::is_in_range(this->iout->last_read, this->current_bot, this->current_top)==0)
+		{
+			// Current outside of tolerance. Recalculate duty cycle.
+			uint32_t temp = (uint32_t)this->pwm_high->get_set_duty() * (uint32_t)this->current;
+			temp /= this->iout->last_read;
+			next_duty = util::sat_cast(temp);
+		};
 	}
 	else
 	{
diff --git a/firmware/src/hw/out_reg.h b/firmware/src/hw/out_reg.h
index 2b93ab1..54ca57a 100644
--- a/firmware/src/hw/out_reg.h
+++ b/firmware/src/hw/out_reg.h
@@ -25,13 +25,17 @@ class OutReg
 	
 	void init(outRegCfg_t* cfg);
 	
-	uint16_t voltage;
-	uint16_t current;
-	uint8_t out_on;
-	uint8_t lock;
-	
 	uint8_t cc_mode_en;
 	uint8_t update_ain;
+	uint16_t cc_tolerance;
+	
+	void write_voltage(uint16_t voltage);
+	void write_current(uint16_t current);
+	void write_on(uint8_t state);
+	void write_lock(uint8_t state);
+	
+	uint16_t read_voltage(void);
+	uint16_t read_current(void);
 	
 	void process(void);
 	void force_off(void);
@@ -44,6 +48,12 @@ class OutReg
 	bsp::AnalogIn* ubat;
 	bsp::AnalogIn* uout;
 	bsp::AnalogIn* iout;
+	uint16_t voltage;
+	uint16_t current;
+	uint16_t current_top;
+	uint16_t current_bot;
+	uint8_t out_on;
+	uint8_t lock;
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index 2da7942..f1f6d4c 100644
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -1,38 +1,56 @@
 /**** Includes ****/
 #include "utils/utils.h"
 
+#include "dccd/dccd_hw.h"
 #include "dccd/dccd.h"
 
 /**** Private definitions ****/
 /**** Private constants ****/
 /**** Private variables ****/
+static dccd::DccdHw dccd_hw;
+static dccd::DccdApp app;
 
 /**** Private function declarations ****/
 /**** Public function definitions ****/
 int main(void)
 {
 	// Setup
-	dccd::DccdApp dccd_app;
+	dccd::DccdHw::dccdHwCfg_t cfg;
+	cfg.handbrake_pull_up = 1;
+	cfg.pwm_f_khz = 16;
+	cfg.speed_hall = 0;
+	cfg.counter_step_us = 2000;
+	dccd_hw.init(&cfg);
 	
-	dccd_app.init();
+	app.init(&dccd_hw);
 	
-	dccd_app.lock_current = 4500;
-	dccd_app.max_hbrake_time = 1000;
-	dccd_app.btn_repeat_time = 500;
-	dccd_app.button_inputs = 1;
-	dccd_app.display_brigth = 100;
-	dccd_app.display_dimm = 50;
+	//#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;
 	
-	dccd_app.btn_force = 0;
-	dccd_app.pot_force = 0;
-	dccd_app.brake_mode = 0;
+	// 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)
 	{	
-		dccd_app.process();
-		
-		continue; // End of super loop
+		// Do stuff
+		app.process();
+		// End of super loop
+		continue; 
 	}
 	
 	// Escape the matrix
diff --git a/firmware/src/uDCCD.cppproj b/firmware/src/uDCCD.cppproj
index 7919a1a..983558e 100644
--- a/firmware/src/uDCCD.cppproj
+++ b/firmware/src/uDCCD.cppproj
@@ -40,6 +40,22 @@
         </dependencies>
       </framework-data>
     </AsfFrameworkConfig>
+    <avrtool>com.atmel.avrdbg.tool.atmelice</avrtool>
+    <avrtoolserialnumber>J42700001490</avrtoolserialnumber>
+    <avrdeviceexpectedsignature>0x1E9516</avrdeviceexpectedsignature>
+    <com_atmel_avrdbg_tool_atmelice>
+      <ToolOptions>
+        <InterfaceProperties>
+          <IspClock>249992</IspClock>
+        </InterfaceProperties>
+        <InterfaceName>ISP</InterfaceName>
+      </ToolOptions>
+      <ToolType>com.atmel.avrdbg.tool.atmelice</ToolType>
+      <ToolNumber>J42700001490</ToolNumber>
+      <ToolName>Atmel-ICE</ToolName>
+    </com_atmel_avrdbg_tool_atmelice>
+    <avrtoolinterface>ISP</avrtoolinterface>
+    <avrtoolinterfaceclock>249992</avrtoolinterfaceclock>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
     <ToolchainSettings>