From 37d8285b97d3436b6a4d8b5f9e79b78fd272c64a Mon Sep 17 00:00:00 2001
From: Siwat Sirichai <siwat@siwatinc.com>
Date: Tue, 25 Oct 2022 21:53:05 +0700
Subject: [PATCH] implement roaming

---
 kuukar_config.py  | 16 +++++++++++++++-
 kuukar_leds.py    |  2 +-
 kuukar_motion.py  | 33 ++++++++++++++++++++++++++++++---
 kuukar_sensors.py | 40 +++++++++++++++++++++++++++++++++++++++-
 4 files changed, 85 insertions(+), 6 deletions(-)

diff --git a/kuukar_config.py b/kuukar_config.py
index 1b55413..d801b0a 100644
--- a/kuukar_config.py
+++ b/kuukar_config.py
@@ -1,4 +1,12 @@
 SERIAL_DRIVER_BOARD = "/dev/serial/by-id/..."
+
+SONAR_1_TRIG_PIN = 0
+SONAR_1_ECHO_PIN = 1
+SONAR_2_TRIG_PIN = 2
+SONAR_2_ECHO_PIN = 3
+SONAR_3_TRIG_PIN = 4
+SONAR_3_ECHO_PIN = 5
+
 MOTOR_FL_F = 6
 MOTOR_FL_R = 7
 MOTOR_FR_F = 8
@@ -8,8 +16,14 @@ MOTOR_RL_R = 11
 MOTOR_RR_F = 12
 MOTOR_RR_R = 13
 
+# Full Speed 90 Degress Turn Time
+TURN_TIME_FS_90DEG_MS = 1500
+
 SERIAL_AUX_BOARD = "/dev/serial/by-id/..."
+
 COLLISION_DETECTOR_PIN = 2
+
 LEDS_DATA_PIN = 3
+LEDS_NUM = 15
+
 DHT22_PIN = 5
-LEDS_NUM = 15
\ No newline at end of file
diff --git a/kuukar_leds.py b/kuukar_leds.py
index 18803f0..3734d41 100644
--- a/kuukar_leds.py
+++ b/kuukar_leds.py
@@ -6,5 +6,5 @@ class leds:
         self.aux_board = aux_board
         self.aux_board.set_pin_mode_neopixel(LEDS_DATA_PIN,LEDS_NUM)
 
-    def flash(r: int,g: int,b: int,duration_ms: int):
+    def flash(self, r: int,g: int,b: int,duration_ms: int):
         pass
\ No newline at end of file
diff --git a/kuukar_motion.py b/kuukar_motion.py
index c763493..ae85b87 100644
--- a/kuukar_motion.py
+++ b/kuukar_motion.py
@@ -1,4 +1,5 @@
-from kuukar_config import MOTOR_FL_F, MOTOR_FL_R, MOTOR_FR_F, MOTOR_FR_R, MOTOR_RL_F, MOTOR_RL_R, MOTOR_RR_F, MOTOR_RR_R
+from random import randint
+from kuukar_config import MOTOR_FL_F, MOTOR_FL_R, MOTOR_FR_F, MOTOR_FR_R, MOTOR_RL_F, MOTOR_RL_R, MOTOR_RR_F, MOTOR_RR_R, TURN_TIME_FS_90DEG_MS
 import time
 from kuukar_leds import leds
 from kuukar_sensors import sensors
@@ -57,5 +58,31 @@ class motion:
                 self.__roam_handle__()
 
     def __roam_handle__(self):
-        #TODO Implement Roaming Function
-        pass
\ No newline at end of file
+        self.drive(50)
+        f_dist = self.sensors.sonar_get_distance(2)
+        if f_dist > 0 and f_dist < 20: # Close to collision, turn the vehicle
+            l_dist = self.sensors.sonar_get_distance
+            r_dist = self.sensors.sonar_get_distance
+            if l_dist < 20: #Left side is blocked
+                if r_dist < 20: #Both side are blocked, do a full 180deg turn
+                    self.turn(100,2*TURN_TIME_FS_90DEG_MS)
+                    self.drive(50)
+                    time.sleep(3)
+                else:
+                    #Right Side is Free, turn right
+                    self.turn(100,TURN_TIME_FS_90DEG_MS)
+                    self.drive(50)
+                    time.sleep(3)
+            elif r_dist < 20: #Right side is blocked, left side is free, turn left
+                self.turn(-100,TURN_TIME_FS_90DEG_MS)
+            else: #Both Side are free, randomize a direction
+                right: bool = randint(0,1)
+                if right:
+                    self.turn(100,TURN_TIME_FS_90DEG_MS)
+                    self.drive(50)
+                    time.sleep(3)
+                else:
+                    self.turn(-100,TURN_TIME_FS_90DEG_MS)
+                    self.drive(50)
+                    time.sleep(3)
+        time.sleep(0.5)
\ No newline at end of file
diff --git a/kuukar_sensors.py b/kuukar_sensors.py
index abc6c23..62adbaa 100644
--- a/kuukar_sensors.py
+++ b/kuukar_sensors.py
@@ -1,5 +1,43 @@
 from telemetrix_rpi_pico import telemetrix_rpi_pico
+
+from kuukar_config import DHT22_PIN, SONAR_1_ECHO_PIN, SONAR_1_TRIG_PIN, SONAR_2_ECHO_PIN, SONAR_2_TRIG_PIN, SONAR_3_ECHO_PIN, SONAR_3_TRIG_PIN
+
+
 class sensors:
+
+    __sonar_trig_pins__ = [SONAR_1_TRIG_PIN, SONAR_2_TRIG_PIN, SONAR_3_TRIG_PIN]
+    __sonar_distances__ = [0, 0, 0]
+    __temperature__ = 0
+    __humidity__ = 0
+
     def __init__(self, aux_board: telemetrix_rpi_pico.TelemetrixRpiPico, driver_board: telemetrix_rpi_pico.TelemetrixRpiPico) -> None:
         self.aux = aux_board
-        self.driver = driver_board
\ No newline at end of file
+        self.driver = driver_board
+        self.driver.set_pin_mode_sonar(
+            SONAR_1_TRIG_PIN, SONAR_1_ECHO_PIN, self.__sonar_callback__)
+        self.driver.set_pin_mode_sonar(
+            SONAR_2_TRIG_PIN, SONAR_2_ECHO_PIN, self.__sonar_callback__)
+        self.driver.set_pin_mode_sonar(
+            SONAR_3_TRIG_PIN, SONAR_3_ECHO_PIN, self.__sonar_callback__)
+        self.driver.set_pin_mode_dht(DHT22_PIN, self.__dht22_callback__)
+
+    def sonar_get_distance(self, id: int) -> float:
+        return self.__sonar_distances__[id]
+
+    def __sonar_callback__(self, data):
+        pin = data[1]
+        distance = data[2]
+        sonar_id = self.sonar_pins.index(pin)
+        self.__sonar_distances__[sonar_id] = distance
+
+    def get_temperature(self) -> float:
+        return self.__temperature__
+
+    def get_humidity_pct(self) -> float:
+        return self.__humidity__
+
+    def __dht22_callback__(self,data):
+        humidity = data[3]
+        temperature = data[4]
+        self.__humidity__ = humidity
+        self.__temperature__ = temperature
\ No newline at end of file