diff --git a/darkwater/DW640.h b/darkwater/DW640.h
index 54c2ae1..0570b3e 100644
--- a/darkwater/DW640.h
+++ b/darkwater/DW640.h
@@ -91,9 +91,9 @@ class DW640 {
         void setMotorOff(uint8_t motor);
         void runMotor( uint8_t control, uint8_t in1, uint8_t in2, uint16_t speed );
 
-        // void setServoOff(uint8_t servo);
-        // void setServoPWMmS(uint8_t servo, float length_mS);
-        // void setServoPWMuS(uint8_t servo, float length_uS);
+        void setServoOff(uint8_t servo);
+        void setServoPWMmS(uint8_t servo, float length_mS);
+        void setServoPWMuS(uint8_t servo, float length_uS);
 
         // void setStepperOff(uint8_t stepper);
         // void setStepperSpeed(uint8_t stepper, uint16_t speed);
diff --git a/examples/Motor/Motor.cpp b/examples/Motor/Motor.cpp
index f354b44..5de6375 100644
--- a/examples/Motor/Motor.cpp
+++ b/examples/Motor/Motor.cpp
@@ -27,28 +27,77 @@ int main()
 	DW640 dw;
 	dw.initialize();
 
-	dw.setMotorSpeed( 1, 1500 );
-	printf("forward\n");
-	dw.setMotorSpeed( 1, 2000 );
-	usleep(2000000);
-	dw.setMotorSpeed( 1, 1950 );
-	usleep(2000000);
-	dw.setMotorSpeed( 1, 1900 );
-	usleep(2000000);
-	dw.setMotorSpeed( 1, 1800 );
-	usleep(2000000);
-	dw.setMotorSpeed( 1, 1700 );
-	usleep(2000000);
-	dw.setMotorSpeed( 1, 1600 );
-	usleep(2000000);
-	printf("stop\n");
-	dw.setMotorSpeed( 1, 1500 );
-	usleep(5000000);
-	printf("reverse\n");
-	dw.setMotorSpeed( 1, 1250 );
-	usleep(5000000);
-	printf("stop\n");
-	dw.setMotorSpeed( 1, 1500 );
+	dw.setMotorOff(1);
+	dw.setMotorOff(2);
+	dw.setMotorOff(3);
+	dw.setMotorOff(4);
+	dw.setMotorOff(5);
+	dw.setMotorOff(6);
+	usleep(1000000);
 
+	printf("Set forward - \n");
+	printf("Motor 1\n");
+	dw.setMotorSpeed( 1, 255 );
+	usleep(1000000);
+	printf("Motor 2\n");
+	dw.setMotorSpeed( 2, 255 );
+	usleep(1000000);
+	printf("Motor 3\n");
+	dw.setMotorSpeed( 3, 255 );
+	usleep(1000000);
+	printf("Motor 4\n");
+	dw.setMotorSpeed( 4, 255 );
+	usleep(1000000);
+	printf("Motor 5\n");
+	dw.setMotorSpeed( 5, 255 );
+	usleep(1000000);
+	printf("Motor 6\n");
+	dw.setMotorSpeed( 6, 255 );
+	usleep(1000000);
+	printf("Stopping - \n");
+	printf("Motor 1\n");
+	dw.setMotorSpeed( 1, 0 );
+	usleep(1000000);
+	printf("Motor 2\n");
+	dw.setMotorSpeed( 2, 0 );
+	usleep(1000000);
+	printf("Motor 3\n");
+	dw.setMotorSpeed( 3, 0 );
+	usleep(1000000);
+	printf("Motor 4\n");
+	dw.setMotorSpeed( 4, 0 );
+	usleep(1000000);
+	printf("Motor 5\n");
+	dw.setMotorSpeed( 5, 0 );
+	usleep(1000000);
+	printf("Motor 6\n");
+	dw.setMotorSpeed( 6, 0 );
+	usleep(1000000);
+	printf("Set reverse - \n");
+	printf("Motor 1\n");
+	dw.setMotorSpeed( 1, -255 );
+	usleep(1000000);
+	printf("Motor 2\n");
+	dw.setMotorSpeed( 2, -255 );
+	usleep(1000000);
+	printf("Motor 3\n");
+	dw.setMotorSpeed( 3, -255 );
+	usleep(1000000);
+	printf("Motor 4\n");
+	dw.setMotorSpeed( 4, -255 );
+	usleep(1000000);
+	printf("Motor 5\n");
+	dw.setMotorSpeed( 5, -255 );
+	usleep(1000000);
+	printf("Motor 6\n");
+	dw.setMotorSpeed( 6, -255 );
+	usleep(1000000);
+	printf("All off \n");
+	dw.setMotorOff(1);
+	dw.setMotorOff(2);
+	dw.setMotorOff(3);
+	dw.setMotorOff(4);
+	dw.setMotorOff(5);
+	dw.setMotorOff(6);
 
 }