diff --git a/firmware/src/bsp/board.h b/firmware/src/bsp/board.h
index bce363e..97a5d9d 100644
--- a/firmware/src/bsp/board.h
+++ b/firmware/src/bsp/board.h
@@ -9,6 +9,7 @@
 #include "din.h"
 #include "dout.h"
 #include "pwm_out.h"
+#include "memory.h"
 
 namespace bsp {
 
diff --git a/firmware/src/bsp/memory.h b/firmware/src/bsp/memory.h
index 141d4ee..b66c293 100644
--- a/firmware/src/bsp/memory.h
+++ b/firmware/src/bsp/memory.h
@@ -1,5 +1,5 @@
-#ifndef DIGITAL_IN_H_
-#define DIGITAL_IN_H_
+#ifndef MEMORY_IN_H_
+#define MEMORY_IN_H_
 
 /**** Includes ****/
 #include <stdint.h>
@@ -33,4 +33,4 @@ class Memory
 
 } //namespace
 
-#endif /* DIGITAL_IN_H_ */
\ No newline at end of file
+#endif /* MEMORY_IN_H_ */
\ No newline at end of file
diff --git a/firmware/src/dccd/dccd.cpp b/firmware/src/dccd/dccd.cpp
new file mode 100644
index 0000000..a77e39f
--- /dev/null
+++ b/firmware/src/dccd/dccd.cpp
@@ -0,0 +1,257 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "dccd.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::DccdApp::DccdApp(void)
+{
+	return;
+}
+
+dccd::DccdApp::~DccdApp(void)
+{
+	return;
+}
+
+void dccd::DccdApp::init(void)
+{
+	// Read settings from EEPROM
+	
+	DccdHw::dccdHwCfg_t cfg;
+	cfg.handbrake_pull_up = 1;
+	cfg.pwm_f_khz = 16;
+	cfg.speed_hall = 0;
+	
+	this->hardware.init(&cfg);
+	
+	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->pot_force = 0;
+	this->brake_mode = 0;
+	
+	this->hardware.read();
+	
+	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.write();
+}
+
+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_repeat_time)))
+	{
+		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;
+	};
+	
+	// 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)))
+	{
+		this->hardware.btn_up.time_reset();
+		this->hardware.btn_up.is_new = 0;
+		// Increase user force
+		this->btn_force += 10;
+		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->hardware.btn_down.time_reset();
+		this->hardware.btn_down.is_new = 0;
+		// Decrease user force
+		this->btn_force -= 10;
+		if(this->btn_force > 100) this->btn_force = 0;
+		is_new_btn_force = 1;
+	};
+	
+	this->pot_force = this->hardware.pot.last_percent;
+	
+	// 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;
+	
+	if((this->hardware.handbrake.state == 1)&&(this->hardware.handbrake.time_read() < this->max_hbrake_time))
+	{
+		// 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:
+			this->brake_mode = -1;
+			break;
+		}
+	}
+	else
+	{
+		// User force
+		next_force = user_force;
+	}
+	
+	// Apply next force
+	if(next_force < 0)
+	{
+		// HiZ
+		this->hardware.outreg.voltage = 0;
+		this->hardware.outreg.current = 0;
+		this->hardware.outreg.out_on = 0;
+	}
+	else if(next_force == 0)
+	{
+		// Open
+		this->hardware.outreg.voltage = 0;
+		this->hardware.outreg.current = 0;
+		this->hardware.outreg.out_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;
+	}
+	
+	// Display image
+	if(is_new_mode)
+	{
+		uint8_t image;
+		switch(this->brake_mode)
+		{
+			case 0:
+			image = 0x07;
+			break;
+			
+			case 1:
+			image = 0x1E;
+			break;
+			
+			case 2:
+			image = 0x38;
+			break;
+			
+			default:
+			image = 0x07;
+			break;
+		}
+		this->hardware.display.write(image, 1000, 1000, 1);
+	}
+	else if(is_new_btn_force)
+	{
+		this->hardware.display.write(img_gen_dot10(this->btn_force), 500, 500, 1);
+	}
+	else if(this->hardware.display.is_cycle_end())
+	{
+		this->hardware.display.write(img_gen_dot10(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);
+	};
+	
+	// Execute outputs
+	this->hardware.write();
+}
+
+/**** 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;
+}
+
+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;
+}
+
+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;
+}
diff --git a/firmware/src/dccd/dccd.h b/firmware/src/dccd/dccd.h
new file mode 100644
index 0000000..a723aa2
--- /dev/null
+++ b/firmware/src/dccd/dccd.h
@@ -0,0 +1,44 @@
+#ifndef DCCD_APP_H_
+#define DCCD_APP_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class DccdApp
+{
+	public:
+	DccdApp(void);
+	~DccdApp(void);
+	
+	void init(void);
+	void process(void);
+	
+	uint16_t lock_current;
+	uint16_t max_hbrake_time;
+	uint16_t btn_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;
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw hardware;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_APP_H_ */
\ No newline at end of file
diff --git a/firmware/src/dccd/dccd_hw.cpp b/firmware/src/dccd/dccd_hw.cpp
index 4ebc45d..7bceec4 100644
--- a/firmware/src/dccd/dccd_hw.cpp
+++ b/firmware/src/dccd/dccd_hw.cpp
@@ -2,7 +2,7 @@
 #include "../utils/utils.h"
 #include "dccd_hw.h"
 
-using namespace hw;
+using namespace dccd;
 
 /**** Private definitions ****/
 /**** Private constants ****/
@@ -22,10 +22,8 @@ dccd::DccdHw::~DccdHw(void)
 void dccd::DccdHw::init(dccdHwCfg_t* cfg)
 {
 	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);
@@ -84,7 +82,13 @@ void dccd::DccdHw::init(dccdHwCfg_t* cfg)
 	this->pot.init(&(this->board_hw.ain2), 500, 4500);
 	this->pot.update_ain = 0;
 	
-	this->outdriver.init(&(this->board_hw.out_pwm), &(this->board_hw.out_low));
+	hw::OutReg::outRegCfg_t outreg_cfg;
+	outreg_cfg.pwm_high = &this->board_hw.out_pwm;
+	outreg_cfg.dout_low = &this->board_hw.out_low;
+	outreg_cfg.ubat = &this->board_hw.battery_voltage;
+	outreg_cfg.uout = &this->board_hw.out_voltage;
+	outreg_cfg.iout = &this->board_hw.out_current;
+	this->outreg.init(&outreg_cfg);
 	
 	hw::LedDisplay::doutCfg_t dsp_cfg;
 	dsp_cfg.led0_dout_ch = &(this->board_hw.od1);
@@ -110,8 +114,13 @@ void dccd::DccdHw::init(dccdHwCfg_t* cfg)
 	else this->board_hw.freq_pull.write(0);
 	
 	// Set initial output states
-	this->outdriver.write((uint16_t)0);
-	this->outdriver.enable();
+	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.process();
 	
 	this->display.write_backlight(100);
 	this->display.write(0x00);
@@ -139,4 +148,10 @@ void dccd::DccdHw::read(void)
 	this->pot.read();
 }
 
+void dccd::DccdHw::write(void)
+{
+	this->display.process();
+	this->outreg.process();
+}
+
 /**** Private function definitions ***/
diff --git a/firmware/src/dccd/dccd_hw.h b/firmware/src/dccd/dccd_hw.h
index a4d864c..6ed8897 100644
--- a/firmware/src/dccd/dccd_hw.h
+++ b/firmware/src/dccd/dccd_hw.h
@@ -10,6 +10,7 @@
 #include "../hw/potentiometer.h"
 #include "../hw/out_driver.h"
 #include "../hw/safe_ain.h"
+#include "../hw/out_reg.h"
 
 namespace dccd {
 
@@ -45,11 +46,10 @@ class DccdHw
 	
 	// Outputs
 	hw::LedDisplay display;
-	hw::OutDriver outdriver;
-	
-	// Safety
+	hw::OutReg outreg;
 	
 	void read(void);
+	void write(void);
 	
 	#ifdef TESTING
 	protected:
diff --git a/firmware/src/hw/out_reg.cpp b/firmware/src/hw/out_reg.cpp
new file mode 100644
index 0000000..2e9f8ff
--- /dev/null
+++ b/firmware/src/hw/out_reg.cpp
@@ -0,0 +1,103 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "out_reg.h"
+
+using namespace bsp;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+hw::OutReg::OutReg(void)
+{
+	return;
+}
+
+hw::OutReg::~OutReg(void)
+{
+	return;
+}
+
+void hw::OutReg::init(outRegCfg_t* cfg)
+{
+	this->pwm_high = cfg->pwm_high;
+	this->dout_low = cfg->dout_low;
+	this->ubat = cfg->ubat;
+	this->uout = cfg->uout;
+	this->iout = cfg->iout;
+	
+	this->voltage = 0;
+	this->current = 0;
+	this->out_on = 0;
+	this->lock = 0;
+	
+	this->cc_mode_en = 0;
+	this->update_ain = 0;
+}
+
+void hw::OutReg::process(void)
+{
+	// Update analog input
+	if(this->update_ain)
+	{
+		this->ubat->read();
+		this->uout->read();
+		this->iout->read();
+	};
+	
+	// Check if turned off
+	if((out_on == 0)||(this->lock != 0))
+	{
+		
+		this->pwm_high->write((uint16_t)0);
+		this->dout_low->write(0);
+		return;
+	}
+	else if(this->dout_low->last_writen == 0)
+	{
+		this->dout_low->write(1);
+	};
+
+	// Calculate next duty cycle setting
+	uint16_t next_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))
+	{
+		// 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);
+	}
+	else
+	{
+		// Constant voltage mode
+		next_duty = util::sat_ratio(this->voltage, this->ubat->last_read);
+	}
+	
+	this->pwm_high->write(next_duty);
+	
+	return;
+}
+
+void hw::OutReg::force_off(void)
+{
+	// Turn off output - HiZ
+	this->pwm_high->write((uint16_t)0);
+	this->dout_low->write(0);
+	
+	// Update targets
+	this->voltage = 0;
+	this->current = 0;
+	this->out_on = 0;
+	this->lock = 1;
+}
+
+/**** Private function definitions ****/
diff --git a/firmware/src/hw/out_reg.h b/firmware/src/hw/out_reg.h
new file mode 100644
index 0000000..2b93ab1
--- /dev/null
+++ b/firmware/src/hw/out_reg.h
@@ -0,0 +1,56 @@
+#ifndef OUTPUT_REGULATOR_H_
+#define OUTPUT_REGULATOR_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../bsp/board.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+
+class OutReg
+{
+	public:
+	typedef struct {
+		bsp::PwmOut* pwm_high;
+		bsp::DigitalOut* dout_low;
+		bsp::AnalogIn* ubat;
+		bsp::AnalogIn* uout;
+		bsp::AnalogIn* iout;
+	} outRegCfg_t;
+	
+	OutReg(void);
+	~OutReg(void);
+	
+	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;
+	
+	void process(void);
+	void force_off(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	bsp::PwmOut* pwm_high;
+	bsp::DigitalOut* dout_low;
+	bsp::AnalogIn* ubat;
+	bsp::AnalogIn* uout;
+	bsp::AnalogIn* iout;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* OUTPUT_REGULATOR_H_ */
\ No newline at end of file
diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index aba810f..2da7942 100644
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -1,32 +1,36 @@
 /**** Includes ****/
 #include "utils/utils.h"
 
-#include "dccd/dccd_hw.h"
+#include "dccd/dccd.h"
 
 /**** Private definitions ****/
 /**** Private constants ****/
 /**** Private variables ****/
-dccd::DccdHw dccd_hw;
 
 /**** Private function declarations ****/
 /**** Public function definitions ****/
 int main(void)
 {
 	// Setup
-	dccd::DccdHw::dccdHwCfg_t hw_cfg;
+	dccd::DccdApp dccd_app;
 	
-	hw_cfg.handbrake_pull_up = 0;
-	hw_cfg.speed_hall = 0;
+	dccd_app.init();
 	
-	dccd_hw.init(&hw_cfg);
+	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;
+	
+	dccd_app.btn_force = 0;
+	dccd_app.pot_force = 0;
+	dccd_app.brake_mode = 0;
 	
 	// Super loop
 	while(1)
 	{	
-		// Update inputs
-		dccd_hw.read();
-		
-		// Do something
+		dccd_app.process();
 		
 		continue; // End of super loop
 	}
diff --git a/firmware/src/sort/cv_out.cpp b/firmware/src/sort/cv_out.cpp
deleted file mode 100644
index 4800e9d..0000000
--- a/firmware/src/sort/cv_out.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "cv_out.h"
-
-using namespace bsp;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-bsp::CVOut::CVOut(void)
-{
-	return;
-}
-
-bsp::CVOut::~CVOut(void)
-{
-	return;
-}
-
-void bsp::CVOut::init(AnalogIn* ain_ch, Halfbridge* out_ch)
-{
-	this->ain_ch = ain_ch;
-	this->out_ch = out_ch;
-	
-	this->voltage = 0;
-	
-	this->disabled = 1;
-	this->process_ain = 1;
-}
-
-void bsp::CVOut::process(void)
-{
-	// Update analog input
-	if(this->process_ain) this->ain_ch->read();
-	
-	// Check to HiZ output
-	if(this->disabled)
-	{
-		this->out_ch->write_hiz();
-		return;
-	};
-	
-	// Calculate and apply necessary PWM
-	this->out_ch->write(util::sat_ratio(this->voltage, this->ain_ch->last_read));
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/sort/cv_out.h b/firmware/src/sort/cv_out.h
deleted file mode 100644
index 87fd437..0000000
--- a/firmware/src/sort/cv_out.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#ifndef CONST_VOLTAGE_OUTPUT_H_
-#define CONST_VOLTAGE_OUTPUT_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "ain.h"
-#include "halfbridge.h"
-
-namespace bsp {
-
-/**** Public definitions ****/
-
-class CVOut
-{
-	public:
-	CVOut(void);
-	~CVOut(void);
-	
-	void init(AnalogIn* ain_ch, Halfbridge* out_ch);
-	
-	uint16_t voltage;
-	uint8_t disabled;
-	uint8_t process_ain;
-	
-	void process(void);
-	
-	#ifndef TESTING
-	protected:
-	#endif
-	AnalogIn* ain_ch;
-	Halfbridge* out_ch;
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* CONST_VOLTAGE_OUTPUT_H_ */
\ No newline at end of file
diff --git a/firmware/src/uDCCD.cppproj b/firmware/src/uDCCD.cppproj
index 5d31aad..7919a1a 100644
--- a/firmware/src/uDCCD.cppproj
+++ b/firmware/src/uDCCD.cppproj
@@ -201,6 +201,12 @@
     <Compile Include="bsp\mcu\mcu_hal_r8.cpp">
       <SubType>compile</SubType>
     </Compile>
+    <Compile Include="dccd\dccd.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\dccd.h">
+      <SubType>compile</SubType>
+    </Compile>
     <Compile Include="dccd\dccd_hw.cpp">
       <SubType>compile</SubType>
     </Compile>
@@ -219,10 +225,16 @@
     <Compile Include="hw\led_display.h">
       <SubType>compile</SubType>
     </Compile>
-    <Compile Include="hw\lockout.cpp">
+    <Compile Include="hw\out_reg.cpp">
       <SubType>compile</SubType>
     </Compile>
-    <Compile Include="hw\lockout.h">
+    <Compile Include="hw\out_reg.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\safe_ain.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\safe_ain.h">
       <SubType>compile</SubType>
     </Compile>
     <Compile Include="hw\out_driver.cpp">