From f3b37be7dbf2783c3f55f6e551c94180b852709c Mon Sep 17 00:00:00 2001
From: shrkey <barry@shrkey.com>
Date: Mon, 10 Oct 2016 20:28:29 +0100
Subject: [PATCH] Initial servo code

---
 darkwater/DW640.cpp | 51 ++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 48 insertions(+), 3 deletions(-)

diff --git a/darkwater/DW640.cpp b/darkwater/DW640.cpp
index e4d84e8..15c975a 100644
--- a/darkwater/DW640.cpp
+++ b/darkwater/DW640.cpp
@@ -233,10 +233,8 @@ 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 );
 				} else if( speed < 0 && speed >= -255 ) {
@@ -250,7 +248,7 @@ void DW640::setMotorSpeed(uint8_t motor, int16_t speed) {
 }
 
 void DW640::setMotorOff(uint8_t motor) {
-
+	setMotorSpeed( motor, 0 );
 }
 
 void DW640::runMotor( uint8_t control, uint8_t in1, uint8_t in2, uint16_t speed ) {
@@ -283,6 +281,53 @@ void DW640::runMotor( uint8_t control, uint8_t in1, uint8_t in2, uint16_t speed
 	}
 }
 
+/* Servo code */
+
+void setServoOff(uint8_t servo) {
+
+	switch(servo) {
+		case 1:
+				setPin( 0, 0 ); // Servo 1 is on PWM 0
+				break;
+		case 2:
+				setPin( 1, 0 ); // Servo 2 is on PWM 1
+				break;
+		default:
+				fprintf(stderr, "Servo number must be between 1 and 2 inclusive");
+	}
+
+}
+    
+void setServoPWMmS(uint8_t servo, float length_mS) {
+
+	switch(servo) {
+		case 1:
+				setPWMmS( 0, length_mS ); // Servo 1 is on PWM 0
+				break;
+		case 2:
+				setPWMmS( 0, length_mS ); // Servo 2 is on PWM 1
+				break;
+		default:
+				fprintf(stderr, "Servo number must be between 1 and 2 inclusive");
+	}
+
+}
+
+void setServoPWMuS(uint8_t servo, float length_uS) {
+
+	switch(servo) {
+		case 1:
+				setPWMuS( 0, length_uS ); // Servo 1 is on PWM 0
+				break;
+		case 2:
+				setPWMuS( 1, length_uS ); // Servo 2 is on PWM 1
+				break;
+		default:
+				fprintf(stderr, "Servo number must be between 1 and 2 inclusive");
+	}
+
+}
+
 /* Private functions */
 
 uint16_t DW640::map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max)