diff --git a/commands.c b/commands.c
index 82e8e75c..fc1be0af 100644
--- a/commands.c
+++ b/commands.c
@@ -324,11 +324,12 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		mcconf.p_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
 		mcconf.p_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
 		mcconf.p_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
+		mcconf.p_pid_ang_div = (float)buffer_get_int32(data, &ind) / 100000.0;
 
 		mcconf.cc_startup_boost_duty = (float)buffer_get_int32(data, &ind) / 1000000.0;
 		mcconf.cc_min_current = (float)buffer_get_int32(data, &ind) / 1000.0;
 		mcconf.cc_gain = (float)buffer_get_int32(data, &ind) / 1000000.0;
-		mcconf.cc_ramp_step_max = (float)buffer_get_int32(data, &ind) / 1000000.0;
+		mcconf.cc_ramp_step_max = buffer_get_float32(data, 1e5, &ind);
 
 		mcconf.m_fault_stop_time_ms = buffer_get_int32(data, &ind);
 		mcconf.m_duty_ramp_step = (float)buffer_get_float32(data, 1000000.0, &ind);
@@ -431,6 +432,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kp * 1000000.0), &ind);
 		buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_ki * 1000000.0), &ind);
 		buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kd * 1000000.0), &ind);
+		buffer_append_float32(send_buffer, mcconf.p_pid_ang_div, 1e5, &ind);
 
 		buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_startup_boost_duty * 1000000.0), &ind);
 		buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_min_current * 1000.0), &ind);
@@ -853,7 +855,7 @@ void commands_send_app_data(unsigned char *data, unsigned int len) {
 	int32_t index = 0;
 
 	send_buffer[index++] = COMM_CUSTOM_APP_DATA;
-	memcpy(send_buffer, data, len);
+	memcpy(send_buffer + index, data, len);
 	index += len;
 
 	commands_send_packet(send_buffer, index);
diff --git a/commands.h b/commands.h
index ac320b59..c25d323a 100644
--- a/commands.h
+++ b/commands.h
@@ -38,5 +38,6 @@ void commands_send_rotor_pos(float rotor_pos);
 void commands_send_experiment_samples(float *samples, int len);
 disp_pos_mode commands_get_disp_pos_mode(void);
 void commands_set_app_data_handler(void(*func)(unsigned char *data, unsigned int len));
+void commands_send_app_data(unsigned char *data, unsigned int len);
 
 #endif /* COMMANDS_H_ */
diff --git a/conf_general.c b/conf_general.c
index 75752976..b2a04556 100644
--- a/conf_general.c
+++ b/conf_general.c
@@ -228,6 +228,7 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
 	conf->p_pid_kp = MCCONF_P_PID_KP;
 	conf->p_pid_ki = MCCONF_P_PID_KI;
 	conf->p_pid_kd = MCCONF_P_PID_KD;
+	conf->p_pid_ang_div = MCCONF_P_PID_ANG_DIV;
 
 	conf->cc_startup_boost_duty = MCCONF_CC_STARTUP_BOOST_DUTY;
 	conf->cc_min_current = MCCONF_CC_MIN_CURRENT;
diff --git a/conf_general.h b/conf_general.h
index 985b2143..a2e3c792 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -27,7 +27,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR	2
-#define FW_VERSION_MINOR	15
+#define FW_VERSION_MINOR	16
 
 #include "datatypes.h"
 
@@ -64,10 +64,7 @@
 /*
  * Select default user motor configuration
  */
-//#define MCCONF_DEFAULT_USER		"mcconf_outrunner2.h"
 //#define MCCONF_DEFAULT_USER		"mcconf_sten.h"
-//#define MCCONF_DEFAULT_USER		"mcconf_foc_erwin.h"
-//#define MCCONF_DEFAULT_USER		"mcconf_foc_scorpion.h"
 
 /*
  * Select default user app configuration
diff --git a/datatypes.h b/datatypes.h
index f1b6aedb..42b50843 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -90,7 +90,8 @@ typedef enum {
 	DISP_POS_MODE_INDUCTANCE,
 	DISP_POS_MODE_OBSERVER,
 	DISP_POS_MODE_ENCODER,
-	DISP_POS_MODE_ENCODER_POS_ERROR,
+	DISP_POS_MODE_PID_POS,
+	DISP_POS_MODE_PID_POS_ERROR,
 	DISP_POS_MODE_ENCODER_OBSERVER_ERROR
 } disp_pos_mode;
 
@@ -181,6 +182,7 @@ typedef struct {
 	float p_pid_kp;
 	float p_pid_ki;
 	float p_pid_kd;
+	float p_pid_ang_div;
 	// Current controller
 	float cc_startup_boost_duty;
 	float cc_min_current;
diff --git a/main.c b/main.c
index b04a7dd5..34075a79 100644
--- a/main.c
+++ b/main.c
@@ -110,8 +110,12 @@ static THD_FUNCTION(periodic_thread, arg) {
 				commands_send_rotor_pos(encoder_read_deg());
 				break;
 
-			case DISP_POS_MODE_ENCODER_POS_ERROR:
-				commands_send_rotor_pos(utils_angle_difference(mc_interface_get_pos_set(), encoder_read_deg()));
+			case DISP_POS_MODE_PID_POS:
+				commands_send_rotor_pos(mc_interface_get_pid_pos_now());
+				break;
+
+			case DISP_POS_MODE_PID_POS_ERROR:
+				commands_send_rotor_pos(utils_angle_difference(mc_interface_get_pid_pos_set(), mc_interface_get_pid_pos_now()));
 				break;
 
 			default:
diff --git a/mc_interface.c b/mc_interface.c
index 6630ccc3..89b47cdc 100644
--- a/mc_interface.c
+++ b/mc_interface.c
@@ -33,6 +33,7 @@
 #include "ch.h"
 #include "hal.h"
 #include "commands.h"
+#include "encoder.h"
 #include <math.h>
 
 // Global variables
@@ -741,10 +742,30 @@ float mc_interface_read_reset_avg_input_current(void) {
 	return res;
 }
 
-float mc_interface_get_pos_set(void) {
+float mc_interface_get_pid_pos_set(void) {
 	return m_position_set;
 }
 
+float mc_interface_get_pid_pos_now(void) {
+	float ret = 0.0;
+
+	switch (m_conf.motor_type) {
+	case MOTOR_TYPE_BLDC:
+	case MOTOR_TYPE_DC:
+		ret = encoder_read_deg();
+		break;
+
+	case MOTOR_TYPE_FOC:
+		ret = mcpwm_foc_get_pid_pos_now();
+		break;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
 float mc_interface_get_last_sample_adc_isr_duration(void) {
 	return m_last_adc_duration_sample;
 }
diff --git a/mc_interface.h b/mc_interface.h
index ea2d81d3..18b24098 100644
--- a/mc_interface.h
+++ b/mc_interface.h
@@ -65,7 +65,8 @@ int mc_interface_get_tachometer_abs_value(bool reset);
 float mc_interface_get_last_inj_adc_isr_duration(void);
 float mc_interface_read_reset_avg_motor_current(void);
 float mc_interface_read_reset_avg_input_current(void);
-float mc_interface_get_pos_set(void);
+float mc_interface_get_pid_pos_set(void);
+float mc_interface_get_pid_pos_now(void);
 float mc_interface_get_last_sample_adc_isr_duration(void);
 void mc_interface_sample_print_data(bool at_start, uint16_t len, uint8_t decimation);
 
diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h
index 31c335d3..22695afc 100644
--- a/mcconf/mcconf_default.h
+++ b/mcconf/mcconf_default.h
@@ -121,6 +121,9 @@
 #ifndef MCCONF_P_PID_KD
 #define MCCONF_P_PID_KD					0.0004	// Derivative gain
 #endif
+#ifndef MCCONF_P_PID_ANG_DIV
+#define MCCONF_P_PID_ANG_DIV			1.0		// Divide angle by this value
+#endif
 
 // Current control parameters
 #ifndef MCCONF_CC_GAIN
@@ -217,7 +220,7 @@
 #define MCCONF_FOC_PLL_KP				2000.0
 #endif
 #ifndef MCCONF_FOC_PLL_KI
-#define MCCONF_FOC_PLL_KI				40000.0
+#define MCCONF_FOC_PLL_KI				20000.0
 #endif
 #ifndef MCCONF_FOC_MOTOR_L
 #define MCCONF_FOC_MOTOR_L				0.000007
diff --git a/mcconf/mcconf_foc_erwin.h b/mcconf/mcconf_foc_erwin.h
deleted file mode 100644
index 62678958..00000000
--- a/mcconf/mcconf_foc_erwin.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * mcconf_foc_erwin.h
- *
- *  Created on: 25 okt 2015
- *      Author: benjamin
- */
-
-#ifndef MCCONF_FOC_ERWIN_H_
-#define MCCONF_FOC_ERWIN_H_
-
-#define MCCONF_DEFAULT_MOTOR_TYPE		MOTOR_TYPE_FOC
-
-#define MCCONF_FOC_CURRENT_KP			0.05
-#define MCCONF_FOC_CURRENT_KI			50.0
-#define MCCONF_FOC_F_SW					50000.0
-#define MCCONF_FOC_ENCODER_INVERTED		true
-#define MCCONF_FOC_ENCODER_OFFSET		92.0
-#define MCCONF_FOC_ENCODER_RATIO		7.0
-#define MCCONF_FOC_SENSOR_MODE			FOC_SENSOR_MODE_ENCODER
-#define MCCONF_FOC_PLL_KP				40.0
-#define MCCONF_FOC_PLL_KI				40000.0
-#define MCCONF_FOC_MOTOR_L				0.000064
-#define MCCONF_FOC_MOTOR_R				0.038
-#define MCCONF_FOC_MOTOR_FLUX_LINKAGE	0.0085
-#define MCCONF_FOC_OBSERVER_GAIN		9e6		// Can be something like 600 / L
-
-#define MCCONF_S_PID_KP					0.0001
-#define MCCONF_S_PID_KI					0.0005
-#define MCCONF_S_PID_KD					0.0
-#define MCCONF_S_PID_MIN_RPM			100.0
-
-#define MCCONF_P_PID_KP					0.01
-#define MCCONF_P_PID_KI					0.0
-#define MCCONF_P_PID_KD					0.0006
-
-#endif /* MCCONF_FOC_ERWIN_H_ */
diff --git a/mcconf/mcconf_foc_scorpion.h b/mcconf/mcconf_foc_scorpion.h
deleted file mode 100644
index 3bd281da..00000000
--- a/mcconf/mcconf_foc_scorpion.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef MCCONF_FOC_SCORPION_H_
-#define MCCONF_FOC_SCORPION_H_
-
-#define MCCONF_DEFAULT_MOTOR_TYPE		MOTOR_TYPE_FOC
-
-#define MCCONF_FOC_CURRENT_KP			0.05
-#define MCCONF_FOC_CURRENT_KI			50.0
-#define MCCONF_FOC_F_SW					50000.0
-#define MCCONF_FOC_SENSOR_MODE			FOC_SENSOR_MODE_SENSORLESS
-#define MCCONF_FOC_PLL_KP				40.0
-#define MCCONF_FOC_PLL_KI				40000.0
-#define MCCONF_FOC_MOTOR_L				0.000007
-#define MCCONF_FOC_MOTOR_R				0.015
-#define MCCONF_FOC_MOTOR_FLUX_LINKAGE	0.00245
-#define MCCONF_FOC_OBSERVER_GAIN		9e7		// Can be something like 600 / L
-
-#define MCCONF_S_PID_KP					0.0001
-#define MCCONF_S_PID_KI					0.0005
-#define MCCONF_S_PID_KD					0.0
-#define MCCONF_S_PID_MIN_RPM			100.0
-
-#define MCCONF_P_PID_KP					0.01
-#define MCCONF_P_PID_KI					0.0
-#define MCCONF_P_PID_KD					0.0006
-
-#endif /* MCCONF_FOC_SCORPION_H_ */
diff --git a/mcconf/mcconf_outrunner2.h b/mcconf/mcconf_outrunner2.h
deleted file mode 100644
index 4836094e..00000000
--- a/mcconf/mcconf_outrunner2.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
-	Copyright 2012-2014 Benjamin Vedder	benjamin@vedder.se
-
-	This program is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-    */
-
-/*
- * mcconf_outrunner2.h
- *
- * A configuration for my scorpion 225kv outrunner.
- *
- *  Created on: 14 apr 2014
- *      Author: benjamin
- */
-
-#ifndef MCCONF_OUTRUNNER2_H_
-#define MCCONF_OUTRUNNER2_H_
-
-/*
- * Parameters
- */
-#define MCCONF_L_CURRENT_MAX				60.0	// Current limit in Amperes (Upper)
-#define MCCONF_L_CURRENT_MIN				-60.0	// Current limit in Amperes (Lower)
-#define MCCONF_L_IN_CURRENT_MAX			60.0	// Input current limit in Amperes (Upper)
-#define MCCONF_L_IN_CURRENT_MIN			-20.0	// Input current limit in Amperes (Lower)
-#define MCCONF_L_MAX_ABS_CURRENT			130.0	// The maximum absolute current above which a fault is generated
-#define MCCONF_L_SLOW_ABS_OVERCURRENT		1		// Use the filtered (and hence slower) current for the overcurrent fault detection
-
-// Sensorless settings
-#define MCCONF_SENSOR_MODE				SENSOR_MODE_SENSORLESS // Sensor mode
-#define MCCONF_SL_MIN_RPM					150		// Auto-commutate below this RPM
-#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT	1100.0	// Minimum RPM to calculate the BEMF coupling from
-#define MCCONF_SL_CYCLE_INT_LIMIT			62.0	// Flux integrator limit 0 ERPM
-#define MCCONF_SL_PHASE_ADVANCE_AT_BR	0.8		// Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
-#define MCCONF_SL_BEMF_COUPLING_K		600.0	// Input voltage to bemf coupling constant
-
-// Speed PID parameters
-#define MCCONF_S_PID_KP					0.0001	// Proportional gain
-#define MCCONF_S_PID_KI					0.002	// Integral gain
-#define MCCONF_S_PID_KD					0.0		// Derivative gain
-#define MCCONF_S_PID_MIN_RPM				900.0	// Minimum allowed RPM
-
-// Position PID parameters
-#define MCCONF_P_PID_KP					0.0001	// Proportional gain
-#define MCCONF_P_PID_KI					0.002	// Integral gain
-#define MCCONF_P_PID_KD					0.0		// Derivative gain
-
-// Current control parameters
-#define MCCONF_CC_GAIN		0.0046	// Current controller error gain
-#define MCCONF_CC_MIN_CURRENT		1.0		// Minimum allowed current
-
-#endif /* MCCONF_OUTRUNNER2_H_ */
diff --git a/mcpwm_foc.c b/mcpwm_foc.c
index 960abe99..5c80ccb9 100644
--- a/mcpwm_foc.c
+++ b/mcpwm_foc.c
@@ -89,7 +89,7 @@ static volatile float m_id_set;
 static volatile float m_iq_set;
 static volatile bool m_dccal_done;
 static volatile bool m_output_on;
-static volatile float m_pos_pid_set_pos;
+static volatile float m_pos_pid_set;
 static volatile float m_speed_pid_set_rpm;
 static volatile float m_phase_now_observer;
 static volatile float m_phase_now_observer_override;
@@ -104,6 +104,7 @@ static volatile mc_sample_t m_samples;
 static volatile int m_tachometer;
 static volatile int m_tachometer_abs;
 static volatile float last_inj_adc_isr_duration;
+static volatile float m_pos_pid_now;
 
 // Private functions
 static void do_dc_cal(void);
@@ -114,7 +115,7 @@ static void pll_run(float phase, float dt, volatile float *phase_var,
 static void control_current(volatile motor_state_t *state_m, float dt);
 static void svm(float alpha, float beta, uint32_t PWMHalfPeriod,
 		uint32_t* tAout, uint32_t* tBout, uint32_t* tCout);
-static void run_pid_control_pos(float dt);
+static void run_pid_control_pos(float angle_now, float angle_set, float dt);
 static void run_pid_control_speed(float dt);
 static void stop_pwm_hw(void);
 static void start_pwm_hw(void);
@@ -201,7 +202,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
 	m_id_set = 0.0;
 	m_iq_set = 0.0;
 	m_output_on = false;
-	m_pos_pid_set_pos = 0.0;
+	m_pos_pid_set = 0.0;
 	m_speed_pid_set_rpm = 0.0;
 	m_phase_now_observer = 0.0;
 	m_phase_now_observer_override = 0.0;
@@ -215,6 +216,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
 	m_tachometer = 0;
 	m_tachometer_abs = 0;
 	last_inj_adc_isr_duration = 0;
+	m_pos_pid_now = 0.0;
 	memset((void*)&m_motor_state, 0, sizeof(motor_state_t));
 	memset((void*)&m_samples, 0, sizeof(mc_sample_t));
 
@@ -517,7 +519,7 @@ void mcpwm_foc_set_pid_speed(float rpm) {
 	m_control_mode = CONTROL_MODE_SPEED;
 	m_speed_pid_set_rpm = rpm;
 
-	if (m_state != MC_STATE_RUNNING && fabsf(rpm) > m_conf->s_pid_min_erpm) {
+	if (m_state != MC_STATE_RUNNING) {
 		m_state = MC_STATE_RUNNING;
 	}
 }
@@ -531,7 +533,7 @@ void mcpwm_foc_set_pid_speed(float rpm) {
  */
 void mcpwm_foc_set_pid_pos(float pos) {
 	m_control_mode = CONTROL_MODE_POS;
-	m_pos_pid_set_pos = pos;
+	m_pos_pid_set = pos;
 
 	if (m_state != MC_STATE_RUNNING) {
 		m_state = MC_STATE_RUNNING;
@@ -597,6 +599,14 @@ float mcpwm_foc_get_duty_cycle_now(void) {
 	return m_motor_state.duty_now;
 }
 
+float mcpwm_foc_get_pid_pos_set(void) {
+	return m_pos_pid_set;
+}
+
+float mcpwm_foc_get_pid_pos_now(void) {
+	return m_pos_pid_now;
+}
+
 /**
  * Get the current switching frequency.
  *
@@ -1123,7 +1133,7 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
 
 	float res_tmp = 0.0;
 	float i_last = 0.0;
-	for (float i = 2.0;i < 35.0;i *= 1.5) {
+	for (float i = 2.0;i < (m_conf->l_current_max / 2.0);i *= 1.5) {
 		res_tmp = mcpwm_foc_measure_resistance(i, 20);
 
 		if (i > (0.5 / res_tmp)) {
@@ -1133,7 +1143,7 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
 	}
 
 	if (i_last < 0.01) {
-		i_last = 35.0;
+		i_last = (m_conf->l_current_max / 2.0);
 	}
 
 	*res = mcpwm_foc_measure_resistance(i_last, 500);
@@ -1406,6 +1416,13 @@ void mcpwm_foc_adc_inj_int_handler(void) {
 			duty_set = 0.0;
 		}
 
+		// Brake when set ERPM is below min ERPM
+		if (m_control_mode == CONTROL_MODE_SPEED &&
+				fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
+			control_duty = true;
+			duty_set = 0.0;
+		}
+
 		if (control_duty) {
 			// Duty cycle control
 			static float duty_i_term = 0.0;
@@ -1516,7 +1533,6 @@ void mcpwm_foc_adc_inj_int_handler(void) {
 		m_motor_state.iq_target = iq_set_tmp;
 
 		control_current(&m_motor_state, dt);
-		run_pid_control_pos(dt);
 	} else {
 		// Track back emf
 		float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
@@ -1584,6 +1600,29 @@ void mcpwm_foc_adc_inj_int_handler(void) {
 		phase_last = m_motor_state.phase;
 	}
 
+	// Track position control angle
+	// TODO: Have another look at this.
+#if ENCODER_ENABLE
+	float angle_now = encoder_read_deg();
+#else
+	float angle_now = m_motor_state.phase * (180.0 / M_PI);
+#endif
+
+	if (m_conf->p_pid_ang_div > 0.98 && m_conf->p_pid_ang_div < 1.02) {
+		m_pos_pid_now = angle_now;
+	} else {
+		static float angle_last = 0.0;
+		float diff_f = utils_angle_difference(angle_now, angle_last);
+		angle_last = angle_now;
+		m_pos_pid_now += diff_f / m_conf->p_pid_ang_div;
+		utils_norm_angle((float*)&m_pos_pid_now);
+	}
+
+	// Run position control
+	if (m_state == MC_STATE_RUNNING) {
+		run_pid_control_pos(m_pos_pid_now, m_pos_pid_set, dt);
+	}
+
 	// MCIF handler
 	mc_interface_mc_timer_isr();
 
@@ -1950,7 +1989,7 @@ static void svm(float alpha, float beta, uint32_t PWMHalfPeriod,
 	*tCout = tC;
 }
 
-static void run_pid_control_pos(float dt) {
+static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
 	static float i_term = 0;
 	static float prev_error = 0;
 	float p_term;
@@ -1963,19 +2002,30 @@ static void run_pid_control_pos(float dt) {
 		return;
 	}
 
-	// Compute error
-	float angle = encoder_read_deg();
-	float error = utils_angle_difference(m_pos_pid_set_pos, angle);
+	// Compute parameters
+	float error = utils_angle_difference(angle_set, angle_now);
 
+#if ENCODER_ENABLE
 	if (m_conf->foc_encoder_inverted) {
 		error = -error;
 	}
+#endif
 
-
-	// Compute parameters
 	p_term = error * m_conf->p_pid_kp;
 	i_term += error * (m_conf->p_pid_ki * dt);
-	d_term = (error - prev_error) * (m_conf->p_pid_kd / dt);
+
+	// Average DT for the D term when the error does not change. This likely
+	// happens at low speed when the position resolution is low and several
+	// control iterations run without position updates.
+	// TODO: Are there problems with this approach?
+	static float dt_int = 0.0;
+	dt_int += dt;
+	if (error == prev_error) {
+		d_term = 0.0;
+	} else {
+		d_term = (error - prev_error) * (m_conf->p_pid_kd / dt_int);
+		dt_int = 0.0;
+	}
 
 	// I-term wind-up protection
 	utils_truncate_number(&i_term, -1.0, 1.0);
@@ -1987,12 +2037,16 @@ static void run_pid_control_pos(float dt) {
 	float output = p_term + i_term + d_term;
 	utils_truncate_number(&output, -1.0, 1.0);
 
+#if ENCODER_ENABLE
 	if (encoder_index_found()) {
 		m_iq_set = output * m_conf->lo_current_max;
 	} else {
 		// Rotate the motor with 40 % power until the encoder index is found.
 		m_iq_set = 0.4 * m_conf->lo_current_max;
 	}
+#else
+	m_iq_set = output * m_conf->lo_current_max;
+#endif
 }
 
 static void run_pid_control_speed(float dt) {
@@ -2008,15 +2062,10 @@ static void run_pid_control_speed(float dt) {
 		return;
 	}
 
-	// Too low RPM set. Stop and return.
+	// Too low RPM set. Reset state and return.
 	if (fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
 		i_term = 0.0;
 		prev_error = 0;
-		m_iq_set = 0.0;
-		m_state = MC_STATE_OFF;
-		if (m_output_on) {
-			stop_pwm_hw();
-		}
 		return;
 	}
 
diff --git a/mcpwm_foc.h b/mcpwm_foc.h
index 4c6b1240..379e3561 100644
--- a/mcpwm_foc.h
+++ b/mcpwm_foc.h
@@ -44,6 +44,8 @@ void mcpwm_foc_set_current(float current);
 void mcpwm_foc_set_brake_current(float current);
 float mcpwm_foc_get_duty_cycle_set(void);
 float mcpwm_foc_get_duty_cycle_now(void);
+float mcpwm_foc_get_pid_pos_set(void);
+float mcpwm_foc_get_pid_pos_now(void);
 float mcpwm_foc_get_switching_frequency_now(void);
 float mcpwm_foc_get_rpm(void);
 float mcpwm_foc_get_tot_current(void);
diff --git a/utils.c b/utils.c
index fd2477bf..54f2b1d3 100644
--- a/utils.c
+++ b/utils.c
@@ -133,18 +133,24 @@ void utils_deadband(float *value, float tres, float max) {
  * The difference between the angles
  */
 float utils_angle_difference(float angle1, float angle2) {
-	utils_norm_angle(&angle1);
-	utils_norm_angle(&angle2);
+//	utils_norm_angle(&angle1);
+//	utils_norm_angle(&angle2);
+//
+//	if (fabsf(angle1 - angle2) > 180.0) {
+//		if (angle1 < angle2) {
+//			angle1 += 360.0;
+//		} else {
+//			angle2 += 360.0;
+//		}
+//	}
+//
+//	return angle1 - angle2;
 
-	if (fabsf(angle1 - angle2) > 180.0) {
-		if (angle1 < angle2) {
-			angle1 += 360.0;
-		} else {
-			angle2 += 360.0;
-		}
-	}
-
-	return angle1 - angle2;
+	// Faster in most cases
+	float difference = angle1 - angle2;
+	while (difference < -180.0) difference += 2.0 * 180.0;
+	while (difference > 180.0) difference -= 2.0 * 180.0;
+	return difference;
 }
 
 /**