oscillate implementation

La fonction oscillate permet de resonner en terme de pas et non de cycles pour faire osciller les servos...
ESTIASYSTEM · Oct 22, 2017 · 13d4b4d · 13d4b4d
1 parent 02d952c
commit 13d4b4d
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 2 deletions.
diff --git a/Kubot.cpp b/Kubot.cpp
@@ -167,6 +167,27 @@ void Kubot::oscillateServos(int A[4], int O[4], int T, double Ph[4], float cycle
   }
 }
 
+ void Kubot::Oscillate(int A[4], int O[4], int T, double phase_diff[4], float steps)
+ {
+   attachServos();
+   setRestState(false);
+
+   float cycles = trunc(steps);
+
+   if(steps >= 1)
+   {
+
+     for(int i(0) ; i < cycles; ++i)
+      oscillateServos(A,O,T,phase_diff);
+   }
+
+   oscillateServos(A,O,T,phase_diff,steps-cycles);
+
+ }
+
+
+
+
 
 ////////////////////////////////////////////////
 /// Homing: Controle de la position de repos ///

diff --git a/Kubot.hpp b/Kubot.hpp
@@ -48,8 +48,8 @@ class Kubot
 
     //-- Contrôle basique des servos
     void moveServos(int duration, int  servo_target[]);
-    void oscillateServos(int A[4], int O[4], int T, double phase_diff[4], float cycle);
-
+    void oscillateServos(int A[4], int O[4], int T, double phase_diff[4], float cycle = 1.0);
+    void  oscillate(int A[4], int O[4], int T, double phase_diff[4], float steps = 1.0);
     //-- HOME = Kubot à sa position de repos
     void home();
     bool getRestState();