From f562e15975c70e5767d0a15b869823c30ec0eaff Mon Sep 17 00:00:00 2001
From: basicByte0x0C <>
Date: Sat, 14 Sep 2024 00:10:45 +0300
Subject: [PATCH 1/9] Autopilot Phase: Added IR Obstacle Sensor; Detection
 works

---
 EcoBot.ino | 48 +++++++++++++++++++++++++++++++++++++++++-------
 Notes.h    |  8 +++++---
 2 files changed, 46 insertions(+), 10 deletions(-)

diff --git a/EcoBot.ino b/EcoBot.ino
index d368db0..8c6740a 100644
--- a/EcoBot.ino
+++ b/EcoBot.ino
@@ -14,7 +14,7 @@
 #define DELAY_1_SECOND  1000
 #define DELAY_DEFAULT   DELAY_1_SECOND
 #define SERIAL_BRATE    115200
-static byte devStuff = E_OK;
+static byte devStuff = E_NOT_OK;
 /* General Stuff end */
 
 /* Motor Stuff */
@@ -45,8 +45,9 @@ static byte exploreState = EXPLORE_AUTOMATE;
 /* Exploration Stuff end */
 
 /* IR Stuff */
-#define PIN_IR_RECEIVER_POWER   A0    /* Power the IR Receiver with this pin */
-#define PIN_IR_RECEIVER_DATA    9     /* Read data from IR Receiver with this pin */
+#define PIN_IR_RECEIVER_POWER   A0      /* Power the IR Receiver with this pin */
+#define PIN_IR_OBSTACLE_DATA    8       /* Read data from IR Obstacle Detector  with this pin */
+#define PIN_IR_RECEIVER_DATA    9       /* Read data from IR Receiver with this pin */
 
 #define IR_VALUE_FORWARD        0xFF18E7    /* Move Backward */
 #define IR_VALUE_BACKWARD       0xFF4AB5    /* Move Forward */
@@ -276,6 +277,41 @@ void HandleIR(void)
     }
 }
 
+/***************************************************************************************
+ * Function: Robot_Autopilot()
+ ***************************************************************************************
+ * Description: This function will use Robot intelligence for driving around.
+ **************************************************************************************/
+void Robot_Autopilot(void)
+{
+    /* Read Distance Sensors and decide if movement is possible */
+    /* - IR Sensor detects on LOW read */
+    if(LOW == digitalRead(PIN_IR_OBSTACLE_DATA))
+    {
+        /* Obstacle Detected Ahead */
+        /* Stop */
+        Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+
+        /* Look around */
+        /* Look Left */
+        /* Look Right */
+
+        /* Decide which way to go */
+
+        /* Some Dev Stuff */
+        if(E_OK == devStuff)
+        {
+            Serial.println("I see");
+        }
+    }
+    else
+    {
+        /* No obstacle ahead */
+        /* Walk forward */
+        Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
+    }
+}
+
 /***************************************************************************************
  * Function: Robot_Explore()
  ***************************************************************************************
@@ -321,9 +357,7 @@ void Robot_Explore(void)
         }
 
         /* Do Autonomous things */
-        /* Check for Obstacles */
-        /* Decide next Direction */
-        /* Move */
+        Robot_Autopilot();
     }
     else
     {
@@ -402,7 +436,7 @@ void Robot_WakeUp(void)
 /***************************************************************************************
  * Function: Robot_Sleep()
  ***************************************************************************************
- * Description: This function checks the Energy Consumption and decide what to do.
+ * Description: This function tries to make the Robot Sleep.
  * Parameters:
  *  - sleepTime[in]   :   Number of seconds to go to sleep
  *                              Supported Inputs: 
diff --git a/Notes.h b/Notes.h
index a654081..8c873a6 100644
--- a/Notes.h
+++ b/Notes.h
@@ -28,13 +28,13 @@
  *  D5  --- Used by Motor B Enable
  *  D6  --- Used by Motor B Phase
  *  D7  --- Used by DRV8834 Sleep Pin
- *  D8  --- 
- *  D9  --- Used to read IR Receiver
+ *  D8  --- Used by IR Obstacle Data
+ *  D9  --- Used by IR Receiver Data
  *  D10 --- Reserved for SPI SS(CS) (possible E-Paper?)
  *  D11 --- Reserved for SPI MOSI   (an E-Paper would be nice)
  *  D12 --- Reserved for SPI MISO   (like, you can draw emotions)
  *  D13 --- Reserved for SPI SCK    (on the E-Paper)
- *  A0  --- Used to power IR Receiver
+ *  A0  --- Used to power IR Modules
  *  A1  ---
  *  A2  ---
  *  A3  --- Used to read Battery Level
@@ -95,6 +95,8 @@
 /* TODO: Don't be blind
  * - An IR Distance Sensor will be used to detect objects ahead.
  * - If an object is detected, the robot will rotate around and try to find another path with no obstacles.
+ * - IR Output is LOW when there is an obstacle detected, else is HIGH.
+ * - Will be powered from the same pin as IR Remote.
  */
 
 /* TODO: Laser Eyes

From f4606da3d7ceef639d50bfafdb3892f774fc4d06 Mon Sep 17 00:00:00 2001
From: basicByte0x0C <>
Date: Sat, 14 Sep 2024 03:25:23 +0300
Subject: [PATCH 2/9] Autopilot Phase: Robot is working as expected

---
 EcoBot.ino | 147 +++++++++++++++++++++++++++++++++++++++++++++++------
 Notes.h    |  32 ++++++++----
 2 files changed, 155 insertions(+), 24 deletions(-)

diff --git a/EcoBot.ino b/EcoBot.ino
index 8c6740a..538773a 100644
--- a/EcoBot.ino
+++ b/EcoBot.ino
@@ -14,7 +14,7 @@
 #define DELAY_1_SECOND  1000
 #define DELAY_DEFAULT   DELAY_1_SECOND
 #define SERIAL_BRATE    115200
-static byte devStuff = E_NOT_OK;
+static byte devStuff = E_OK;
 /* General Stuff end */
 
 /* Motor Stuff */
@@ -29,19 +29,19 @@ static byte devStuff = E_NOT_OK;
 #define DRV8834_MOTOR_BOTH          3u
 #define DRV8834_DIRECTION_BACKWARD  0u
 #define DRV8834_DIRECTION_FORWARD   1u
-#define DRV8834_POWER_FULL          255u
-#define DRV8834_POWER_HALF          127u
+#define DRV8834_POWER_FULL          255u    /* Doesn't work with less than max, lower than 3V is not enough for motors or PWM is sketchy */
 #define DRV8834_POWER_NONE          0u
-#define DRV8834_WAKEUP_WAIT         1     /* Miliseconds until DRV8834 should be fully working after wakeup */
+#define DRV8834_WAKEUP_WAIT         1       /* Miliseconds until DRV8834 should be fully working after wakeup */
 #define DRV8834_WALK_TIME           100
-#define DRV8834_BREAK_TIMEOUT       115   /* Lower than this and the timeout is too short */
+#define DRV8834_BREAK_TIMEOUT       115     /* Lower than this and the timeout is too short */
+#define MOTOR_TURN_TIME_FACTOR      5       /* Multiply by degree to turn around. Shoulkd be time for 1 degree */
 /* Motor Stuff end */
 
 /* Exploration Stuff */
 #define EXPLORE_AUTOMATE  0u    /* Autonomous driving */
 #define EXPLORE_MANUAL    1u    /* Manual driving from IR */
 
-static byte exploreState = EXPLORE_AUTOMATE;
+static byte exploreState = EXPLORE_MANUAL;
 /* Exploration Stuff end */
 
 /* IR Stuff */
@@ -64,7 +64,7 @@ decode_results results;
 #define PIN_INSOMNIA          	    2       /* Used for development purpose to keep the Robot awake */
 #define ADC_MAX_VALUE               1023.0
 #define ADC_MAX_VOLTAGE             3.3
-#define BATTERY_SLEEP_THRESHOLD     3.3     /* Voltage drops by 0.05 V when motors are working */
+#define BATTERY_SLEEP_THRESHOLD     2.0     /* Voltage drops by 0.05 V when motors are working */
 #define ROBOT_SLEEP_1_SECOND        1
 #define ROBOT_SLEEP_10_SECONDS      10
 #define ROBOT_SLEEP_1_MINUTE        60
@@ -162,7 +162,7 @@ void Motor_SwitchDirection(byte motorIdentifier, byte motorDirection)
 /***************************************************************************************
  * Function: Motor_EnableMotor()
  ***************************************************************************************
- * Description: This function will switch the direction of a specified motor
+ * Description: This function will power a specified motor
  * Parameters:
  *  - motorIdentifier[in]   :   Identifier of the motor to be switched forward
  *                              Supported Inputs: 
@@ -200,6 +200,50 @@ void Motor_EnableMotor(byte motorIdentifier, byte motorPower)
     }
 }
 
+/***************************************************************************************
+ * Function: Motor_RotateRight()
+ ***************************************************************************************
+ * Description: This function will use motors to rotate right.
+ * Parameters:
+ *  - degrees[in]   :   Number of degrees to rotate right
+ **************************************************************************************/
+void Motor_RotateRight(uint16_t degrees)
+{
+    /* Stop Walking */
+    Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+
+    /* Rotate around */
+    Motor_SwitchDirection(DRV8834_MOTOR_A, DRV8834_DIRECTION_FORWARD);
+    Motor_SwitchDirection(DRV8834_MOTOR_B, DRV8834_DIRECTION_BACKWARD);
+    Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
+    delay(degrees * MOTOR_TURN_TIME_FACTOR);
+
+    /* Stop Turning around */
+    Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+}
+
+/***************************************************************************************
+ * Function: Motor_RotateLeft()
+ ***************************************************************************************
+ * Description: This function will use motors to rotate left.
+ * Parameters:
+ *  - degrees[in]   :   Number of degrees to rotate left
+ **************************************************************************************/
+void Motor_RotateLeft(uint16_t degrees)
+{
+    /* Stop Walking */
+    Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+
+    /* Rotate around */
+    Motor_SwitchDirection(DRV8834_MOTOR_A, DRV8834_DIRECTION_BACKWARD);
+    Motor_SwitchDirection(DRV8834_MOTOR_B, DRV8834_DIRECTION_FORWARD);
+    Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
+    delay(degrees * MOTOR_TURN_TIME_FACTOR);
+
+    /* Stop Turning around */
+    Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+}
+
 /***************************************************************************************
  * Function: Motor_TestMotor()
  ***************************************************************************************
@@ -217,21 +261,21 @@ void Motor_TestMotor(byte motorIdentifier)
   /* To test the motor functionality, the motor shall run in each direction
    * and also the switching of the direction shall work. */
 
-  static byte direction = 1u;
+  static byte direction = DRV8834_DIRECTION_FORWARD;
 
   /* Run the motor for 1s */
-  Motor_EnableMotor(motorIdentifier, 255u);
+  Motor_EnableMotor(motorIdentifier, DRV8834_POWER_FULL);
   digitalWrite(LED_BUILTIN, HIGH);
   delay(DELAY_1_SECOND);
 
   /* Stop the motor for 1s */
-  Motor_EnableMotor(motorIdentifier, 0u);
+  Motor_EnableMotor(motorIdentifier, DRV8834_POWER_NONE);
   digitalWrite(LED_BUILTIN, LOW);
   delay(DELAY_1_SECOND);
 
   /* Switch Motor Direction */
-  Motor_SwitchDirection(motorIdentifier, !direction);
   direction = !direction;
+  Motor_SwitchDirection(motorIdentifier, direction);
 }
 
 /***************************************************************************************
@@ -241,6 +285,12 @@ void Motor_TestMotor(byte motorIdentifier)
  **************************************************************************************/
 void HandleIR(void)
 {
+    /* Dev Stuff */
+    if(E_OK == devStuff)
+    {
+        Serial.print("IR: ");
+        Serial.println(results.value, HEX);
+    }
     /* Robot reaction based on the IR value */
     switch(results.value)
     {
@@ -277,6 +327,27 @@ void HandleIR(void)
     }
 }
 
+/***************************************************************************************
+ * Function: Robot_DetectAhead()
+ ***************************************************************************************
+ * Description: Read sensors so we know if the road is blocked.
+ **************************************************************************************/
+byte Robot_DetectAhead(void)
+{
+    /* Return if there is any obstacle ahead */
+    if(LOW == digitalRead(PIN_IR_OBSTACLE_DATA))
+    {
+        /* Obstacle ahead */
+        return E_NOT_OK;
+    }
+    else
+    {
+        /* No obstacle ahead */
+    }
+
+    return E_OK;
+}
+
 /***************************************************************************************
  * Function: Robot_Autopilot()
  ***************************************************************************************
@@ -284,19 +355,64 @@ void HandleIR(void)
  **************************************************************************************/
 void Robot_Autopilot(void)
 {
+    /* Each bit represents one way: Left, ahead(or back) and Right <-- MSB to LSM */
+    byte whereToGo = 3u;
+    uint16_t randomDegrees = 180;
+
     /* Read Distance Sensors and decide if movement is possible */
     /* - IR Sensor detects on LOW read */
-    if(LOW == digitalRead(PIN_IR_OBSTACLE_DATA))
+    if(E_NOT_OK == Robot_DetectAhead())
     {
         /* Obstacle Detected Ahead */
         /* Stop */
         Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+        delay(DELAY_1_SECOND);
 
         /* Look around */
         /* Look Left */
+        Motor_RotateLeft(45);
+        Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+        delay(DELAY_1_SECOND);
+        if(E_NOT_OK == Robot_DetectAhead())
+        {
+            /* Remove this way */
+            whereToGo -= 1u;
+        }
+
         /* Look Right */
+        Motor_RotateRight(90);
+        delay(DELAY_1_SECOND);
+        if(E_NOT_OK == Robot_DetectAhead())
+        {
+            /* Remove this way as well */
+            whereToGo -= 2u;
+        }
+
+        /* Return to original position */
+        Motor_RotateLeft(45);
+        delay(DELAY_1_SECOND);
 
         /* Decide which way to go */
+        switch(whereToGo)
+        {
+            case 0u:
+                /* Only way is to turn around */
+                /* Turn around random degrees */
+                randomDegrees = (uint16_t)(millis() % 360);
+                Motor_RotateLeft(randomDegrees);
+                break;
+            case 1u:
+                /* I prefeer to try to go to Right first, don't know why */
+                Motor_RotateRight(45);
+                break;
+            case 2u:
+                /* Otherwise i go Left */
+                Motor_RotateLeft(45);
+                break;
+            default:
+                /* If i panic i do nothing */
+                break;
+        }
 
         /* Some Dev Stuff */
         if(E_OK == devStuff)
@@ -308,6 +424,7 @@ void Robot_Autopilot(void)
     {
         /* No obstacle ahead */
         /* Walk forward */
+        Motor_SwitchDirection(DRV8834_MOTOR_BOTH, DRV8834_DIRECTION_FORWARD);
         Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
     }
 }
@@ -598,7 +715,7 @@ void loop(void)
     if(E_OK == devStuff)
     {
         /* Dev Stuff */
-        Robot_Testing();
+        //Robot_Testing();
     }
     else
     {
@@ -606,7 +723,7 @@ void loop(void)
     }
 
     /* Battery Management */
-    Robot_PowerManagement();
+    //Robot_PowerManagement();
 
     /* Movement Control */
     Robot_Explore();
diff --git a/Notes.h b/Notes.h
index 8c873a6..e7f8073 100644
--- a/Notes.h
+++ b/Notes.h
@@ -64,6 +64,7 @@
 /* ----- Energy Management -----
  * - Voltage measured with Voltage Divider. R1 == R2. Resistence used: 120 Ohm probably 1W. 2W would be better.
  * - Pin Read Voltage drops by 0.05 V when Motors are running. Take in consideration for threshold. 
+ * - TODO: Decide if to desolder IR Obstacle LEDs. There are 2 bright leds, so it must consume some power.
  * -- Measurements ::
  *  == No Modifications; no Loop Code ==
  *      - Startup : jumps to ~5mA, fluctuates to 4 and 6 a little bit, then it jumps to Idle current of 7.03mA
@@ -72,9 +73,24 @@
  *      - Only Motor Driver Asleep : 4.51 mA => Motor Driver in Idle consume aprox 2.5 mA
  *      - Only IR Receiver Asleep : 6.68 mA => IR Receiver in Idle consume aprox 0.4 mA
  *      - Robot Sleeping : 1.58 mA
- *  == Battery Voltage Reader
- *      - Pin Read : 3.97 V == Multimeter Reading : 4.01 V => Accuracy up to 0.04 V
- *      - Pin Read : 3.95 V == Multimeter Reading : 3.99 V => Accuracy up to 0.04 V
+ *  == Battery Voltage Reader==
+ *      - Pin Read : 3.97 V == Multimeter Reading : 4.01 V => Accuracy up to 0.04 V (R1 == R2 == 120 Ohm)
+ *      - Pin Read : 3.95 V == Multimeter Reading : 3.99 V => Accuracy up to 0.04 V (R1 == R2 == 120 Ohm)
+ *      - Pin Read : 3.60 V == Multimeter Reading : 3.88 V => Accuracy up to 0.28 V (R1 == R2 == 10 kOhm)
+ *      - Pin Read : 3.87 V == Multimeter Reading : 3.89 V => Accuracy up to 0.02 V (R1 == R2 == 2 kOhm)
+ *      - Pin Read : 3.86 V == Multimeter Reading : 3.88-3.89 V => Accuracy up to 0.03 V (R1 == R2 == 5 kOhm)
+ *      - Pin Read : 3.86 V == Multimeter Reading : 3.99 V => Accuracy up to 0.03V (R1 == R2 == 10 kOhm)
+ *  == IR Obstacle Working; Battery Management Checks ==
+ *      - IR No Obstacle : 40.2 mA
+ *      - IR Obstacle : 40.9 mA
+ *      - Jumps to 20.2 for 1 second, then when IR starts working it jumps to 40,6 mA
+ *      - This increase in battery consumption seems to be caused by Voltage Divider. This is not ok.
+ *      - Robot Sleeping : 17.3 mA. Inacceptible! Need Higher Resistence!
+ *      - Changed Resistences from 120 Ohm to 10 kOhm => Sleep at 1.7 mA. Seems to work fine.
+ *      - Unknown Resistence Wattage. Still doesn't heat when Idle/Sleep.
+ *      - Robot Sleeping : 2.4 mA with R = 2 kOhm. Try with 5 kOhm.
+ *      - Robot Sleeping : 1.8 mA with R = 5 kOhm. Best is 10 kOhm but let's see also accuracy.
+ *      - Robot Running : 0.19A, Spike to 0.21 or slightly more
  *      
  */
 
@@ -92,7 +108,7 @@
  * - Consume aprox 0.4mA when Idle, according to some measurements.
  */
 
-/* TODO: Don't be blind
+/* ----- Don't be blind -----
  * - An IR Distance Sensor will be used to detect objects ahead.
  * - If an object is detected, the robot will rotate around and try to find another path with no obstacles.
  * - IR Output is LOW when there is an obstacle detected, else is HIGH.
@@ -119,13 +135,11 @@
  * - Each Motor has a ENABLE Pin and a PHASE Pin (xENBL and xPHASE).
  * - xPHASE is controlling the direction, while xENBL controlls the motor.
  * - xENBL can be used as PWM to controll the motor.
+ * - PWM doesn't work probably because motors need the 3.something voltage to work.
+ * - TODO: Rotate Left and Right are not calibrated.
  */
 
-/* TODO: RotateRight(degree) and RotateLeft(degree) functions
- * - Which will move the motors accordingly so the robot can rotate x degrees to the right or left .
- */
-
-/* ----- Seel like a baby -----
+/* ----- Sleep like a baby -----
  * - Robot will sleep while charging and when the battery level is low.
  * - Motor Driver put into sleep mode by setting the SLEEP Pin to LOW.
  * - Every other unessential peripheral is powered down.

From 556815a017e0931777cf408581d218189f185eab Mon Sep 17 00:00:00 2001
From: basicByte0x0C <>
Date: Sat, 14 Sep 2024 04:56:46 +0300
Subject: [PATCH 3/9] Autopilot Phase: Small Improvements and bugfixes

---
 EcoBot.ino | 98 ++++++++++++++++++++++++++++++++++++------------------
 Notes.h    |  6 ++--
 2 files changed, 70 insertions(+), 34 deletions(-)

diff --git a/EcoBot.ino b/EcoBot.ino
index 538773a..12ef4b0 100644
--- a/EcoBot.ino
+++ b/EcoBot.ino
@@ -14,7 +14,7 @@
 #define DELAY_1_SECOND  1000
 #define DELAY_DEFAULT   DELAY_1_SECOND
 #define SERIAL_BRATE    115200
-static byte devStuff = E_OK;
+static byte devStuff = E_NOT_OK;
 /* General Stuff end */
 
 /* Motor Stuff */
@@ -348,6 +348,48 @@ byte Robot_DetectAhead(void)
     return E_OK;
 }
 
+/***************************************************************************************
+ * Function: Robot_LookAround()
+ ***************************************************************************************
+ * Description: Return a value which correspond to obstacles ahead.
+ * return       0u      Obstacles everywhere: Front, Lelft, Right
+ *              1u      Left is free
+ *              2u      Right is free
+ *              3u      Left and Right are both free
+ **************************************************************************************/
+byte Robot_LookAround(void)
+{
+    /* Return Left and Right Readings */
+    byte retVal = 3u;
+
+    /* Look Left */
+    Motor_RotateLeft(45);
+    Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+    delay(DELAY_DEFAULT);
+    if(E_NOT_OK == Robot_DetectAhead())
+    {
+        /* Remove this way */
+        retVal -= 1u;
+    }
+
+    /* Center back */
+    Motor_RotateRight(45);
+    delay(DELAY_DEFAULT);
+
+    /* Look Right */
+    Motor_RotateRight(45);
+    delay(DELAY_DEFAULT);
+    if(E_NOT_OK == Robot_DetectAhead())
+    {
+        /* Remove this way as well */
+        retVal -= 2u;
+    }
+
+    /* Return to original position */
+    Motor_RotateLeft(45);
+    delay(DELAY_DEFAULT);
+
+}
 /***************************************************************************************
  * Function: Robot_Autopilot()
  ***************************************************************************************
@@ -356,7 +398,7 @@ byte Robot_DetectAhead(void)
 void Robot_Autopilot(void)
 {
     /* Each bit represents one way: Left, ahead(or back) and Right <-- MSB to LSM */
-    byte whereToGo = 3u;
+    byte whereToGo = 2u;
     uint16_t randomDegrees = 180;
 
     /* Read Distance Sensors and decide if movement is possible */
@@ -366,48 +408,40 @@ void Robot_Autopilot(void)
         /* Obstacle Detected Ahead */
         /* Stop */
         Motor_BreakMotor(DRV8834_MOTOR_BOTH);
-        delay(DELAY_1_SECOND);
+        delay(DELAY_DEFAULT);
 
-        /* Look around */
-        /* Look Left */
-        Motor_RotateLeft(45);
-        Motor_BreakMotor(DRV8834_MOTOR_BOTH);
-        delay(DELAY_1_SECOND);
-        if(E_NOT_OK == Robot_DetectAhead())
-        {
-            /* Remove this way */
-            whereToGo -= 1u;
-        }
-
-        /* Look Right */
-        Motor_RotateRight(90);
-        delay(DELAY_1_SECOND);
-        if(E_NOT_OK == Robot_DetectAhead())
-        {
-            /* Remove this way as well */
-            whereToGo -= 2u;
-        }
+        /* Go a step Back */
+        Motor_SwitchDirection(DRV8834_MOTOR_BOTH, DRV8834_DIRECTION_BACKWARD);
+        Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
+        delay(100);
 
-        /* Return to original position */
-        Motor_RotateLeft(45);
-        delay(DELAY_1_SECOND);
+        /* Look around */
+        //whereToGo = Robot_LookAround();
 
         /* Decide which way to go */
+        /* Sensor used is not ok, go random for now */
+        whereToGo = 0u;
         switch(whereToGo)
         {
+            case 3u: /* Only obstacle ahead; fallthrough to obstacles everywhere */
             case 0u:
                 /* Only way is to turn around */
                 /* Turn around random degrees */
-                randomDegrees = (uint16_t)(millis() % 360);
+                randomSeed(millis());
+                randomDegrees = (uint16_t)random(360);
                 Motor_RotateLeft(randomDegrees);
+                delay(DELAY_DEFAULT);
                 break;
             case 1u:
-                /* I prefeer to try to go to Right first, don't know why */
+                /* Obstacle in Right side */
                 Motor_RotateRight(45);
+                delay(DELAY_DEFAULT);
                 break;
+            
             case 2u:
-                /* Otherwise i go Left */
-                Motor_RotateLeft(45);
+                /* Obstacle in Left side */
+                Motor_RotateRight(45);
+                delay(DELAY_DEFAULT);
                 break;
             default:
                 /* If i panic i do nothing */
@@ -679,7 +713,7 @@ void Robot_Testing(void)
     float batteryVoltage = (batteryLevel * (ADC_MAX_VOLTAGE / ADC_MAX_VALUE)) * 2;
 
     /* Show battery level on Serial */
-    Serial.println(batteryVoltage);
+    //Serial.println(batteryVoltage);
 }
 
 /***************************************************************************************
@@ -715,7 +749,7 @@ void loop(void)
     if(E_OK == devStuff)
     {
         /* Dev Stuff */
-        //Robot_Testing();
+        Robot_Testing();
     }
     else
     {
@@ -723,7 +757,7 @@ void loop(void)
     }
 
     /* Battery Management */
-    //Robot_PowerManagement();
+    Robot_PowerManagement();
 
     /* Movement Control */
     Robot_Explore();
diff --git a/Notes.h b/Notes.h
index e7f8073..bfa31a4 100644
--- a/Notes.h
+++ b/Notes.h
@@ -79,7 +79,7 @@
  *      - Pin Read : 3.60 V == Multimeter Reading : 3.88 V => Accuracy up to 0.28 V (R1 == R2 == 10 kOhm)
  *      - Pin Read : 3.87 V == Multimeter Reading : 3.89 V => Accuracy up to 0.02 V (R1 == R2 == 2 kOhm)
  *      - Pin Read : 3.86 V == Multimeter Reading : 3.88-3.89 V => Accuracy up to 0.03 V (R1 == R2 == 5 kOhm)
- *      - Pin Read : 3.86 V == Multimeter Reading : 3.99 V => Accuracy up to 0.03V (R1 == R2 == 10 kOhm)
+ *      - Pin Read : 3.86 V == Multimeter Reading : 3.89 V => Accuracy up to 0.03V (R1 == R2 == 10 kOhm)
  *  == IR Obstacle Working; Battery Management Checks ==
  *      - IR No Obstacle : 40.2 mA
  *      - IR Obstacle : 40.9 mA
@@ -113,6 +113,8 @@
  * - If an object is detected, the robot will rotate around and try to find another path with no obstacles.
  * - IR Output is LOW when there is an obstacle detected, else is HIGH.
  * - Will be powered from the same pin as IR Remote.
+ * - Very bad distance readings with this IR Distance Sensor. Maybe change the sensor with a better one.
+ * - If the configured threshold is too high the robot will see objects everywhere. At the limit it is reading 2cm ahead...
  */
 
 /* TODO: Laser Eyes
@@ -136,7 +138,7 @@
  * - xPHASE is controlling the direction, while xENBL controlls the motor.
  * - xENBL can be used as PWM to controll the motor.
  * - PWM doesn't work probably because motors need the 3.something voltage to work.
- * - TODO: Rotate Left and Right are not calibrated.
+ * - TODO: Rotate Left and Right are not calibrated. Just a little bit calibrated.
  */
 
 /* ----- Sleep like a baby -----

From 80a51d36ec369768cd8c17bc03ee4360b343d62f Mon Sep 17 00:00:00 2001
From: basicByte0x0C <>
Date: Sat, 14 Sep 2024 13:14:12 +0300
Subject: [PATCH 4/9] Autopilot Phase: Add Sleep Command to Manual IR Control

---
 EcoBot.ino | 239 ++++++++++++++++++++++++++++-------------------------
 1 file changed, 125 insertions(+), 114 deletions(-)

diff --git a/EcoBot.ino b/EcoBot.ino
index 12ef4b0..004aca0 100644
--- a/EcoBot.ino
+++ b/EcoBot.ino
@@ -14,7 +14,7 @@
 #define DELAY_1_SECOND  1000
 #define DELAY_DEFAULT   DELAY_1_SECOND
 #define SERIAL_BRATE    115200
-static byte devStuff = E_NOT_OK;
+static byte devStuff = E_OK;
 /* General Stuff end */
 
 /* Motor Stuff */
@@ -54,6 +54,7 @@ static byte exploreState = EXPLORE_MANUAL;
 #define IR_VALUE_LEFT           0xFF10EF    /* Rotate Left */
 #define IR_VALUE_RIGHT          0xFF5AA5    /* Rotate Right */
 #define IR_VALUE_MODE           0xFF38C7    /* Switch between Manual and Automate Exploring */
+#define IR_VALUE_SLEEP          0xFF6897    /* Go to Sleep for a while */
 
 IRrecv irrecv(PIN_IR_RECEIVER_DATA);
 decode_results results;
@@ -67,7 +68,10 @@ decode_results results;
 #define BATTERY_SLEEP_THRESHOLD     2.0     /* Voltage drops by 0.05 V when motors are working */
 #define ROBOT_SLEEP_1_SECOND        1
 #define ROBOT_SLEEP_10_SECONDS      10
+#define ROBOT_SLEEP_30_SECONDS      30
 #define ROBOT_SLEEP_1_MINUTE        60
+#define ROBOT_SLEEP_10_MINUTES      600
+#define ROBOT_SLEEP_30_MINUTES      1800
 #define ROBOT_SLEEP_TIME_DEFAULT    ROBOT_SLEEP_1_SECOND
 /* Power Management Stuff end */
 
@@ -278,6 +282,121 @@ void Motor_TestMotor(byte motorIdentifier)
   Motor_SwitchDirection(motorIdentifier, direction);
 }
 
+
+/***************************************************************************************
+ * Function: Robot_WakeUp()
+ ***************************************************************************************
+ * Description: This function is executed when the robot wakes up. It will initialize
+ *              everything and will make the robot functional.
+ **************************************************************************************/
+void Robot_WakeUp(void)
+{
+    /* Initialize things */
+    /* PIN Modes */
+    pinMode(LED_BUILTIN, OUTPUT);   /* Debug purposes */
+    pinMode(PIN_MA_ENABLE, OUTPUT);
+    pinMode(PIN_MA_PHASE, OUTPUT);
+    pinMode(PIN_MB_ENABLE, OUTPUT);
+    pinMode(PIN_MB_PHASE, OUTPUT);
+    pinMode(PIN_DRV8834_SLEEP, OUTPUT);
+    pinMode(PIN_IR_RECEIVER_POWER, OUTPUT);
+    pinMode(PIN_IR_RECEIVER_DATA, INPUT);
+
+    /* Wakeup the Motor Driver */
+    digitalWrite(PIN_DRV8834_SLEEP, HIGH);
+    delay(DRV8834_WAKEUP_WAIT);
+
+    /* Set default direction of both Motors to move Forward */
+    Motor_SwitchDirection(DRV8834_MOTOR_BOTH, DRV8834_DIRECTION_FORWARD);
+
+    /* Power on the IR Receiver */
+    digitalWrite(PIN_IR_RECEIVER_POWER, HIGH);
+
+    /* Enable IR Receiver */
+    irrecv.enableIRIn();
+
+    /* Dev Stuff */
+    if(E_OK == devStuff)
+    {
+        /* New Day! */
+        Serial.println("Good Morning!");
+    }
+}
+
+/***************************************************************************************
+ * Function: Robot_Sleep()
+ ***************************************************************************************
+ * Description: This function tries to make the Robot Sleep.
+ * Parameters:
+ *  - sleepTime[in]   :   Number of seconds to go to sleep
+ *                              Supported Inputs: 
+ *                                  0u - 3600u
+ **************************************************************************************/
+void Robot_Sleep(uint16_t sleepTime)
+{
+    /* Check if Robot is able to sleep */
+    if(HIGH == digitalRead(PIN_INSOMNIA))
+    {
+        /* Insomnia is here, can't sleep */
+    }
+    else
+    {
+        /* Insomnia is not around, sleep */
+        /* Set wakeup conditions */
+        /* GPIO External Interrupt Wakeup */
+        attachInterrupt(PIN_INSOMNIA, Robot_WakeUp, HIGH);
+
+        /* Ensure All Used Pins are configured to output, then output nothing */
+        /* Exception for Insomnia to wake it up and Battery Level as it will be always on */
+        pinMode(PIN_IR_RECEIVER_POWER, OUTPUT);
+        digitalWrite(PIN_IR_RECEIVER_POWER, LOW);   /* Disable IR Receiver */
+        pinMode(PIN_IR_RECEIVER_DATA, OUTPUT);
+        digitalWrite(PIN_IR_RECEIVER_DATA, LOW);
+        pinMode(PIN_DRV8834_SLEEP, OUTPUT);
+        digitalWrite(PIN_DRV8834_SLEEP, LOW);   /* Send Motor Driver to sleep */
+        pinMode(PIN_MA_ENABLE, OUTPUT);
+        digitalWrite(PIN_MA_ENABLE, LOW);
+        pinMode(PIN_MA_PHASE, OUTPUT);
+        digitalWrite(PIN_MA_PHASE, LOW);
+        pinMode(PIN_MB_ENABLE, OUTPUT);
+        digitalWrite(PIN_MB_ENABLE, LOW);
+        pinMode(PIN_MB_PHASE, OUTPUT);
+        digitalWrite(PIN_MB_PHASE, LOW);
+        pinMode(LED_BUILTIN, OUTPUT);
+        digitalWrite(LED_BUILTIN, LOW);        
+
+        /* Dev Stuff */
+        if(E_OK == devStuff)
+        {
+            /* Say Good night */
+            Serial.println("Good night!");
+
+            delay(100);
+        }
+
+        /* Go to sleep */
+        if(sleepTime & 1u)
+        {
+            LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF);
+        }
+        if(sleepTime & 2u)
+        {
+            LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF);
+        }
+        if(sleepTime & 4u)
+        {
+            LowPower.powerDown(SLEEP_4S, ADC_OFF, BOD_OFF);
+        }
+
+        while(sleepTime & 0xFFF8u)
+        {
+            sleepTime -= 8;
+            LowPower.powerDown(SLEEP_8S, ADC_OFF, BOD_OFF);
+        }
+    }
+}
+
+
 /***************************************************************************************
  * Function: HandleIR()
  ***************************************************************************************
@@ -321,6 +440,11 @@ void HandleIR(void)
             exploreState = !exploreState;
             Motor_BreakMotor(DRV8834_MOTOR_BOTH);
             break;
+        case IR_VALUE_SLEEP:
+            /* Sleep for a while */
+            Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+            Robot_Sleep(ROBOT_SLEEP_1_MINUTE);
+            break;
         default:
             /* Do nothing */
             break;
@@ -544,119 +668,6 @@ void Robot_Explore(void)
     }
 }
 
-/***************************************************************************************
- * Function: Robot_WakeUp()
- ***************************************************************************************
- * Description: This function is executed when the robot wakes up. It will initialize
- *              everything and will make the robot functional.
- **************************************************************************************/
-void Robot_WakeUp(void)
-{
-    /* Initialize things */
-    /* PIN Modes */
-    pinMode(LED_BUILTIN, OUTPUT);   /* Debug purposes */
-    pinMode(PIN_MA_ENABLE, OUTPUT);
-    pinMode(PIN_MA_PHASE, OUTPUT);
-    pinMode(PIN_MB_ENABLE, OUTPUT);
-    pinMode(PIN_MB_PHASE, OUTPUT);
-    pinMode(PIN_DRV8834_SLEEP, OUTPUT);
-    pinMode(PIN_IR_RECEIVER_POWER, OUTPUT);
-    pinMode(PIN_IR_RECEIVER_DATA, INPUT);
-
-    /* Wakeup the Motor Driver */
-    digitalWrite(PIN_DRV8834_SLEEP, HIGH);
-    delay(DRV8834_WAKEUP_WAIT);
-
-    /* Set default direction of both Motors to move Forward */
-    Motor_SwitchDirection(DRV8834_MOTOR_BOTH, DRV8834_DIRECTION_FORWARD);
-
-    /* Power on the IR Receiver */
-    digitalWrite(PIN_IR_RECEIVER_POWER, HIGH);
-
-    /* Enable IR Receiver */
-    irrecv.enableIRIn();
-
-    /* Dev Stuff */
-    if(E_OK == devStuff)
-    {
-        /* New Day! */
-        Serial.println("Good Morning!");
-    }
-}
-
-/***************************************************************************************
- * Function: Robot_Sleep()
- ***************************************************************************************
- * Description: This function tries to make the Robot Sleep.
- * Parameters:
- *  - sleepTime[in]   :   Number of seconds to go to sleep
- *                              Supported Inputs: 
- *                                  0u - 3600u
- **************************************************************************************/
-void Robot_Sleep(uint16_t sleepTime)
-{
-    /* Check if Robot is able to sleep */
-    if(HIGH == digitalRead(PIN_INSOMNIA))
-    {
-        /* Insomnia is here, can't sleep */
-    }
-    else
-    {
-        /* Insomnia is not around, sleep */
-        /* Set wakeup conditions */
-        /* GPIO External Interrupt Wakeup */
-        attachInterrupt(PIN_INSOMNIA, Robot_WakeUp, HIGH);
-
-        /* Ensure All Used Pins are configured to output, then output nothing */
-        /* Exception for Insomnia to wake it up and Battery Level as it will be always on */
-        pinMode(PIN_IR_RECEIVER_POWER, OUTPUT);
-        digitalWrite(PIN_IR_RECEIVER_POWER, LOW);   /* Disable IR Receiver */
-        pinMode(PIN_IR_RECEIVER_DATA, OUTPUT);
-        digitalWrite(PIN_IR_RECEIVER_DATA, LOW);
-        pinMode(PIN_DRV8834_SLEEP, OUTPUT);
-        digitalWrite(PIN_DRV8834_SLEEP, LOW);   /* Send Motor Driver to sleep */
-        pinMode(PIN_MA_ENABLE, OUTPUT);
-        digitalWrite(PIN_MA_ENABLE, LOW);
-        pinMode(PIN_MA_PHASE, OUTPUT);
-        digitalWrite(PIN_MA_PHASE, LOW);
-        pinMode(PIN_MB_ENABLE, OUTPUT);
-        digitalWrite(PIN_MB_ENABLE, LOW);
-        pinMode(PIN_MB_PHASE, OUTPUT);
-        digitalWrite(PIN_MB_PHASE, LOW);
-        pinMode(LED_BUILTIN, OUTPUT);
-        digitalWrite(LED_BUILTIN, LOW);        
-
-        /* Dev Stuff */
-        if(E_OK == devStuff)
-        {
-            /* Say Good night */
-            Serial.println("Good night!");
-
-            delay(100);
-        }
-
-        /* Go to sleep */
-        if(sleepTime & 1u)
-        {
-            LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF);
-        }
-        if(sleepTime & 2u)
-        {
-            LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF);
-        }
-        if(sleepTime & 4u)
-        {
-            LowPower.powerDown(SLEEP_4S, ADC_OFF, BOD_OFF);
-        }
-
-        while(sleepTime & 0xFFF8u)
-        {
-            sleepTime -= 8;
-            LowPower.powerDown(SLEEP_8S, ADC_OFF, BOD_OFF);
-        }
-    }
-}
-
 /***************************************************************************************
  * Function: Robot_PowerManagement()
  ***************************************************************************************

From 20d56b1635e4572aa255cbcbeae11267572a4021 Mon Sep 17 00:00:00 2001
From: basicByte0x0C <>
Date: Sat, 14 Sep 2024 13:18:52 +0300
Subject: [PATCH 5/9] Autopilot Phase: Bugfix for not waking up after IR Sleep

---
 EcoBot.ino | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/EcoBot.ino b/EcoBot.ino
index 004aca0..e63076d 100644
--- a/EcoBot.ino
+++ b/EcoBot.ino
@@ -396,7 +396,6 @@ void Robot_Sleep(uint16_t sleepTime)
     }
 }
 
-
 /***************************************************************************************
  * Function: HandleIR()
  ***************************************************************************************
@@ -444,6 +443,7 @@ void HandleIR(void)
             /* Sleep for a while */
             Motor_BreakMotor(DRV8834_MOTOR_BOTH);
             Robot_Sleep(ROBOT_SLEEP_1_MINUTE);
+            Robot_WakeUp();
             break;
         default:
             /* Do nothing */
@@ -673,7 +673,7 @@ void Robot_Explore(void)
  ***************************************************************************************
  * Description: This function checks the Energy Consumption and decide what to do.
  **************************************************************************************/
-void Robot_PowerManagement()
+void Robot_PowerManagement(void)
 {
     volatile uint16_t batteryLevel;
     volatile float batteryVoltage;

From 5bc4adef5df81eef21ecb5bba1623ce73529b950 Mon Sep 17 00:00:00 2001
From: basicByte0x0C <>
Date: Sat, 14 Sep 2024 15:15:51 +0300
Subject: [PATCH 6/9] Autopilot Phase: Bugfixing detection behavior

---
 EcoBot.ino | 111 ++++++++++++++++++++++++++++++++++++++++-------------
 Notes.h    |   5 +++
 2 files changed, 90 insertions(+), 26 deletions(-)

diff --git a/EcoBot.ino b/EcoBot.ino
index e63076d..a0979df 100644
--- a/EcoBot.ino
+++ b/EcoBot.ino
@@ -34,7 +34,7 @@ static byte devStuff = E_OK;
 #define DRV8834_WAKEUP_WAIT         1       /* Miliseconds until DRV8834 should be fully working after wakeup */
 #define DRV8834_WALK_TIME           100
 #define DRV8834_BREAK_TIMEOUT       115     /* Lower than this and the timeout is too short */
-#define MOTOR_TURN_TIME_FACTOR      5       /* Multiply by degree to turn around. Shoulkd be time for 1 degree */
+#define MOTOR_TURN_TIME_FACTOR      4       /* Multiply by degree to turn around. Shoulkd be time for 1 degree */
 /* Motor Stuff end */
 
 /* Exploration Stuff */
@@ -219,11 +219,15 @@ void Motor_RotateRight(uint16_t degrees)
     /* Rotate around */
     Motor_SwitchDirection(DRV8834_MOTOR_A, DRV8834_DIRECTION_FORWARD);
     Motor_SwitchDirection(DRV8834_MOTOR_B, DRV8834_DIRECTION_BACKWARD);
-    Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
-    delay(degrees * MOTOR_TURN_TIME_FACTOR);
 
-    /* Stop Turning around */
-    Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+    /* Try to rotate slowly */
+    do
+    {
+        Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
+        delay(MOTOR_TURN_TIME_FACTOR);
+        Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+        degrees--;
+    }while(0 < degrees);
 }
 
 /***************************************************************************************
@@ -241,11 +245,15 @@ void Motor_RotateLeft(uint16_t degrees)
     /* Rotate around */
     Motor_SwitchDirection(DRV8834_MOTOR_A, DRV8834_DIRECTION_BACKWARD);
     Motor_SwitchDirection(DRV8834_MOTOR_B, DRV8834_DIRECTION_FORWARD);
-    Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
-    delay(degrees * MOTOR_TURN_TIME_FACTOR);
 
-    /* Stop Turning around */
-    Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+    /* Try to rotate slowly */
+    do
+    {
+        Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
+        delay(MOTOR_TURN_TIME_FACTOR);
+        Motor_BreakMotor(DRV8834_MOTOR_BOTH);
+        degrees--;
+    }while(0 < degrees);
 }
 
 /***************************************************************************************
@@ -282,7 +290,6 @@ void Motor_TestMotor(byte motorIdentifier)
   Motor_SwitchDirection(motorIdentifier, direction);
 }
 
-
 /***************************************************************************************
  * Function: Robot_WakeUp()
  ***************************************************************************************
@@ -513,7 +520,16 @@ byte Robot_LookAround(void)
     Motor_RotateLeft(45);
     delay(DELAY_DEFAULT);
 
+    /* Dev Stuff */
+    if(E_OK == devStuff)
+    {
+        Serial.print("Around: ");
+        Serial.println(retVal, HEX);
+    }
+
+    return retVal;
 }
+
 /***************************************************************************************
  * Function: Robot_Autopilot()
  ***************************************************************************************
@@ -534,20 +550,48 @@ void Robot_Autopilot(void)
         Motor_BreakMotor(DRV8834_MOTOR_BOTH);
         delay(DELAY_DEFAULT);
 
-        /* Go a step Back */
-        Motor_SwitchDirection(DRV8834_MOTOR_BOTH, DRV8834_DIRECTION_BACKWARD);
-        Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL);
-        delay(100);
+        /* Some Dev Stuff */
+        if(E_OK == devStuff)
+        {
+            Serial.println("I see");
+        }
 
         /* Look around */
-        //whereToGo = Robot_LookAround();
+        whereToGo = Robot_LookAround();
 
         /* Decide which way to go */
-        /* Sensor used is not ok, go random for now */
-        whereToGo = 0u;
         switch(whereToGo)
         {
-            case 3u: /* Only obstacle ahead; fallthrough to obstacles everywhere */
+            case 3u: 
+                /* Only obstacle ahead */
+                /* Choose Left or right at random */
+                randomSeed(millis());
+                if(E_OK == random(2) % 2)
+                {
+                    /* Go  Right */
+                    Motor_RotateRight(45);
+                    delay(DELAY_DEFAULT);
+
+                    /* Dev Stuff */
+                    if(E_OK == devStuff)
+                    {
+                        Serial.println("3 -> Go Right");
+                    }
+                }
+                else
+                {
+                    /* Go Left */
+                    Motor_RotateLeft(45);
+                    delay(DELAY_DEFAULT);
+
+                    /* Dev Stuff */
+                    if(E_OK == devStuff)
+                    {
+                        Serial.println("3 -> Go Left");
+                    }
+                }
+
+                break;
             case 0u:
                 /* Only way is to turn around */
                 /* Turn around random degrees */
@@ -555,28 +599,42 @@ void Robot_Autopilot(void)
                 randomDegrees = (uint16_t)random(360);
                 Motor_RotateLeft(randomDegrees);
                 delay(DELAY_DEFAULT);
+
+                /* Dev Stuff */
+                if(E_OK == devStuff)
+                {
+                    Serial.println("0 -> Go Random");
+                }
+
                 break;
             case 1u:
                 /* Obstacle in Right side */
-                Motor_RotateRight(45);
+                Motor_RotateLeft(45);
                 delay(DELAY_DEFAULT);
+                
+                /* Dev Stuff */
+                if(E_OK == devStuff)
+                {
+                    Serial.println("1 -> Go Left");
+                }
+
                 break;
-            
             case 2u:
                 /* Obstacle in Left side */
                 Motor_RotateRight(45);
                 delay(DELAY_DEFAULT);
+
+                /* Dev Stuff */
+                if(E_OK == devStuff)
+                {
+                    Serial.println("2 -> Go Right");
+                }
+
                 break;
             default:
                 /* If i panic i do nothing */
                 break;
         }
-
-        /* Some Dev Stuff */
-        if(E_OK == devStuff)
-        {
-            Serial.println("I see");
-        }
     }
     else
     {
@@ -739,6 +797,7 @@ void setup(void)
     {
         /* Prepare Debug */
         Serial.begin(SERIAL_BRATE);
+        while(!Serial);
     }
     else
     {
diff --git a/Notes.h b/Notes.h
index bfa31a4..76e4770 100644
--- a/Notes.h
+++ b/Notes.h
@@ -115,6 +115,11 @@
  * - Will be powered from the same pin as IR Remote.
  * - Very bad distance readings with this IR Distance Sensor. Maybe change the sensor with a better one.
  * - If the configured threshold is too high the robot will see objects everywhere. At the limit it is reading 2cm ahead...
+ *      This is caused by Ambient Light, sensor reads ok if not in direct light.
+ * - Changed IR Obstacle Sensor to Ambient Light resistent one(KY-032 KeyesIR 4 pin Sensor). Not it works in ambient light
+ * - PROBLEM: The IR Sensor detects in a narrow direction, obstacles too high or too low are not detected.
+ *      Solution 1: Make Robot compact and short. But there are a lot of wires, can't do now.
+ *          Solution to Solution 1: Make a design board or a PCB to make everything compact.
  */
 
 /* TODO: Laser Eyes

From b6396f9b66c6c0f871700798457ab08774de63b6 Mon Sep 17 00:00:00 2001
From: basicByte0x0C <>
Date: Sat, 14 Sep 2024 15:35:04 +0300
Subject: [PATCH 7/9] Autopilot Phase: Some Cleanup

---
 EcoBot.ino |  2 +-
 Notes.h    | 17 ++++++++---------
 2 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/EcoBot.ino b/EcoBot.ino
index a0979df..24fc348 100644
--- a/EcoBot.ino
+++ b/EcoBot.ino
@@ -662,7 +662,7 @@ void Robot_Explore(void)
     static volatile long breakTime = 0;
 
     
-    /* Check Exploreing state */
+    /* Check Exploring state */
     if(exploreState == EXPLORE_AUTOMATE)
     {
         /* Try to read IR Receiver */
diff --git a/Notes.h b/Notes.h
index 76e4770..2cfb053 100644
--- a/Notes.h
+++ b/Notes.h
@@ -46,8 +46,8 @@
  */
 
 /* ----- DRV8834 -----
- * - CFG pin is wired to GND(maybe not needed?).
- * - M1 pin is wired to GND(we don't really need to use it, and Slow Decay is perfect).
+ * - CFG pin is wired to GND.
+ * - M1 pin is wired to GND.
  * - Motors need some amperage, doesn't work powered by TTL Serial Programmer, must use a LiPo Battery or similar.
  * - In documentation it is mentioned that when waking up from Sleep the Driver might take up to 1ms to become functional.
  * - Consume aprox 2.5mA when Idle, according to some measurements.
@@ -57,12 +57,12 @@
  * - Used an old LiPo single cell Battery, looks like a 14500.
  * - 1200mA or 2200mA one or the other.
  * - For BMS one small, round, green, unnamed, 1S component was used that is based on 8205A Z1J0802 chip.
- * - For Solar Controller an CN3065 Solar Charger v1.0 was used, MPPT doesn't really make sense vor a 6V Panel.
+ * - For Solar Controller an CN3065 Solar Charger v1.0 was used, MPPT doesn't really make sense for a 6V Panel.
  * - LiPo connected to BMS -> BMS connected to Solar Charger -> Solar Charger connected to Solar Panel + Robot(Arduino+DRV8834).
  */
 
 /* ----- Energy Management -----
- * - Voltage measured with Voltage Divider. R1 == R2. Resistence used: 120 Ohm probably 1W. 2W would be better.
+ * - Voltage measured with Voltage Divider. R1 == R2. Resistence used: 10 kOhm probably less than 1W. 2W would be better.
  * - Pin Read Voltage drops by 0.05 V when Motors are running. Take in consideration for threshold. 
  * - TODO: Decide if to desolder IR Obstacle LEDs. There are 2 bright leds, so it must consume some power.
  * -- Measurements ::
@@ -91,7 +91,6 @@
  *      - Robot Sleeping : 2.4 mA with R = 2 kOhm. Try with 5 kOhm.
  *      - Robot Sleeping : 1.8 mA with R = 5 kOhm. Best is 10 kOhm but let's see also accuracy.
  *      - Robot Running : 0.19A, Spike to 0.21 or slightly more
- *      
  */
 
 /* ----- IR Remote Control -----
@@ -104,7 +103,7 @@
  *      Autonomous and Manual.
  * - In Manual State, it will listen for IR Commands and execute them.
  * - In Autonomous State it will walk autonomously and avoid obstacles with sensors.
- * - If IR is not resumed after reading it it will be stuck with the same value forever. Resume let it read the next command.
+ * - If IR is not resumed after reading it it will be stuck with the same value forever. Resume to let it read the next command.
  * - Consume aprox 0.4mA when Idle, according to some measurements.
  */
 
@@ -113,10 +112,9 @@
  * - If an object is detected, the robot will rotate around and try to find another path with no obstacles.
  * - IR Output is LOW when there is an obstacle detected, else is HIGH.
  * - Will be powered from the same pin as IR Remote.
- * - Very bad distance readings with this IR Distance Sensor. Maybe change the sensor with a better one.
- * - If the configured threshold is too high the robot will see objects everywhere. At the limit it is reading 2cm ahead...
+ * - PROBLEM: If the configured threshold is too high the robot will see objects everywhere. At the limit it is reading 2cm ahead...
  *      This is caused by Ambient Light, sensor reads ok if not in direct light.
- * - Changed IR Obstacle Sensor to Ambient Light resistent one(KY-032 KeyesIR 4 pin Sensor). Not it works in ambient light
+ * - Changed IR Obstacle Sensor to Ambient Light resistent one(KY-032 KeyesIR 4 pin Sensor). Now it works in ambient light.
  * - PROBLEM: The IR Sensor detects in a narrow direction, obstacles too high or too low are not detected.
  *      Solution 1: Make Robot compact and short. But there are a lot of wires, can't do now.
  *          Solution to Solution 1: Make a design board or a PCB to make everything compact.
@@ -128,6 +126,7 @@
  * - It will be positioned looking down ahead, probably at an angle of 45 or 60 degrees.
  * - If a the measured distance is higher than a configured threshold then this means there is a hole/stairs ahead
  *      => don't move 
+ * - One alternative solution would be to use a Servo Motor to tilt the IR Obstacle Sensor down. Might consume more tho.
  */
 
 /* TODO: Function to set Driver Max Current 

From 3a150fd66bbaab8001e04e65b3a7f639b720af51 Mon Sep 17 00:00:00 2001
From: basicByte0x0C <>
Date: Sat, 14 Sep 2024 16:07:19 +0300
Subject: [PATCH 8/9] Autopilot Phase: Optimize Testing

---
 EcoBot.ino | 30 +++++++++++++++++-------------
 1 file changed, 17 insertions(+), 13 deletions(-)

diff --git a/EcoBot.ino b/EcoBot.ino
index 24fc348..2f53d5a 100644
--- a/EcoBot.ino
+++ b/EcoBot.ino
@@ -14,7 +14,7 @@
 #define DELAY_1_SECOND  1000
 #define DELAY_DEFAULT   DELAY_1_SECOND
 #define SERIAL_BRATE    115200
-static byte devStuff = E_OK;
+static byte devStuff = E_NOT_OK;
 /* General Stuff end */
 
 /* Motor Stuff */
@@ -65,7 +65,7 @@ decode_results results;
 #define PIN_INSOMNIA          	    2       /* Used for development purpose to keep the Robot awake */
 #define ADC_MAX_VALUE               1023.0
 #define ADC_MAX_VOLTAGE             3.3
-#define BATTERY_SLEEP_THRESHOLD     2.0     /* Voltage drops by 0.05 V when motors are working */
+#define BATTERY_SLEEP_THRESHOLD     3.3     /* Voltage drops by 0.05 V when motors are working */
 #define ROBOT_SLEEP_1_SECOND        1
 #define ROBOT_SLEEP_10_SECONDS      10
 #define ROBOT_SLEEP_30_SECONDS      30
@@ -769,20 +769,24 @@ void Robot_PowerManagement(void)
  **************************************************************************************/
 void Robot_Testing(void)
 {
-    /* Sleep for 20 seconds to Measure Energy Consumption */
-    //Robot_Sleep(20);
+    static uint32_t batteryReadTimeout = 0;
+    static float batteryVoltage = 0;
 
-    /* Test if motors stopped working */
-    //Motor_TestMotor(DRV8834_MOTOR_BOTH);
-
-    /* Read Battery Level */
-    float batteryLevel = analogRead(PIN_BATTERY_LEVEL);
+    /* Read Battery Level once every second */
+    if(DELAY_1_SECOND < (millis() - batteryReadTimeout))
+    {
+        /* Update read timeout */
+        batteryReadTimeout = millis();
         
-    /* Battery voltage is double the reading value; Voltage Divider is used with R1 = R2 */
-    float batteryVoltage = (batteryLevel * (ADC_MAX_VOLTAGE / ADC_MAX_VALUE)) * 2;
+        /* Battery voltage is double the reading value; Voltage Divider is used with R1 = R2 */
+        batteryVoltage = 2 * (analogRead(PIN_BATTERY_LEVEL) * (ADC_MAX_VOLTAGE / ADC_MAX_VALUE));
 
-    /* Show battery level on Serial */
-    //Serial.println(batteryVoltage);
+        /* Show battery level on Serial */
+        Serial.println(batteryVoltage);
+    }
+
+    /* Test if motors stopped working */
+    //Motor_TestMotor(DRV8834_MOTOR_BOTH);
 }
 
 /***************************************************************************************

From c29ac9987b72c05ce621bed1981fd7e652c4488f Mon Sep 17 00:00:00 2001
From: basicByte0x0C <>
Date: Sat, 14 Sep 2024 18:19:11 +0300
Subject: [PATCH 9/9] Autopilot Phase: Rework Detection; Add new Measurements

---
 EcoBot.ino | 54 ++++++++++++++++++++++++++++++++----------------------
 Notes.h    | 14 ++++++++++----
 2 files changed, 42 insertions(+), 26 deletions(-)

diff --git a/EcoBot.ino b/EcoBot.ino
index 2f53d5a..5d5be30 100644
--- a/EcoBot.ino
+++ b/EcoBot.ino
@@ -38,8 +38,17 @@ static byte devStuff = E_NOT_OK;
 /* Motor Stuff end */
 
 /* Exploration Stuff */
-#define EXPLORE_AUTOMATE  0u    /* Autonomous driving */
-#define EXPLORE_MANUAL    1u    /* Manual driving from IR */
+#define EXPLORE_AUTOMATE        0u    /* Autonomous driving */
+#define EXPLORE_MANUAL          1u    /* Manual driving from IR */
+#define EXPLORE_ROTATE_45       45
+#define EXPLORE_ROTATE_90       90
+#define EXPLORE_ROTATE_180      180
+#define EXPLORE_ROTATE_CUSTOM   135
+#define EXPLORE_ROTATE_DEFAULT  EXPLORE_ROTATE_90
+#define EXPLORE_OBSTACLE_NONE   0u
+#define EXPLORE_OBSTACLE_LEFT   1u
+#define EXPLORE_OBSTACLE_RIGHT  2u
+#define EXPLORE_OBSTACLE_BOTH   3u 
 
 static byte exploreState = EXPLORE_MANUAL;
 /* Exploration Stuff end */
@@ -483,41 +492,41 @@ byte Robot_DetectAhead(void)
  * Function: Robot_LookAround()
  ***************************************************************************************
  * Description: Return a value which correspond to obstacles ahead.
- * return       0u      Obstacles everywhere: Front, Lelft, Right
- *              1u      Left is free
- *              2u      Right is free
- *              3u      Left and Right are both free
+ * return       EXPLORE_OBSTACLE_NONE   == 0 : Left and Right are both free
+ *              EXPLORE_OBSTACLE_LEFT   == 1 : Left is blocked
+ *              EXPLORE_OBSTACLE_RIGHT  == 2 : Right is blocked
+ *              EXPLORE_OBSTACLE_BOTH   == 3 : Both Left and Right are blocked
  **************************************************************************************/
 byte Robot_LookAround(void)
 {
     /* Return Left and Right Readings */
-    byte retVal = 3u;
+    byte retVal = EXPLORE_OBSTACLE_NONE;
 
     /* Look Left */
-    Motor_RotateLeft(45);
+    Motor_RotateLeft(EXPLORE_ROTATE_DEFAULT);
     Motor_BreakMotor(DRV8834_MOTOR_BOTH);
     delay(DELAY_DEFAULT);
     if(E_NOT_OK == Robot_DetectAhead())
     {
         /* Remove this way */
-        retVal -= 1u;
+        retVal += EXPLORE_OBSTACLE_LEFT;
     }
 
     /* Center back */
-    Motor_RotateRight(45);
+    Motor_RotateRight(EXPLORE_ROTATE_DEFAULT);
     delay(DELAY_DEFAULT);
 
     /* Look Right */
-    Motor_RotateRight(45);
+    Motor_RotateRight(EXPLORE_ROTATE_DEFAULT);
     delay(DELAY_DEFAULT);
     if(E_NOT_OK == Robot_DetectAhead())
     {
         /* Remove this way as well */
-        retVal -= 2u;
+        retVal += EXPLORE_OBSTACLE_RIGHT;
     }
 
     /* Return to original position */
-    Motor_RotateLeft(45);
+    Motor_RotateLeft(EXPLORE_ROTATE_DEFAULT);
     delay(DELAY_DEFAULT);
 
     /* Dev Stuff */
@@ -560,16 +569,17 @@ void Robot_Autopilot(void)
         whereToGo = Robot_LookAround();
 
         /* Decide which way to go */
+        /* Ahead is already blocked */
         switch(whereToGo)
         {
-            case 3u: 
+            case EXPLORE_OBSTACLE_NONE: 
                 /* Only obstacle ahead */
-                /* Choose Left or right at random */
+                /* Choose Left or Right at random */
                 randomSeed(millis());
                 if(E_OK == random(2) % 2)
                 {
                     /* Go  Right */
-                    Motor_RotateRight(45);
+                    Motor_RotateRight(EXPLORE_ROTATE_DEFAULT);
                     delay(DELAY_DEFAULT);
 
                     /* Dev Stuff */
@@ -581,7 +591,7 @@ void Robot_Autopilot(void)
                 else
                 {
                     /* Go Left */
-                    Motor_RotateLeft(45);
+                    Motor_RotateLeft(EXPLORE_ROTATE_DEFAULT);
                     delay(DELAY_DEFAULT);
 
                     /* Dev Stuff */
@@ -592,7 +602,7 @@ void Robot_Autopilot(void)
                 }
 
                 break;
-            case 0u:
+            case EXPLORE_OBSTACLE_BOTH:
                 /* Only way is to turn around */
                 /* Turn around random degrees */
                 randomSeed(millis());
@@ -607,9 +617,9 @@ void Robot_Autopilot(void)
                 }
 
                 break;
-            case 1u:
+            case EXPLORE_OBSTACLE_RIGHT:
                 /* Obstacle in Right side */
-                Motor_RotateLeft(45);
+                Motor_RotateLeft(EXPLORE_ROTATE_DEFAULT);
                 delay(DELAY_DEFAULT);
                 
                 /* Dev Stuff */
@@ -619,9 +629,9 @@ void Robot_Autopilot(void)
                 }
 
                 break;
-            case 2u:
+            case EXPLORE_OBSTACLE_LEFT:
                 /* Obstacle in Left side */
-                Motor_RotateRight(45);
+                Motor_RotateRight(EXPLORE_ROTATE_DEFAULT);
                 delay(DELAY_DEFAULT);
 
                 /* Dev Stuff */
diff --git a/Notes.h b/Notes.h
index 2cfb053..1886d60 100644
--- a/Notes.h
+++ b/Notes.h
@@ -73,7 +73,7 @@
  *      - Only Motor Driver Asleep : 4.51 mA => Motor Driver in Idle consume aprox 2.5 mA
  *      - Only IR Receiver Asleep : 6.68 mA => IR Receiver in Idle consume aprox 0.4 mA
  *      - Robot Sleeping : 1.58 mA
- *  == Battery Voltage Reader==
+ *  == Battery Voltage Reader ==
  *      - Pin Read : 3.97 V == Multimeter Reading : 4.01 V => Accuracy up to 0.04 V (R1 == R2 == 120 Ohm)
  *      - Pin Read : 3.95 V == Multimeter Reading : 3.99 V => Accuracy up to 0.04 V (R1 == R2 == 120 Ohm)
  *      - Pin Read : 3.60 V == Multimeter Reading : 3.88 V => Accuracy up to 0.28 V (R1 == R2 == 10 kOhm)
@@ -91,6 +91,11 @@
  *      - Robot Sleeping : 2.4 mA with R = 2 kOhm. Try with 5 kOhm.
  *      - Robot Sleeping : 1.8 mA with R = 5 kOhm. Best is 10 kOhm but let's see also accuracy.
  *      - Robot Running : 0.19A, Spike to 0.21 or slightly more
+ *  == Everything Ready ==
+ *      - Manual Mode Idle : 11.99 mA
+ *      - Manual Mode Idle(without IR Detector) : 7.45 mA => IR Detector consume aprox 4.5 mA
+ *          I expect ~2 mA if i desolder the LEDs
+ *      - Robot Sleeping : 1.68 mA
  */
 
 /* ----- IR Remote Control -----
@@ -104,7 +109,7 @@
  * - In Manual State, it will listen for IR Commands and execute them.
  * - In Autonomous State it will walk autonomously and avoid obstacles with sensors.
  * - If IR is not resumed after reading it it will be stuck with the same value forever. Resume to let it read the next command.
- * - Consume aprox 0.4mA when Idle, according to some measurements.
+ * - Consume aprox 0.4 mA when Idle, according to some measurements.
  */
 
 /* ----- Don't be blind -----
@@ -112,12 +117,13 @@
  * - If an object is detected, the robot will rotate around and try to find another path with no obstacles.
  * - IR Output is LOW when there is an obstacle detected, else is HIGH.
  * - Will be powered from the same pin as IR Remote.
- * - PROBLEM: If the configured threshold is too high the robot will see objects everywhere. At the limit it is reading 2cm ahead...
+ * - PROBLEM(solved): If the configured threshold is too high the robot will see objects everywhere. At the limit it is reading 2cm ahead...
  *      This is caused by Ambient Light, sensor reads ok if not in direct light.
  * - Changed IR Obstacle Sensor to Ambient Light resistent one(KY-032 KeyesIR 4 pin Sensor). Now it works in ambient light.
  * - PROBLEM: The IR Sensor detects in a narrow direction, obstacles too high or too low are not detected.
  *      Solution 1: Make Robot compact and short. But there are a lot of wires, can't do now.
  *          Solution to Solution 1: Make a design board or a PCB to make everything compact.
+ * - Consume aprox 4.5 mA when Idle, according to some measurements.
  */
 
 /* TODO: Laser Eyes
@@ -130,7 +136,7 @@
  */
 
 /* TODO: Function to set Driver Max Current 
- * - My Motors seems to use 120mA max each.
+ * - My Motors seems to use 120 mA max each.
  * - Something that shall be made on Driver Hardware?
  */