diff --git a/build/BLDC_4_ChibiOS.bin b/build/BLDC_4_ChibiOS.bin
index 762c1c5f..3437ec15 100755
Binary files a/build/BLDC_4_ChibiOS.bin and b/build/BLDC_4_ChibiOS.bin differ
diff --git a/main.c b/main.c
index e302f594..81cc9897 100644
--- a/main.c
+++ b/main.c
@@ -118,13 +118,13 @@ static msg_t periodic_thread(void *arg) {
 
 		if (pwr < MIN_PWR) {
 			mcpwm_use_pid(0);
-			mcpwm_set_duty(0);
+			mcpwm_set_duty(0.0);
 		} else {
 			mcpwm_use_pid(0);
 			mcpwm_set_duty(pwr);
 		}
 #endif
-		chThdSleepMilliseconds(1);
+		chThdSleepMilliseconds(20);
 	}
 
 	return 0;
diff --git a/mcpwm.c b/mcpwm.c
index e28ada24..752a0a17 100644
--- a/mcpwm.c
+++ b/mcpwm.c
@@ -562,8 +562,13 @@ void mcpwm_set_duty(float dutyCycle) {
 
 	dutycycle_set = dutyCycle;
 
-	if (state != MC_STATE_RUNNING) {
-		set_duty_cycle(dutyCycle);
+	if (state != MC_STATE_RUNNING && dutyCycle > MCPWM_MIN_DUTY_CYCLE) {
+		if (dutyCycle > 0.0) {
+			dutycycle_now = (MCPWM_MIN_DUTY_CYCLE + 0.01);
+		} else {
+			dutycycle_now = -(MCPWM_MIN_DUTY_CYCLE + 0.01);
+		}
+		set_duty_cycle(dutycycle_now);
 	}
 }
 
@@ -734,6 +739,7 @@ static void set_duty_cycle(float dutyCycle) {
 	if (dutyCycle < MCPWM_MIN_DUTY_CYCLE) {
 		switch (state) {
 		case MC_STATE_STARTING:
+		case MC_STATE_RUNNING:
 			state = MC_STATE_OFF;
 			if (MCPWM_FULL_BRAKE_AT_STOP) {
 				mcpwm_full_brake();
@@ -747,13 +753,6 @@ static void set_duty_cycle(float dutyCycle) {
 			stop_pwm(); // TODO: Full break?
 			break;
 
-		case MC_STATE_RUNNING:
-			if (!MCPWM_FULL_BRAKE_AT_STOP) {
-				state = MC_STATE_OFF;
-				stop_pwm();
-			}
-			break;
-
 		default:
 			break;
 		}
@@ -1235,29 +1234,20 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
 		if (fabsf(dutycycle_now) <= MCPWM_MIN_DUTY_CYCLE) {
 			if (dutycycle_set_tmp > MCPWM_MIN_DUTY_CYCLE) {
 				dutycycle_now = MCPWM_MIN_DUTY_CYCLE + 0.01;
+#if MCPWM_IS_SENSORLESS
 				direction = 1;
 				set_open_loop();
+#endif
 			} else if (dutycycle_set_tmp < -MCPWM_MIN_DUTY_CYCLE) {
 				dutycycle_now = -MCPWM_MIN_DUTY_CYCLE - 0.01;
+#if MCPWM_IS_SENSORLESS
 				direction = 0;
 				set_open_loop();
+#endif
 			}
 		}
 
 		set_duty_cycle(dutycycle_now);
-
-		if (state == MC_STATE_RUNNING &&
-				fabsf(dutycycle_set) < MCPWM_MIN_DUTY_CYCLE &&
-				fabsf(dutycycle_now) <= MCPWM_MIN_DUTY_CYCLE + 0.03) {
-			state = MC_STATE_OFF;
-			dutycycle_now = 0.0;
-			dutycycle_set = 0.0;
-			if (MCPWM_FULL_BRAKE_AT_STOP) {
-				mcpwm_full_brake();
-			} else {
-				stop_pwm();
-			}
-		}
 	}
 
 	main_dma_adc_handler();
diff --git a/mcpwm.h b/mcpwm.h
index eb6efffe..25ce20d6 100644
--- a/mcpwm.h
+++ b/mcpwm.h
@@ -86,7 +86,7 @@ extern volatile int ADC_curr_norm_value[];
  */
 #define MCPWM_SWITCH_FREQUENCY			40000	// Switching frequency in HZ
 #define MCPWM_DEAD_TIME_CYCLES			80		// Dead time
-#define MCPWM_PWM_MODE					PWM_MODE_BIPOLAR // Default PWM mode
+#define MCPWM_PWM_MODE					PWM_MODE_SYNCHRONOUS // Default PWM mode
 #define MCPWM_MIN_DUTY_CYCLE			0.01	// Minimum duty cycle
 #define MCPWM_MAX_DUTY_CYCLE			0.95	// Maximum duty cycle
 #define MCPWM_AVG_COM_RPM				6		// Number of commutations to average RPM over
@@ -94,7 +94,7 @@ extern volatile int ADC_curr_norm_value[];
 #define MCPWM_HALL_SENSOR_ORDER			5		// Order in which hall sensors are connected
 #define MCPWM_RAMP_STEP					0.005	// Ramping step (1000 times/sec)
 #define MCPWM_CURRENT_MAX				30.0	// Current limit in Amperes
-#define MCPWM_CURRENT_MIN				-10.0	// Current limit in Amperes
+#define MCPWM_CURRENT_MIN				-30.0	// Current limit in Amperes
 #define MCPWM_IN_CURRENT_LIMIT			20.0	// Input current limit in Amperes
 #define MCPWM_FULL_BRAKE_AT_STOP		0		// Brake the motor when the power is set to stopped