From d21a52d6b21fbf70e77fff3ccf5e2da48f900f12 Mon Sep 17 00:00:00 2001
From: Lukas Bachschwell <lukas@lbsfilm.at>
Date: Tue, 12 Dec 2017 21:19:45 +0100
Subject: [PATCH] Cleanup comments and more

---
 src/LARS.cpp | 45 ++++++++++++++-------------------------------
 src/LARS.h   |  3 ++-
 2 files changed, 16 insertions(+), 32 deletions(-)

diff --git a/src/LARS.cpp b/src/LARS.cpp
index 6037357..05a189e 100644
--- a/src/LARS.cpp
+++ b/src/LARS.cpp
@@ -16,40 +16,23 @@
 */
 
 
-LARS::LARS(int FRH, int FLH, int BRH, int BLH, int FRL, int FLL, int BRL, int BLL): reverse{0, 0, 0, 0, 0, 0, 0, 0}, trim{0, 0, 0, 0, 0, 0, 0, 0} {
-  board_pins[FRONT_RIGHT_HIP] = FRH; // front right inner
-	board_pins[FRONT_LEFT_HIP] = FLH; // front left inner
-	board_pins[BACK_RIGHT_HIP] = BRH; // back right inner
-	board_pins[BACK_LEFT_HIP] = BLH; // back left inner
-	board_pins[FRONT_RIGHT_LEG] = FRL; // front right outer
-	board_pins[FRONT_LEFT_LEG] = FLL; //  front left outer       // POSITIONS LOOKING FROM THE MIDDLE OF THE ROBOT!!!!!
-	board_pins[BACK_RIGHT_LEG] = BRL; // back right outer
-	board_pins[BACK_LEFT_LEG] = BLL; // back left outer
+LARS::LARS(): reverse{0, 0, 0, 0, 0, 0, 0, 0}, trim{0, 0, 0, 0, 0, 0, 0, 0} {
 }
 
-void LARS::init() {
-  /*
-     trim[] for calibrating servo deviation,
-     initial posture (home) should like below
-     in symmetric
-        \       /
-         \_____/
-         |     |
-         |_____|
-         /     \
-        /       \
-  */
-  /*
-    trim[FRONT_LEFT_HIP] = 0;
-    trim[FRONT_RIGHT_HIP] = -8;
-    trim[BACK_LEFT_HIP] = 8;
-    trim[BACK_RIGHT_HIP] = 5;
+void LARS::init(){
+  init(26, 25, 17, 16, 27, 5, 23, 13); // Calling init with default values
+}
 
-    trim[FRONT_LEFT_LEG] = 2;
-    trim[FRONT_RIGHT_LEG] = -6;
-    trim[BACK_LEFT_LEG] = 6;
-    trim[BACK_RIGHT_LEG] = 5;
-  */
+void LARS::init(int FRH, int FLH, int BRH, int BLH, int FRL, int FLL, int BRL, int BLL) {
+
+  board_pins[FRONT_RIGHT_HIP] = FRH; // front right inner
+  board_pins[FRONT_LEFT_HIP] = FLH; // front left inner
+  board_pins[BACK_RIGHT_HIP] = BRH; // back right inner
+  board_pins[BACK_LEFT_HIP] = BLH; // back left inner
+  board_pins[FRONT_RIGHT_LEG] = FRL; // front right outer
+  board_pins[FRONT_LEFT_LEG] = FLL; //  front left outer       // POSITIONS LOOKING FROM THE MIDDLE OF THE ROBOT!!!!!
+  board_pins[BACK_RIGHT_LEG] = BRL; // back right outer
+  board_pins[BACK_LEFT_LEG] = BLL; // back left outer
 
   for (int i = 0; i < 8; i++) {
     oscillator[i].start();
diff --git a/src/LARS.h b/src/LARS.h
index 8a9d772..bb89fd7 100644
--- a/src/LARS.h
+++ b/src/LARS.h
@@ -24,8 +24,9 @@
 class LARS {
 
   public:
-    LARS(int FRH = 26, int FLH = 25, int BRH = 17, int BLH = 16, int FRL = 27, int FLL = 5, int BRL = 23, int BLL = 13);
+    LARS();
     void init();
+    void init(int FRH = 26, int FLH = 25, int BRH = 17, int BLH = 16, int FRL = 27, int FLL = 5, int BRL = 23, int BLL = 13);
     void walk(int dir = 1, float steps = 1, float T = 800); // T initial 400
     void omniWalk(float steps, float T, bool side, float turn_factor);
     void turnL(float steps, float period);