diff --git a/darkwater/DW640.cpp b/darkwater/DW640.cpp
index 420de95..e4d84e8 100644
--- a/darkwater/DW640.cpp
+++ b/darkwater/DW640.cpp
@@ -232,8 +232,10 @@ void DW640::setMotorSpeed(uint8_t motor, int16_t speed) {
 	}
 	// Speed deciphering for the two control modes
 	if( speed >= 1000 && speed < 1500 ) {
+		runMotor( DW_REVERSE, in1, in2, map(speed, 1500, 1000, 0, 255 ) );
 		printf( "%d - %d\n", speed, map(speed, 1500, 1000, 0, 255 ) );
 		} else if( speed > 1500 && speed <= 2000 ) {	
+			runMotor( DW_FORWARD, in1, in2, map(speed, 1500, 2000, 0, 255 ) );
 			printf( "%d - %d\n", speed, map(speed, 1500, 2000, 0, 255 ) );
 			} else if( speed > 0 && speed <= 255 ) {
 				runMotor( DW_FORWARD, in1, in2, speed );