C++: Examples: Servo: add Navio+ support

Makefile: add Navio+ libraries Servo: rewrite file with RCOutput interface
IgorAnohin · Nov 2, 2017 · bbf1212 · bbf1212
1 parent a6185ad
commit bbf1212
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 22 deletions.
diff --git a/C++/Examples/Servo/Makefile b/C++/Examples/Servo/Makefile
@@ -1,9 +1,9 @@
 CXX ?= g++
 NAVIO = ../../Navio
-INCLUDES = -I ../..
+INCLUDES = -I ../../Navio
 
 all:
-	$(CXX) -std=gnu++11 $(INCLUDES) Servo.cpp $(NAVIO)/PWM.cpp $(NAVIO)/Util.cpp -o Servo
+	$(CXX) -std=gnu++11 $(INCLUDES) Servo.cpp $(NAVIO)/Navio2/PWM.cpp $(NAVIO)/Navio+/PCA9685.cpp $(NAVIO)/Common/I2Cdev.cpp $(NAVIO)/Common/gpio.cpp $(NAVIO)/Common/Util.cpp $(NAVIO)/Navio+/RCOutput_Navio.cpp $(NAVIO)/Navio2/RCOutput_Navio2.cpp -o Servo
 
 clean:
 	rm Servo
diff --git a/C++/Examples/Servo/Servo.cpp b/C++/Examples/Servo/Servo.cpp
@@ -14,35 +14,54 @@ sudo ./Servo
 */
 
 #include <unistd.h>
-#include "Navio/PWM.h"
-#include "Navio/Util.h"
+#include "Navio2/PWM.h"
+#include "Navio+/RCOutput_Navio.h"
+#include "Navio2/RCOutput_Navio2.h"
+#include "Common/Util.h"
+#include <memory>
+
+#define SERVO_MIN 1250 /*mS*/
+#define SERVO_MAX 1750 /*mS*/
 
 #define PWM_OUTPUT 0
-#define SERVO_MIN 1.250 /*mS*/
-#define SERVO_MAX 1.750 /*mS*/
+
+
+using namespace Navio;
+
+std::unique_ptr <RCOutput> get_rcout()
+{
+    if (get_navio_version() == NAVIO2)
+    {
+        auto ptr = std::unique_ptr <RCOutput>{ new RCOutput_Navio2() };
+        return ptr;
+    } else
+    {
+        auto ptr = std::unique_ptr <RCOutput>{ new RCOutput_Navio() };
+        return ptr;
+    }
+
+}
 
 int main()
 {
-    PWM pwm;
 
-    if (check_apm()) {
-        return 1;
-    }
+        auto pwm = get_rcout();
 
-    if (!pwm.init(PWM_OUTPUT)) {
-        fprintf(stderr, "Output Enable not set. Are you root?\n");
-        return 0;
-    }
+        if (check_apm()) {
+            return 1;
+        }
 
-    pwm.enable(PWM_OUTPUT);
-    pwm.set_period(PWM_OUTPUT, 50);
+        if( !(pwm->initialize(PWM_OUTPUT)) ) {
+            return 1;
+        }
+        pwm->set_frequency(PWM_OUTPUT, 50);
 
-    while (true) {
-        pwm.set_duty_cycle(PWM_OUTPUT, SERVO_MIN);
-        sleep(1);
-        pwm.set_duty_cycle(PWM_OUTPUT, SERVO_MAX);
-        sleep(1);
-    }
+        while (true) {
+            pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MIN);
+            sleep(1);
+            pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MAX);
+            sleep(1);
+        }
 
     return 0;
 }