From bb5338d5d7afa5bfcaf5e863d67b0917100d7960 Mon Sep 17 00:00:00 2001
From: Ola Palm <ola.palm@me.com>
Date: Sat, 17 Jun 2017 21:48:33 +0200
Subject: [PATCH] adjustment to backward_until_inside function.. Added
 slopreadnings defininition angle will be read this number of times, to get an
 avarage.

---
 Liam/Controller.cpp |  7 ++-----
 Liam/Definition.h   | 12 ++++++++----
 Liam/Liam.ino       |  2 --
 Liam/MMA_7455.cpp   |  4 ++--
 4 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/Liam/Controller.cpp b/Liam/Controller.cpp
index b24171e..e6ceadb 100644
--- a/Liam/Controller.cpp
+++ b/Liam/Controller.cpp
@@ -141,15 +141,12 @@ int CONTROLLER::GoBackwardUntilInside (BWFSENSOR *Sensor) {
   int counter = MAX_GO_BACKWARD_TIME;
   // Check if tiltAngle is greater then slopeangle, if not return directly.
   int angle = 0;
-  int timesToCheck = 10;
-  for (int i = 0; i < timesToCheck; i++)
+  for (int i = 0; i < SLOPEREADINGS; i++)
   {
     angle += compass->getTiltAngle();
-    
     delay(100);
   }
-  Serial.print("\nbackwards active. angle is :");Serial.print(angle/10); 
-  if (abs(angle / timesToCheck) <= SLOPEANGLE)
+  if (abs(angle / SLOPEREADINGS) <= SLOPEANGLE)
     return 0;
   
   while (Sensor->isInside() == false)
diff --git a/Liam/Definition.h b/Liam/Definition.h
index a4a076f..c42ba07 100644
--- a/Liam/Definition.h
+++ b/Liam/Definition.h
@@ -42,7 +42,7 @@
    You can also see some values reported by the sensors. You need probably to tweek some
    of the parameters in this file and when you are done remove or comment out this line to
    run your mover in real mode. */
-#define __SETUP_AND_DEBUG_MODE__ true
+#define __SETUP_AND_DEBUG_MODE__ false
 
 /******************************************************************
   User specific settings depends on how you have built your mover.
@@ -54,7 +54,7 @@
                     BRUSHLESS  0     (for all hobbyking motors with external ESC)
                     BRUSHED:   1     (for all brushed motors, A3620 and NIDEC 22)
                     NIDEC      2     (for NIDEC 24 or NIDEC 22 connected to morgan shield without any modifications) */
-const int MY_CUTTERMOTOR = 2;
+const int MY_CUTTERMOTOR = 1;
 
 /* Configure which type of battery you have.
    Types availible:
@@ -121,8 +121,12 @@ const int NUMBER_OF_SENSORS = 2;
    If mower is not inside within x seconds mower will stop. */
 #define GO_BACKWARD_UNTIL_INSIDE true
 #define MAX_GO_BACKWARD_TIME 5
-/* Slopeangle. Mower will not go backwards if slope is less then slopeangle.*/
-#define SLOPEANGLE 20
+// The amount of times to check and sum angle, sum will be divided by this value to get avrage angle of slopereadings number of time read.
+#define SLOPEREADINGS 1
+/* Slopeangle. Mower will not go backwards if slope is less then slopeangle.
+if you have no angle-sensor and still want mower to go backwards until it's inside, then set SLOPEANGLE to -1
+*/
+#define SLOPEANGLE -1
 
 /******************************************************************
   Common settings that should be same for the most of us.
diff --git a/Liam/Liam.ino b/Liam/Liam.ino
index 4540e88..7fa8fa9 100644
--- a/Liam/Liam.ino
+++ b/Liam/Liam.ino
@@ -295,8 +295,6 @@ void loop() {
 
 #if GO_BACKWARD_UNTIL_INSIDE
         err = Mower.GoBackwardUntilInside(&Sensor);
-        if (err == 0)
-          return;
 #endif
 
         // Tries to turn, but if timeout then reverse and try again
diff --git a/Liam/MMA_7455.cpp b/Liam/MMA_7455.cpp
index fb3217a..279575b 100644
--- a/Liam/MMA_7455.cpp
+++ b/Liam/MMA_7455.cpp
@@ -53,7 +53,7 @@ void MMA_7455::autoupdate()
   int16_t xc = 0, yc = 0, zc = 0;
   float lpratio = 0.2; /* between 0 and 1 */
   /* Set serial baud rate */
-  //Serial.begin(115200);
+  Serial.begin(115200);
   /* Set accelerometer sensibility to 2g */
   setSensitivity(2);
   /* Verify sensibility - optional */
@@ -106,7 +106,7 @@ void MMA_7455::autoupdate()
 }
 boolean MMA_7455::initialize() {
   Wire.begin();
-  //Serial.begin(115200);
+  Serial.begin(115200);
   setSensitivity(2);
   setMode(measure);
   setAxisOffset(X_ANGLE_NORMAL, Y_ANGLE_NORMAL, Z_ANGLE_NORMAL);