diff --git a/firmware/main.c b/firmware/main.c
index d0244a4..0670207 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -14,14 +14,15 @@ int main(void) {
 
 
 	while(1) {
-		for(uint8_t speed = 50; speed <= 255; speed++) {
+
+		for(uint8_t speed = 50; speed < 255; speed++) {
 			for(uint16_t i = 0; i < 1000; i++) {
 				pwm_cycle_forwards(speed);
 			}
 		}
 
 		for(uint16_t i = 0; i < 10000; i++) {
-			pwm_cycle_forwards(255);
+			pwm_cycle_forwards(254);
 		}
 
 		hbridge_unset_h2();
@@ -32,16 +33,15 @@ int main(void) {
 		hbridge_unset_l2();
 		hbridge_unset_l2();
 
-
-
-		for(uint8_t speed = 50; speed <= 255; speed++) {
+		for(uint8_t speed = 50; speed < 255; speed++) {
 			for(uint16_t i = 0; i < 1000; i++) {
 				pwm_cycle_backwards(speed);
 			}
 		}
 
+
 		for(uint16_t i = 0; i < 10000; i++) {
-			pwm_cycle_forwards(255);
+			pwm_cycle_backwards(254);
 		}
 
 		hbridge_unset_h2();
@@ -51,28 +51,6 @@ int main(void) {
 		_delay_ms(1000);
 		hbridge_unset_l2();
 		hbridge_unset_l2();
-
-		/*
-
-		for(uint16_t i = 0; i  < 5000; i++) {
-			hbridge_set_h2();
-			_delay_ms(0.9);
-			hbridge_unset_h2();
-			_delay_ms(0.1);
-		}
-
-		hbridge_set_l2();
-		_delay_ms(100);
-		hbridge_unset_l1();
-
-
-		for(uint16_t i = 0; i  < 5000; i++) {
-			hbridge_set_h1();
-			_delay_ms(0.9);
-			hbridge_unset_h1();
-			_delay_ms(0.1);
-		}
-*/
 	}
 
 
diff --git a/firmware/pwm.h b/firmware/pwm.h
index b2b4dc5..84735f2 100644
--- a/firmware/pwm.h
+++ b/firmware/pwm.h
@@ -41,29 +41,31 @@ static inline void hbridge_unset_l2(void) {
 	PORTB &= ~(1 << PB4);
 }
 
-const double delay_time = 1;
+const double delay_time = 0.25;
 
 static inline void pwm_cycle_backwards(uint8_t speed) {
-	PIND |= (1 << PD1);
+	if(speed > 0) {
+		hbridge_unset_h1();
+		hbridge_set_h2();
+		hbridge_set_l1();
+		hbridge_unset_l2();
 
-	for(uint8_t i = 0; i < 8; i++) {
-		if((speed & (1 << i)) != 0) {
-			hbridge_unset_h1();
-			hbridge_set_h2();
-			hbridge_set_l1();
-			hbridge_unset_l2();
-		}
-		else {
-			hbridge_unset_h1();
-			hbridge_unset_h2();
-			hbridge_unset_l1();
-			hbridge_set_l2();
-		}
-		for(uint8_t j = 0; j < (1 << i); j++) {
+		for(uint8_t i = 0; i < speed; i++) {
 			_delay_us(delay_time);
 		}
 	}
 
+
+	hbridge_unset_h1();
+	hbridge_unset_h2();
+	hbridge_unset_l1();
+	hbridge_set_l2();
+
+	// Always have one cycle l2 low to recharge the bootstrap cap
+	for(uint8_t i = 0; i < (255 - speed) + 1; i++) {
+		_delay_us(delay_time);
+	}
+
 	hbridge_unset_h1();
 	hbridge_unset_h2();
 	hbridge_unset_l1();
@@ -72,27 +74,28 @@ static inline void pwm_cycle_backwards(uint8_t speed) {
 
 
 static inline void pwm_cycle_forwards(uint8_t speed) {
-	PIND |= (1 << PD1);
+	if(speed > 0) {
+		hbridge_set_h1();
+		hbridge_unset_h2();
+		hbridge_unset_l1();
+		hbridge_set_l2();
 
-
-	for(uint8_t i = 0; i < 8; i++) {
-		if((speed & (1 << i)) != 0) {
-			hbridge_set_h1();
-			hbridge_unset_h2();
-			hbridge_unset_l1();
-			hbridge_set_l2();
-		}
-		else {
-			hbridge_unset_h1();
-			hbridge_unset_h2();
-			hbridge_set_l1();
-			hbridge_unset_l2();
-		}
-		for(uint8_t j = 0; j < (1 << i); j++) {
+		for(uint8_t i = 0; i < speed; i++) {
 			_delay_us(delay_time);
 		}
 	}
 
+
+	hbridge_unset_h1();
+	hbridge_unset_h2();
+	hbridge_set_l1();
+	hbridge_unset_l2();
+
+	// Always have one cycle l1 low to recharge the bootstrap cap
+	for(uint8_t i = 0; i < (255 - speed) + 1; i++) {
+		_delay_us(delay_time);
+	}
+
 	hbridge_unset_h1();
 	hbridge_unset_h2();
 	hbridge_unset_l1();