From f68e03753773e41e059fb26156dd942595972f34 Mon Sep 17 00:00:00 2001
From: Siwat Sirichai <siwat@siwatinc.com>
Date: Fri, 4 Nov 2022 13:56:52 +0700
Subject: [PATCH] migrate to 2 rpi board

---
 app.py                  |  5 +++--
 kuukar/kuukar_config.py |  1 +
 kuukar/kuukar_motion.py | 13 +++++++------
 pure_test.py            | 26 ++++++++++++++++++++++++++
 4 files changed, 37 insertions(+), 8 deletions(-)
 create mode 100644 pure_test.py

diff --git a/app.py b/app.py
index 9817469..c3c0ddc 100644
--- a/app.py
+++ b/app.py
@@ -8,14 +8,15 @@ import kuukar.kuukar_sensors as kuukar_sensors
 import kuukar.kuukar_environment as kuukar_environment
 from flask import Flask, request
 
-from kuukar.kuukar_config import SERIAL_MCU
+from kuukar.kuukar_config import SERIAL_MCU, SERIAL_DRV
 
 app = Flask(__name__)
 
 mcu = telemetrix_rpi_pico.TelemetrixRpiPico(com_port=SERIAL_MCU)
+drv = telemetrix_rpi_pico.TelemetrixRpiPico(com_port=SERIAL_DRV)
 sensors = kuukar_sensors.sensors(mcu)
 leds = kuukar_leds.leds(mcu)
-motion = kuukar_motion.motion(mcu, leds, sensors)
+motion = kuukar_motion.motion(mcu, drv, leds, sensors)
 lcd = kuukar_lcd.lcd(motion, sensors)
 environment = kuukar_environment.environment(lcd, leds, sensors)
 collision = kuukar_collision.collision(mcu, lcd, leds)
diff --git a/kuukar/kuukar_config.py b/kuukar/kuukar_config.py
index 7d63ba3..132e9c5 100644
--- a/kuukar/kuukar_config.py
+++ b/kuukar/kuukar_config.py
@@ -1,4 +1,5 @@
 SERIAL_MCU = "COM4"
+SERIAL_DRV = "COM20"
 
 # HC Sonar Pins
 SONAR_1_TRIG_PIN = 0
diff --git a/kuukar/kuukar_motion.py b/kuukar/kuukar_motion.py
index 0c903ac..d07f7f7 100644
--- a/kuukar/kuukar_motion.py
+++ b/kuukar/kuukar_motion.py
@@ -11,8 +11,9 @@ class motion:
 
     roaming = False
 
-    def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico, leds: leds, sensors: sensors) -> None:
+    def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico, drv: telemetrix_rpi_pico.TelemetrixRpiPico, leds: leds, sensors: sensors) -> None:
         self.mcu = mcu
+        self.drv = drv
         self.leds = leds
         self.sensors = sensors
         self.roam_thread = threading.Thread(target=self.__roam_looper)
@@ -21,15 +22,15 @@ class motion:
         drive_pins = [MOTOR_FL_F, MOTOR_FL_R, MOTOR_FR_F,
                       MOTOR_FR_R, MOTOR_RL_F, MOTOR_RL_R, MOTOR_RR_F, MOTOR_RR_R]
         for drive_pin in drive_pins:
-            self.mcu.set_pin_mode_pwm_output(drive_pin)
+            self.drv.set_pin_mode_pwm_output(drive_pin)
 
     def motor_write(self, forward_pin: int, reverse_pin: int, speed: int):
         if speed < 0:
-            self.mcu.pwm_write(forward_pin, 0)
-            self.mcu.pwm_write(reverse_pin, -speed)
+            self.drv.pwm_write(forward_pin, 0)
+            self.drv.pwm_write(reverse_pin, -speed)
         elif speed >= 0:
-            self.mcu.pwm_write(forward_pin, speed)
-            self.mcu.pwm_write(reverse_pin, 0)
+            self.drv.pwm_write(forward_pin, speed)
+            self.drv.pwm_write(reverse_pin, 0)
 
     def drive(self, speed: int):
         self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
diff --git a/pure_test.py b/pure_test.py
new file mode 100644
index 0000000..193d560
--- /dev/null
+++ b/pure_test.py
@@ -0,0 +1,26 @@
+from telemetrix_rpi_pico import telemetrix_rpi_pico
+import time
+import kuukar.kuukar_config as config
+
+global mcu,drv,distance
+mcu = telemetrix_rpi_pico.TelemetrixRpiPico("COM4")
+drv = telemetrix_rpi_pico.TelemetrixRpiPico("COM20")
+
+distance = 0
+drv.set_pin_mode_pwm_output(config.MOTOR_RR_F)
+drv.pwm_write(config.MOTOR_RR_F,30)
+
+def sonarcallback(data):
+    global mcu,drv,distance
+    pin = data[1]
+    distance = data[2]
+        
+mcu.set_pin_mode_sonar(config.SONAR_2_TRIG_PIN,config.SONAR_2_ECHO_PIN, sonarcallback)
+
+while True:
+    print(distance)
+    if distance < 20.0 and distance != 0:
+        drv.pwm_write(config.MOTOR_RR_F,0)
+        print("STOP")
+        time.sleep(1)
+        drv.pwm_write(config.MOTOR_RR_F,30)
\ No newline at end of file