diff --git a/darkwater/DW640.cpp b/darkwater/DW640.cpp
index 4fd9f02..814dfcf 100644
--- a/darkwater/DW640.cpp
+++ b/darkwater/DW640.cpp
@@ -104,7 +104,6 @@ void DW640::setMode(uint8_t mode) {
  * @param Channel number (0-15)
  * @param Offset (0-4095)
  * @param Length (0-4095)
- * @see PCA9685_RA_LED0_ON_L
  */
 void DW640::setPWM(uint8_t channel, uint16_t offset, uint16_t length) {
 	this->pwm->setPWM( channel, offset, length );
@@ -113,7 +112,6 @@ void DW640::setPWM(uint8_t channel, uint16_t offset, uint16_t length) {
 /** Set channel's pulse length
  * @param Channel number (0-15)
  * @param Length (0-4095)
- * @see PCA9685_RA_LED0_ON_L
  */
 void DW640::setPWM(uint8_t channel, uint16_t length) {
     this->pwm->setPWM(channel, length);
@@ -122,7 +120,6 @@ void DW640::setPWM(uint8_t channel, uint16_t length) {
 /** Set channel's pulse length in milliseconds
  * @param Channel number (0-15)
  * @param Length in milliseconds
- * @see PCA9685_RA_LED0_ON_L
  */
 void DW640::setPWMmS(uint8_t channel, float length_mS) {
     this->pwm->setPWMmS(channel, length_mS);
@@ -131,7 +128,6 @@ void DW640::setPWMmS(uint8_t channel, float length_mS) {
 /** Set channel's pulse length in microseconds
  * @param Channel number (0-15)
  * @param Length in microseconds
- * @see PCA9685_RA_LED0_ON_L
  */
 void DW640::setPWMuS(uint8_t channel, float length_uS) {
     this->pwm->setPWMuS(channel, length_uS);
@@ -140,7 +136,6 @@ void DW640::setPWMuS(uint8_t channel, float length_uS) {
 /** Set start offset of the pulse and it's length for all channels
  * @param Offset (0-4095)
  * @param Length (0-4095)
- * @see PCA9685_RA_ALL_LED_ON_L
  */
 void DW640::setAllPWM(uint16_t offset, uint16_t length) {
 	this->pwm->setAllPWM(offset, length);
@@ -148,7 +143,6 @@ void DW640::setAllPWM(uint16_t offset, uint16_t length) {
 
 /** Set pulse length for all channels
  * @param Length (0-4095)
- * @see PCA9685_RA_ALL_LED_ON_L
  */
 void DW640::setAllPWM(uint16_t length) {
     this->pwm->setAllPWM(length);
@@ -156,7 +150,6 @@ void DW640::setAllPWM(uint16_t length) {
 
 /** Set pulse length in milliseconds for all channels
  * @param Length in milliseconds
- * @see PCA9685_RA_ALL_LED_ON_L
  */
 void DW640::setAllPWMmS(float length_mS) {
 	this->pwm->setAllPWMmS(length_mS);
@@ -164,7 +157,6 @@ void DW640::setAllPWMmS(float length_mS) {
 
 /** Set pulse length in microseconds for all channels
  * @param Length in microseconds
- * @see PCA9685_RA_ALL_LED_ON_L
  */
 void DW640::setAllPWMuS(float length_uS) {
 	this->pwm->setAllPWMuS(length_uS);
@@ -195,4 +187,95 @@ void DW640::setAllPin(uint8_t value) {
 		fprintf(stderr, "Pin value must be 0 or 1!");
 	}
 
+}
+
+void DW640::allOff() {
+	setAllPin( 0 );
+}
+
+/* DC Motor specific code */
+
+void DW640::setMotorSpeed(uint8_t motor, uint16_t speed) {
+
+	uint8_t in1;
+	uint8_t in2;
+
+	// Get the motor
+	switch(motor) {
+		case 1:
+				in2 = 2;
+				in1 = 3;
+				break;
+		case 2:
+				in2 = 4;
+				in1 = 5;
+				break;
+		case 3:
+				in2 = 6;
+				in1 = 7;
+				break;
+		case 4:
+				in2 = 8;
+				in1 = 9;
+				break;
+		case 5:
+				in2 = 10;
+				in1 = 11;
+				break;
+		case 6:
+				in2 = 12;
+				in1 = 13;
+				break;
+		default:
+				fprintf(stderr, "Motor number must be between 1 and 6 inclusive");
+	}
+	// Speed deciphering for the two control modes
+	if( speed >= 1000 && speed < 1500 ) {
+
+		} else if( speed > 1500 && speed <= 2000 ) {
+
+			} else if( speed > 0 && speed <= 255 ) {
+				runMotor( DW_FORWARD, in1, in2, speed );
+				} else if( speed < 0 && speed >= -255 ) {
+					runMotor( DW_REVERSE, in1, in2, abs(speed) );
+					} else if( speed == 0 || speed == 1500 ) {
+						runMotor( DW_STOP, in1, in2, speed );
+					}
+
+	
+
+}
+
+void DW640::setMotorOff(uint8_t motor) {
+
+}
+
+void DW640::runMotor( uint8_t control, uint8_t in1, uint8_t in2, uint16_t speed ) {
+	// get the mode
+	if( getMode() = DW_PHASE ) {
+		if( control == DW_FORWARD ) {
+				setPin( in2, 0 );
+				setPWM( in1, 0, speed * 16 );
+			} else if( control == DW_REVERSE ) {
+					setPin( in2, 1 );
+					setPWM( in1, 0, speed * 16 );
+				} else if( control == DW_STOP ) {
+					setPin( in2, 0 );
+					setPin( in1, 0 );
+				}
+	} else {	// DW_ININ
+		if( control == DW_FORWARD ) {
+			setPin( in2, 0 );
+			setPWM( in1, 0, speed * 16 );
+			} else if( control == DW_REVERSE ) {
+				setPin( in1, 0 );
+				setPWM( in2, 0, speed * 16 );
+				} else if( control == DW_STOP ) {
+					setPin( in1, 1 );
+					setPin( in2, 1 );
+					} else if( control == DW_COAST ) {
+						setPin( in1, 0 );
+						setPin( in2, 0 );
+					}
+	}
 }
\ No newline at end of file
diff --git a/darkwater/DW640.h b/darkwater/DW640.h
index ba009d3..a28ab90 100644
--- a/darkwater/DW640.h
+++ b/darkwater/DW640.h
@@ -85,10 +85,11 @@ class DW640 {
         void setPin(uint8_t channel, uint8_t value);
         void setAllPin(uint8_t value);
 
-        // void allOff(uint16_t value);
+        void allOff();
 
-        // void setMotorSpeed(uint8_t motor, uint16_t speed);
-        // void setMotorOff(uint8_t motor);
+        void setMotorSpeed(uint8_t motor, uint16_t speed);
+        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);
@@ -105,6 +106,8 @@ class DW640 {
         uint8_t mode;
         PCA9685* pwm;
         Pin* modePin;
+
+
 };
 
 #endif // DW640_H
\ No newline at end of file
diff --git a/examples/Motor/Motor.cpp b/examples/Motor/Motor.cpp
index b089a2b..442ec2c 100644
--- a/examples/Motor/Motor.cpp
+++ b/examples/Motor/Motor.cpp
@@ -27,6 +27,14 @@ int main()
 	DW640 dw;
 	dw.initialize();
 
+	dw.setMotorOff( 1 );
+	dw.setMotorSpeed( 1, 200 );
+	sleep(5);
+	dw.setMotorOff( 1 );
+	sleep(5);
+	dw.setMotorSpeed( 1, -200 );
+	sleep(5);
+	dw.setMotorOff( 1 );
 
 
 }