From caa9fc4584b612e8f6e3c49dded307550a2d9bc1 Mon Sep 17 00:00:00 2001
From: roboticafacil <roboticafacil.es@gmail.com>
Date: Wed, 18 Apr 2018 18:59:53 +0200
Subject: [PATCH] 	modified:   src/Hippie.cpp 	modified:  
 src/Hippie.h

---
 src/Hippie.cpp | 22 ++++++++++++++++++++--
 src/Hippie.h   |  2 +-
 2 files changed, 21 insertions(+), 3 deletions(-)

diff --git a/src/Hippie.cpp b/src/Hippie.cpp
index 3cdbddf..71f912c 100644
--- a/src/Hippie.cpp
+++ b/src/Hippie.cpp
@@ -4,14 +4,31 @@
 #include <Oscillator.h>
 
 
-
-void Hippie::init(int YL, int YR, int RL, int RR) {
+void Hippie::init(int YL, int YR, int RL, int RR, int OYL, int OYR, int ORL, int ORR) {
 
   servo_pins[0] = YL;
   servo_pins[1] = YR;
   servo_pins[2] = RL;
   servo_pins[3] = RR;
+  
+  attachServos();
+  isHippieResting=false;
 
+
+  for (int i = 0; i < 4; i++) servo_position[i] = 90;
+}
+
+void Hippie::init_with_trim(int YL, int YR, int RL, int RR, int OYL, int OYR, int ORL, int ORR) {
+
+  servo_pins[0] = YL;
+  servo_pins[1] = YR;
+  servo_pins[2] = RL;
+  servo_pins[3] = RR;
+  servo[0].SetTrim(OYL);
+  servo[1].SetTrim(OYR);
+  servo[2].SetTrim(ORL);
+  servo[3].SetTrim(ORR);
+  
   attachServos();
   isHippieResting=false;
 
@@ -71,6 +88,7 @@ void Hippie::oscillateServos(int A[4], int O[4], int T, double phase_diff[4], fl
     servo[i].SetA(A[i]);
     servo[i].SetT(T);
     servo[i].SetPh(phase_diff[i]);
+	servo[i].SetTrim(servo_trim[i]);
   }
   double ref=millis();
    for (double x=ref; x<=T*cycle+ref; x=millis()){
diff --git a/src/Hippie.h b/src/Hippie.h
index 65485a6..5bcf3ad 100644
--- a/src/Hippie.h
+++ b/src/Hippie.h
@@ -19,7 +19,7 @@ class Hippie
 
     //-- Hippie initialisation
     void init(int YL, int YR, int RL, int RR);
-
+	void init_with_trim(int YL, int YR, int RL, int RR, int OYL, int OYR, int ORL, int ORR);
     //-- Attach & detach functions
     void attachServos();
     void detachServos();