From e62c361970f254c3382195f9ed5c511fc33a2e89 Mon Sep 17 00:00:00 2001
From: Rihards Skuja <rssk@protonmail.com>
Date: Mon, 22 Jan 2018 21:24:41 +0200
Subject: [PATCH] Initial logic for avoiding obstacles

---
 robot/include/main.h |   2 +
 robot/src/main.c     | 110 +++++++++++++++++++++++++++++++------------
 2 files changed, 83 insertions(+), 29 deletions(-)

diff --git a/robot/include/main.h b/robot/include/main.h
index b5fa035..1f51152 100644
--- a/robot/include/main.h
+++ b/robot/include/main.h
@@ -5,5 +5,7 @@
 #include "common.h"
 
 void convert_data_to_raw(data_packet_t *in, uint8_t *out);
+void send_sensor_data(const int32_t *distance_in_cm);
+void decide_movement(const int32_t *distance_in_cm);
 
 #endif
diff --git a/robot/src/main.c b/robot/src/main.c
index b706bf0..75c3699 100644
--- a/robot/src/main.c
+++ b/robot/src/main.c
@@ -14,16 +14,18 @@
 #include "debug.h"
 #endif /* DEBUG */
 
+#define OBSTACLE_THRESHOLD  10
+
+uint8_t tx_raw[sizeof(data_packet_t)];
+data_packet_t tx_data;
+
 int main(void)
 {
-    uint8_t *tx_raw;
-    data_packet_t *tx_data;
+    int32_t distance_in_cm;
 
     // Data packet is too large, should split in multiple packets
     if (sizeof(data_packet_t) > MAX_PAYLOAD_SIZE)
         return R_PACKAGE_SIZE;
-    tx_data = malloc(sizeof(data_packet_t));
-    tx_raw = malloc(sizeof(data_packet_t));
 
     init_leds();
     init_servos();
@@ -41,35 +43,85 @@ int main(void)
 
     sei();
 
-    run_servos();
-
+    servo_full_forw();
     while (1) {
-        uint16_t distance_int_cm = read_sonar();
-        double temp = ds18b20_gettemp();
+        // Distance is measured early so the same value gets sent to the base
+        // station and gets used to decide movement. Needs to be done in order
+        // for base station to receive meaningful distance value.
+        distance_in_cm = read_sonar();
+        decide_movement(&distance_in_cm);
 
-        if (distance_int_cm <= 10 && distance_int_cm != SONAR_ERROR) {
-            SET(LED_PORT, LED_BLUE_BIT);
-        } else {
-            CLR(LED_PORT, LED_BLUE_BIT);
-        }
-        if (temp >= 23) {
-            SET(LED_PORT, LED_GREEN_BIT);
-        } else {
-            CLR(LED_PORT, LED_GREEN_BIT);
-        }
-
-        memset(tx_data, 0, sizeof(data_packet_t));
-        tx_data->distance = distance_int_cm;
-        tx_data->temp = temp;
-
-        memcpy(tx_raw, tx_data, sizeof(data_packet_t));
-        nrf24_send(tx_raw);
-        // TODO: prevent transmission to block the main loop
-        while (nrf24_isSending()); // Wait for transmission to finish
+        // Update base station
+        send_sensor_data(&distance_in_cm);
     }
 
-    free(tx_data);
-    free(tx_raw);
+    cli();
 
     return R_OK;
 }
+
+void send_sensor_data(const int32_t *distance_in_cm)
+{
+    double temp;
+
+    temp = ds18b20_gettemp();
+    /* if (temp >= 23) */
+
+
+    // Prepare the data to be sent
+    memset(&tx_data, 0, sizeof(data_packet_t));
+    tx_data.distance = *distance_in_cm;
+    tx_data.temp = temp;
+    memcpy(tx_raw, &tx_data, sizeof(data_packet_t));
+
+    nrf24_send(tx_raw);
+
+    // Indicate package sending with a signal LED
+    SET(LED_PORT, LED_GREEN_BIT);
+    while (nrf24_isSending());
+    CLR(LED_PORT, LED_GREEN_BIT);
+}
+
+/// Control servos based on received distance data
+void decide_movement(const int32_t *distance_in_cm)
+{
+    uint8_t avoid_obstacle = 0;
+    int32_t distance_to_obstacle = *distance_in_cm;
+
+    // No obstacle found
+    if (distance_to_obstacle == ECHO_TIMEOUT) return;
+
+    while (1) {
+        if (distance_to_obstacle == ECHO_TIMEOUT)  {
+            // No obstacle found
+            servo_full_forw();
+            avoid_obstacle = 0;
+        } else if (distance_to_obstacle >= 0
+                   && distance_to_obstacle <= MIN_SONAR_RANGE) {
+            // Obstacle is right in front, can't make a turn
+            servo_full_backw();
+            avoid_obstacle = 1;
+            _delay_ms(SONAR_DELAY);
+        } else if (distance_to_obstacle > MIN_SONAR_RANGE
+                   && distance_to_obstacle <= OBSTACLE_THRESHOLD) {
+            // Obstacle is close and in front, make a turn backwards
+            servo_turn_backw_r();
+            avoid_obstacle = 1;
+            _delay_ms(SONAR_DELAY);
+        } else {
+            // TODO: factor this in decision making about maneuvers
+            // Obstacle is in front, but a safe distance away
+            servo_full_forw();
+            avoid_obstacle = 0;
+        }
+
+        if (avoid_obstacle == 1) {
+            SET(LED_PORT, LED_BLUE_BIT);
+        } else {
+            CLR(LED_PORT, LED_BLUE_BIT);
+            return;
+        }
+
+        distance_to_obstacle = read_sonar();
+    }
+}