From d1044b7aadc6e07d0b8cd7cedaa4c5dc11c30fe4 Mon Sep 17 00:00:00 2001
From: shrkey <barry@shrkey.com>
Date: Mon, 10 Oct 2016 20:59:15 +0100
Subject: [PATCH] servo test

---
 examples/Servo/Makefile  |  2 +-
 examples/Servo/Servo.cpp | 46 ++++++++--------------------------------
 2 files changed, 10 insertions(+), 38 deletions(-)

diff --git a/examples/Servo/Makefile b/examples/Servo/Makefile
index 93ce50a..1b6dcb3 100644
--- a/examples/Servo/Makefile
+++ b/examples/Servo/Makefile
@@ -3,7 +3,7 @@ DW = ../../darkwater
 INCLUDES = -I ../..
 
 all:
-	$(CC) $(INCLUDES) Servo.cpp $(DW)/PCA9685.cpp $(DW)/I2Cdev.cpp $(DW)/gpio.cpp $(DW)/Util.cpp -o Servo
+	$(CC) $(INCLUDES) Servo.cpp $(DW)/DW640.cpp $(DW)/PCA9685.cpp $(DW)/I2Cdev.cpp $(DW)/gpio.cpp $(DW)/Util.cpp -o Servo
 
 clean:
 	rm Servo
diff --git a/examples/Servo/Servo.cpp b/examples/Servo/Servo.cpp
index 247f60f..27e5960 100644
--- a/examples/Servo/Servo.cpp
+++ b/examples/Servo/Servo.cpp
@@ -1,56 +1,28 @@
-/*
-Provided to you by Emlid Ltd (c) 2014.
-twitter.com/emlidtech || www.emlid.com || info@emlid.com
-
-Example: Control servos connected to PCA9685 driver onboard of Navio shield for Raspberry Pi.
-
-Connect servo to Navio's rc output and watch it work.
-Output 1 on board is connected to PCA9685 channel 3, output 2 to channel 4 and so on.
-To use full range of your servo correct SERVO_MIN and SERVO_MAX according to it's specification.
-
-To run this example navigate to the directory containing it and run following commands:
-make
-./Servo
-*/
-
-#define NAVIO_RCOUTPUT_1 3
 #define SERVO_MIN 1.250 /*mS*/
 #define SERVO_MAX 1.750 /*mS*/
 
-#include <darkwater/gpio.h>
-#include "darkwater/PCA9685.h"
+#include "darkwater/DW640.h"
 #include "darkwater/Util.h"
+#include <stdlib.h>
 
 using namespace DarkWater;
 
 int main()
 {
-    static const uint8_t outputEnablePin = RPI_GPIO_27;
     
     if (check_apm()) {
         return 1;
     }
 
-    Pin pin(outputEnablePin);
-
-    if (pin.init()) {
-        pin.setMode(Pin::GpioModeOutput);
-        pin.write(0); /* drive Output Enable low */
-    } else {
-        fprintf(stderr, "Output Enable not set. Are you root?\n");
-        return 1;
-    }
-
-    PCA9685 pwm;
-
-    pwm.initialize();
-    pwm.setFrequency(50);
+    DW640 dw;
+    dw.initialize();
+    dw.setFrequency(50);
 
     while (true) {
-        pwm.setPWMmS(NAVIO_RCOUTPUT_1, SERVO_MIN);
-        sleep(1);
-        pwm.setPWMmS(NAVIO_RCOUTPUT_1, SERVO_MAX);
-        sleep(1);
+        dw.setServoPWMmS(1, SERVO_MIN);
+        usleep(1000000);
+        dw.setServoPWMmS(1, SERVO_MAX);
+        usleep(1000000);
     }
 
     return 0;