finalize code

Update initailTrackBarVals to MainRobotLane

ColorPicker.py

@@ -0,0 +1,55 @@
+import cv2
+import numpy as np
+ 
+frameWidth = 400
+frameHeight = 500
+cap = cv2.VideoCapture(0)
+cap.set(3, frameWidth)
+cap.set(4, frameHeight)
+ 
+ 
+def empty(a):
+    pass
+ 
+cv2.namedWindow("HSV")
+cv2.resizeWindow("HSV", 640, 240)
+cv2.createTrackbar("HUE Min", "HSV", 0, 179, empty)
+cv2.createTrackbar("HUE Max", "HSV", 179, 179, empty)
+cv2.createTrackbar("SAT Min", "HSV", 0, 255, empty)
+cv2.createTrackbar("SAT Max", "HSV", 255, 255, empty)
+cv2.createTrackbar("VALUE Min", "HSV", 0, 255, empty)
+cv2.createTrackbar("VALUE Max", "HSV", 255, 255, empty)
+ 
+
+frameCounter = 0
+ 
+while True:
+    frameCounter +=1
+    if cap.get(cv2.CAP_PROP_FRAME_COUNT) ==frameCounter:
+        cap.set(cv2.CAP_PROP_POS_FRAMES,0)
+        frameCounter=0
+ 
+    _, img = cap.read()
+    imgHsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+ 
+    h_min = cv2.getTrackbarPos("HUE Min", "HSV")
+    h_max = cv2.getTrackbarPos("HUE Max", "HSV")
+    s_min = cv2.getTrackbarPos("SAT Min", "HSV")
+    s_max = cv2.getTrackbarPos("SAT Max", "HSV")
+    v_min = cv2.getTrackbarPos("VALUE Min", "HSV")
+    v_max = cv2.getTrackbarPos("VALUE Max", "HSV")
+    print(h_min)
+ 
+    lower = np.array([h_min, s_min, v_min])
+    upper = np.array([h_max, s_max, v_max])
+    mask = cv2.inRange(imgHsv, lower, upper)
+    result = cv2.bitwise_and(img, img, mask=mask)
+ 
+    mask = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
+    hStack = np.hstack([img, mask, result])
+    cv2.imshow('Horizontal Stacking', hStack)
+    if cv2.waitKey(1) and 0xFF == ord('q'):
+        break
+ 
+cap.release()
+cv2.destroyAllWindows()

KeyPressModule.py

@@ -0,0 +1,27 @@
+import pygame
+ 
+def init():
+    pygame.init()
+    win = pygame.display.set_mode((100,100))
+ 
+def getKey(keyName):
+    ans = False
+    for eve in pygame.event.get():pass
+    keyInput = pygame.key.get_pressed()
+    myKey = getattr(pygame,'K_{}'.format(keyName))
+    if keyInput [myKey]:
+        ans = True
+    pygame.display.update()
+ 
+    return ans
+ 
+def main():
+    if getKey('LEFT'):
+        print('Key Left was pressed')
+    if getKey('RIGHT'):
+        print('Key Right was pressed')
+ 
+if __name__ == '__main__':
+    init()
+    while True:
+        main()

LaneDetectionModule.py

@@ -0,0 +1,103 @@
+import cv2
+import numpy as np
+import utils
+
+
+curveList = []
+avgVal = 10
+
+def getLaneCurve(img, display = 2):
+
+    imgCopy = img.copy()
+    imgResult = img.copy()
+    #### STEP 1
+    imgThres = utils.thresholding(img)
+    
+    #### STEP 2
+    hT, wT, c = img.shape
+    points = utils.valTrackbars()
+    imgWarp = utils.warpImg(imgThres, points, wT, hT)
+    imgWarpPoints = utils.drawPoints(imgCopy, points)
+
+    #### STEP 3
+    middlePoint,imgHist = utils.getHistogram(imgWarp,display=True,minPer=0.5,region=4)
+    curveAveragePoint, imgHist = utils.getHistogram(imgWarp, display=True, minPer=0.9)
+    curveRaw = curveAveragePoint - middlePoint
+    
+    #### STEP 4
+    curveList.append(curveRaw)
+    if len(curveList)>avgVal:
+        curveList.pop(0)
+    curve = int(sum(curveList)/len(curveList))
+    '''
+    cv2.imshow('Thres', imgThres)
+    cv2.imshow('Warp', imgWarp)
+    cv2.imshow('Warp Points', imgWarpPoints)
+    cv2.imshow('Histogram', imgHist)
+    return None
+    '''
+     #### STEP 5
+    if display != 0:
+        imgInvWarp = utils.warpImg(imgWarp, points, wT, hT, inv=True)
+        imgInvWarp = cv2.cvtColor(imgInvWarp, cv2.COLOR_GRAY2BGR)
+        imgInvWarp[0:hT // 3, 0:wT] = 0, 0, 0
+        imgLaneColor = np.zeros_like(img)
+        imgLaneColor[:] = 0, 255, 0
+        imgLaneColor = cv2.bitwise_and(imgInvWarp, imgLaneColor)
+        imgResult = cv2.addWeighted(imgResult, 1, imgLaneColor, 1, 0)
+        midY = 450
+        cv2.putText(imgResult, str(curve), (wT // 2 - 80, 85), cv2.FONT_HERSHEY_COMPLEX, 2, (255, 0, 255), 3)
+        cv2.line(imgResult, (wT // 2, midY), (wT // 2 + (curve * 3), midY), (255, 0, 255), 5)
+        cv2.line(imgResult, ((wT // 2 + (curve * 3)), midY - 25), (wT // 2 + (curve * 3), midY + 25), (0, 255, 0), 5)
+        for x in range(-30, 30):
+            w = wT // 20
+            cv2.line(imgResult, (w * x + int(curve // 50), midY - 10),
+                     (w * x + int(curve // 50), midY + 10), (0, 0, 255), 2)
+        #fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer);
+        #cv2.putText(imgResult, 'FPS ' + str(int(fps)), (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (230, 50, 50), 3);
+    if display == 2:
+        imgStacked = utils.stackImages(0.7, ([img, imgWarpPoints, imgWarp],
+                                             [imgHist, imgLaneColor, imgResult]))
+        cv2.imshow('ImageStack', imgStacked)
+    elif display == 1:
+        cv2.imshow('Resutlt', imgResult)
+
+    '''
+    cv2.imshow('Thres', imgThres)
+    cv2.imshow('Warp', imgWarp)
+    cv2.imshow('Warp Points', imgWarpPoints)
+    cv2.imshow('Histogramp', imgHist)
+    '''
+    #### NORMALIZATION
+    curve = curve/100
+    if curve>1: curve ==1
+    if curve<-1:curve == -1
+ 
+    return curve
+
+    
+
+if __name__ == '__main__':
+    cap = cv2.VideoCapture(0)
+    initialTrackBarVals = [98, 141, 77, 220]
+    utils.initializeTrackbars(initialTrackBarVals)
+    frameCounter = 0
+    curveList = []
+
+    while True:
+        frameCounter +=1
+        if cap.get(cv2.CAP_PROP_FRAME_COUNT) ==frameCounter:
+            cap.set(cv2.CAP_PROP_POS_FRAMES,0)
+            frameCounter=0
+
+        success, img = cap.read()
+        img = cv2.resize(img,(480,240))
+        curve = getLaneCurve(img, display = 2)
+        print(curve)
+        #cv2.imshow('Vid', img)
+        cv2.waitKey(1)
+
+
+
+
+

MainRobotLane.py

@@ -0,0 +1,52 @@
+from time import perf_counter
+import cv2
+from kuukar.kuukar import kuukar
+from LaneDetectionModule import getLaneCurve
+from utils import initializeTrackbars
+import WebcamModule
+from threading import Thread
+global speed, sen, car, delay
+sen = 1
+speed = 0
+delay = 0.15
+car = kuukar()
+
+##################################################
+############### MAIN ROBOT LANE ##################
+##################################################
+
+
+def main():
+    global speed, sen, car, delay
+    while True:
+        stt = perf_counter()
+        img = WebcamModule.getImg()
+        curveVal = getLaneCurve(img, 2)
+        maxVAl = 1.0  # MAX SPEED
+        if curveVal > maxVAl:
+            curveVal = maxVAl
+        if curveVal < -maxVAl:
+            curveVal = -maxVAl
+        # print(curveVal)
+        if curveVal > 0:
+            sen = 1.7
+            if curveVal < 0.05:
+                curveVal = 0
+        else:
+            if curveVal > -0.08:
+                curveVal = 0
+        car.motion.skt_drive(speed, -curveVal*sen, delay)
+        turn_value = -curveVal*sen
+        #print("Curve Value : " + str(curveVal) + " Turn Value : "+ str(round(turn_value,2)))
+        #print(f"looptime: {perf_counter()-stt}s")
+        cv2.waitKey(1)
+
+
+if __name__ == '__main__':
+    initialTrackBarVals = [107, 134, 89, 221]
+    initializeTrackbars(initialTrackBarVals)
+    Thread(target=main).start()
+    while True:
+        speed = float(input("speed: "))
+        sen = float(input("sen: "))
+        delay = float(input("delay: "))

RobotMain2.py

@@ -0,0 +1,22 @@
+from kuukar.kuukar import kuukar
+import KeyPressModule as kp
+
+car = kuukar()
+kp.init()
+
+def main(self):
+
+    if kp.getKey('UP'):
+        car.motion.skt_drive(0.4,0.0,0.05)
+    elif kp.getKey('DOWN'):
+        car.motion.skt_drive(-0.4,0.0,0.05)
+    elif kp.getKey('LEFT'):
+        car.motion.skt_drive(0.5,0.3,0.05)
+    elif kp.getKey('RIGHT'):
+        car.motion.skt_drive(0.5,-0.3,0.05)
+    else:
+        car.motion.stop()
+
+if __name__ == '__main__':
+    while True:
+        main()

WebcamModule.py

@@ -0,0 +1,14 @@
+import cv2
+ 
+cap = cv2.VideoCapture(0)
+ 
+def getImg(display= False,size=[480,240]):
+    _, img = cap.read()
+    img = cv2.resize(img,(size[0],size[1]))
+    if display:
+        cv2.imshow('IMG',img)
+    return img
+ 
+if __name__ == '__main__':
+    while True:
+        img = getImg(True)

app.py

@@ -1,32 +1,10 @@
-import time
-# from telemetrix_rpi_pico import telemetrix_rpi_pico
-# import kuukar.kuukar_leds as kuukar_leds
-# import kuukar.kuukar_collision as kuukar_collision
-# import kuukar.kuukar_motion as kuukar_motion
-# import kuukar.kuukar_lcd as kuukar_lcd
-# import kuukar.kuukar_sensors as kuukar_sensors
-# import kuukar.kuukar_environment as kuukar_environment
-
-# from kuukar.kuukar_config import SERIAL_MCU, SERIAL_DRV
-
 from kuukar.kuukar import kuukar
-
-# mcu = telemetrix_rpi_pico.TelemetrixRpiPico(com_port=SERIAL_MCU, sleep_tune=0.001)
-# drv = telemetrix_rpi_pico.TelemetrixRpiPico(com_port=SERIAL_DRV, sleep_tune=0.001)
-# sensors = kuukar_sensors.sensors(mcu)
-# leds = kuukar_leds.leds(mcu)
-# 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)
+import time
 
 car = kuukar()
-
 while True:
     temp = car.sensors.get_temperature()
     humid = car.sensors.get_humidity_pct()
-    dist1 = car.sensors.sonar_get_distance(0)
-    dist2 = car.sensors.sonar_get_distance(1)
-    dist3 = car.sensors.sonar_get_distance(2)
-    print(f'temp: {temp}, humidity: {humid}, dist1: {dist1}, dist2: {dist2}, dist3: {dist3}')
+    dist = car.sensors.sonar_get_distance()
+    print(f'temp: {temp}, humidity: {humid}, dist: {dist}')
     time.sleep(1)

kuukar/kuukar.py

@@ -5,21 +5,21 @@ import kuukar.kuukar_motion as kuukar_motion
 import kuukar.kuukar_lcd as kuukar_lcd
 import kuukar.kuukar_sensors as kuukar_sensors
 import kuukar.kuukar_environment as kuukar_environment
-from kuukar.kuukar_config import SERIAL_MCU, SERIAL_DRV
+import kuukar.kuukar_cv as kuukar_cv
+from kuukar.kuukar_config import SERIAL_MCU
 
 
 class kuukar:
     def __init__(self) -> None:
         self.mcu = telemetrix_rpi_pico.TelemetrixRpiPico(
             com_port=SERIAL_MCU, sleep_tune=0.001)
-        self.drv = telemetrix_rpi_pico.TelemetrixRpiPico(
-            com_port=SERIAL_DRV, sleep_tune=0.001)
-        self.sensors = kuukar_sensors.sensors(self.mcu, self.drv)
+        self.sensors = kuukar_sensors.sensors(self.mcu)
         self.leds = kuukar_leds.leds(self.mcu)
         self.motion = kuukar_motion.motion(
-            self.mcu, self.drv, self.leds, self.sensors)
+            self.mcu, self.leds, self.sensors)
         self.lcd = kuukar_lcd.lcd(self.motion, self.sensors)
         self.environment = kuukar_environment.environment(
             self.lcd, self.leds, self.sensors)
         self.collision = kuukar_collision.collision(
             self.mcu, self.lcd, self.leds)
+        self.cv = kuukar_cv.cv(self.lcd, self.leds, self.motion)

kuukar/kuukar_collision.py

@@ -3,15 +3,18 @@ from kuukar.kuukar_config import COLLISION_DETECTOR_PIN, COLLISION_ENABLE
 
 from kuukar.kuukar_lcd import lcd
 from kuukar.kuukar_leds import leds
+
+
 class collision:
     def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico, lcd: lcd, leds: leds) -> None:
         self.mcu = mcu
         self.lcd = lcd
         self.leds = leds
         if COLLISION_ENABLE:
-            self.mcu.set_pin_mode_digital_input(COLLISION_DETECTOR_PIN, self.collision_handle)
-        
+            self.mcu.set_pin_mode_digital_input(
+                COLLISION_DETECTOR_PIN, self.collision_handle)
+
     def collision_handle(self, data):
         val = data[2]
         if val == 1:
-            self.lcd.play_video("keke_hurt")
+            self.lcd.play_video("keke_hurt")

kuukar/kuukar_config.py

@@ -1,17 +1,15 @@
-SERIAL_MCU = "/dev/serial/by-id/usb-Raspberry_Pi_Pico_E660B4400771212A-if00"
-SERIAL_DRV = "/dev/serial/by-id/usb-Raspberry_Pi_Pico_E6609CB2D3846033-if00"
+SERIAL_MCU = "/dev/serial/by-id/usb-Raspberry_Pi_Pico_E6609CB2D3846033-if00"
 SERIAL_LCD = "/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0"
 
-# HC Sonar Pins
-SONAR_1_ADC_PIN = 0
-SONAR_2_ADC_PIN = 1
-SONAR_3_ADC_PIN = 2
+# Sonar Pin
+SONAR_ADC_PIN = 1
+
 
 # Motor Pins
-MOTOR_FL_F = 7
-MOTOR_FL_R = 6
-MOTOR_FR_F = 8
-MOTOR_FR_R = 9
+MOTOR_FL_F = 6
+MOTOR_FL_R = 7
+MOTOR_FR_F = 9
+MOTOR_FR_R = 8
 MOTOR_RL_F = 10
 MOTOR_RL_R = 11
 MOTOR_RR_F = 13
@@ -20,8 +18,6 @@ MOTOR_RR_R = 12
 # Full Speed 90 Degrees Turn Time
 TURN_TIME_FS_90DEG_MS = 3000.0
 
-BEEPER_PIN = 15
-
 COLLISION_DETECTOR_PIN = 16
 COLLISION_ENABLE = True
 
@@ -32,6 +28,6 @@ LEFT_SIGNAL_LEDS = [4, 5, 6, 7]
 RIGHT_SIGNAL_LEDS = [0, 1, 14, 15]
 REVERSE_LEDS = [11, 10, 9]
 
-LIGHT_SENSOR_PIN = 2
+LIGHT_SENSOR_PIN = 0
 
-DHT22_PIN = 14
+DHT22_PIN = 3

kuukar/kuukar_cv.py

@@ -1,13 +1,14 @@
 from kuukar.kuukar_lcd import lcd
 from kuukar.kuukar_leds import leds
+from kuukar.kuukar_motion import motion
 import threading
 import multiprocessing
 import cv2
 
 
-
 class cv:
-    def __init__(self, lcd: lcd, leds: leds) -> None:
+    def __init__(self, lcd: lcd, leds: leds, motion: motion) -> None:
+        self.motion = motion
         self.lcd = lcd
         self.leds = leds
         self.camera = cv2.VideoCapture(0)
@@ -16,15 +17,20 @@ class cv:
     def _cv_loop(self):
         while True:
             img = self.capture_image()
-            stop_sign = self.detect_sign(img)
-            
+            stop_signs = self.detect_sign(img)
+            if len(stop_signs) >= 1:  # Stop Sign Found~!
+                self.motion.stop()
+
+    def get_road_curve(self, frame):
+        pass
 
     def capture_image(self):
         ret, frame = self.camera.read()
         return frame
+
     def detect_sign(self, frame):
         stopsign_cascade = cv2.CascadeClassifier('./stopsign_good.xml')
         image = frame.array
-        gray_img=cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
-        found_stopsigns=stopsign_cascade.detectMultiScale(gray_img,1.1,5)
+        gray_img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+        found_stopsigns = stopsign_cascade.detectMultiScale(gray_img, 1.1, 5)
         return found_stopsigns

kuukar/kuukar_environment.py

@@ -8,24 +8,25 @@ from kuukar.kuukar_sensors import sensors
 class environment:
     t_alerted = False
     h_alerted = False
+
     def __init__(self, lcd: lcd, leds: leds, sensors: sensors) -> None:
         self.lcd = lcd
         self.leds = leds
         self.sensors = sensors
         threading.Thread(target=self.__checker).start()
-    
+
     def __checker(self):
         while True:
             if self.sensors.get_temperature() > 38.0:
                 if not self.t_alerted:
-                    #self.lcd.play_video("keke_died")
+                    self.lcd.play_video("keke_hurt")
                     self.t_alerted = True
             else:
                 self.t_alerted = False
 
-            if self.sensors.get_humidity_pct() > 60.0:
+            if self.sensors.get_humidity_pct() > 80.0:
                 if not self.h_alerted:
-                    #self.lcd.play_video("keke_cute_noise")
+                    self.lcd.play_video("kanon_hurt")
                     self.h_alerted = True
             else:
                 self.h_alerted = False
@@ -34,4 +35,4 @@ class environment:
                 self.leds.set_headlights(True)
             else:
                 self.leds.set_headlights(False)
-            sleep(1)
+            sleep(1)

kuukar/kuukar_lcd.py

@@ -51,7 +51,8 @@ class lcd:
                     turn = (x - 5) / (150 - 5) * 200 - 100
                     print(f"Setting speed to {speed} and turn angle to {turn}")
                     if not self.motion.is_roaming():
-                        self.motion.drive_skew(speed,turn)
+                        self.motion.drive_skew(speed, turn)
+
     def _sensor_screen_updator(self):
         while True:
             self.nextion.send_command(
@@ -61,5 +62,5 @@ class lcd:
             self.nextion.send_command(
                 f"brightnessTXT.txt=\"{self.sensors.get_brightness_pct()}\"")
             self.nextion.send_command(
-                f"sonarTXT.txt=\"{int(self.sensors.sonar_get_distance(2))} / {int(self.sensors.sonar_get_distance(1))} / {int(self.sensors.sonar_get_distance(0))}\"")
+                f"sonarTXT.txt=\"{self.sensors.sonar_get_distance()}\"")
             sleep(1)

kuukar/kuukar_leds.py

@@ -18,7 +18,7 @@ class leds:
     def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico) -> None:
         self.mcu = mcu
         self.mcu.set_pin_mode_neopixel(LEDS_DATA_PIN, LEDS_NUM)
-        self.ambient_light = [2, 2, 10] #Keke Image Color
+        self.ambient_light = [2, 2, 10]  # Keke Image Color
         self.__update_leds()
 
     def set_headlights(self, state: bool):

kuukar/kuukar_motion.py

@@ -11,9 +11,8 @@ class motion:
 
     roaming = False
 
-    def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico, drv: telemetrix_rpi_pico.TelemetrixRpiPico, leds: leds, sensors: sensors) -> None:
+    def __init__(self, mcu: 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)
@@ -22,15 +21,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.drv.set_pin_mode_pwm_output(drive_pin)
+            self.mcu.set_pin_mode_pwm_output(drive_pin)
 
     def motor_write(self, forward_pin: int, reverse_pin: int, speed: int):
         if speed < 0:
-            self.drv.pwm_write(forward_pin, 0)
-            self.drv.pwm_write(reverse_pin, -int(speed))
+            self.mcu.pwm_write(forward_pin, 0)
+            self.mcu.pwm_write(reverse_pin, -int(speed))
         elif speed >= 0:
-            self.drv.pwm_write(forward_pin, int(speed))
-            self.drv.pwm_write(reverse_pin, 0)
+            self.mcu.pwm_write(forward_pin, int(speed))
+            self.mcu.pwm_write(reverse_pin, 0)
 
     def drive(self, speed: int):
         self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
@@ -72,23 +71,76 @@ class motion:
             self.leds.set_right_signal_led(False)
             self.leds.set_left_signal_led(True)
 
-
         lm_speed = (100+dir/2)*speed/100
-        if lm_speed>99:
+        if lm_speed > 99:
             lm_speed = 99
-        elif lm_speed<-99:
+        elif lm_speed < -99:
             lm_speed = -99
         rm_speed = (100-dir/2)*speed/100
         if rm_speed > 99:
             rm_speed = 99
         elif rm_speed < -99:
-            rm_speed = -99;
+            rm_speed = -99
 
         self.motor_write(MOTOR_FL_F, MOTOR_FL_R, lm_speed)
         self.motor_write(MOTOR_FR_F, MOTOR_FR_R, rm_speed)
         self.motor_write(MOTOR_RL_F, MOTOR_RL_R, lm_speed)
         self.motor_write(MOTOR_RR_F, MOTOR_RR_R, rm_speed)
-        
+
+    '''    
+    def skt_drive(self, speed: float, turn: float, t=0):
+        speed *=100
+        turn *=100
+        leftSpeed = speed - turn
+        rightSpeed = speed + turn
+        if leftSpeed>99: leftSpeed=99
+        elif leftSpeed<-99: leftSpeed= -99
+        if rightSpeed>99: rightSpeed=99
+        elif rightSpeed<-99: rightSpeed= -99
+        if speed>99: speed=99
+        elif speed<-99: speed= -99
+
+        if (rightSpeed - leftSpeed) > 0:     
+            ##self.motor_write(MOTOR_FL_F, MOTOR_FL_R, -1*abs(leftSpeed))
+            self.motor_write(MOTOR_FR_F, MOTOR_FR_R, abs(rightSpeed))
+            ##self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
+            self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)
+        elif (rightSpeed - leftSpeed) < 0:
+            self.motor_write(MOTOR_FL_F, MOTOR_FL_R, abs(leftSpeed))
+            ##self.motor_write(MOTOR_FR_F, MOTOR_FR_R, -1*abs(rightSpeed))
+            self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
+            ##self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)
+        else:
+            self.motor_write(MOTOR_FL_F, MOTOR_FL_R, abs(leftSpeed))
+            self.motor_write(MOTOR_FR_F, MOTOR_FR_R, abs(rightSpeed))
+            self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
+            self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)
+    '''
+
+    def skt_drive(self, speed: float, turn: float, t=0):
+        speed *= 100
+        turn *= 100
+        leftSpeed = speed - turn
+        rightSpeed = speed + turn
+        if leftSpeed > 99:
+            leftSpeed = 99
+        elif leftSpeed < -99:
+            leftSpeed = -99
+        if rightSpeed > 99:
+            rightSpeed = 99
+        elif rightSpeed < -99:
+            rightSpeed = -99
+        if speed > 99:
+            speed = 99
+        elif speed < -99:
+            speed = -99
+
+        self.motor_write(MOTOR_FL_F, MOTOR_FL_R, abs(leftSpeed))
+        self.motor_write(MOTOR_FR_F, MOTOR_FR_R, abs(rightSpeed))
+        #self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
+        #self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)
+        time.sleep(t)
+
     def stop(self):
         self.leds.set_left_signal_led(False)
         self.leds.set_right_signal_led(False)
@@ -111,43 +163,12 @@ class motion:
         while True:
             if self.roaming:
                 self.__roam_handle()
-            time.sleep(0.3)
+            time.sleep(0.1)
 
     def __roam_handle(self):
-        sensitivity = 35
-        turn_speed = 40
-        drive_speed = 25
+        if self.sensors.sonar_get_distance() < 900:
+            self.turn(32, 2600)
+            self.drive(25)
 
-        f_dist = self.sensors.sonar_get_distance(1)
-        if 0 < f_dist < sensitivity:  # Close to collision, turn the vehicle
-            print("collision")
-            self.drive(-drive_speed)
-            time.sleep(0.75)
-            self.stop()
-            l_dist = self.sensors.sonar_get_distance(0)
-            r_dist = self.sensors.sonar_get_distance(2)
-            if l_dist < sensitivity:  # Left side is blocked
-                if r_dist < sensitivity:  # Both side are blocked, do a full 180deg turn
-                    print("full turn")
-                    self.turn(30, 2)
-                    self.drive(23)
-                else:
-                    # Right Side is Free, turn right
-                    print("right turn")
-                    self.turn(turn_speed, 1)
-                    self.drive(drive_speed)
-            elif r_dist < sensitivity:  # Right side is blocked, left side is free, turn left
-                print("right left")
-                self.turn(-turn_speed, 1)
-                self.drive(drive_speed)
-            else:  # Both Side are free, randomize a direction
-                print("turn random")
-                right: bool = randint(0, 1)
-                if right:
-                    self.turn(turn_speed, 1)
-                    self.drive(drive_speed)
-                else:
-                    self.turn(-turn_speed, 1)
-                    self.drive(drive_speed)
         if not self.roaming:
-            self.stop()
+            self.stop()

kuukar/kuukar_sensors.py

@@ -1,32 +1,25 @@
 from telemetrix_rpi_pico import telemetrix_rpi_pico
-from kuukar.kuukar_config import DHT22_PIN, SONAR_1_ADC_PIN, SONAR_2_ADC_PIN, SONAR_3_ADC_PIN, LIGHT_SENSOR_PIN
+from kuukar.kuukar_config import DHT22_PIN, SONAR_ADC_PIN, LIGHT_SENSOR_PIN
 
 
 class sensors:
-    __sonar_adc_pins = [SONAR_1_ADC_PIN,
-                         SONAR_2_ADC_PIN, SONAR_3_ADC_PIN]
-    __sonar_distances = [0, 0, 0]
+    __sonar_distances = 0
     __temperature = 0
     __humidity = 0
     __brightness = 0
 
-    def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico, drv: telemetrix_rpi_pico.TelemetrixRpiPico) -> None:
+    def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico) -> None:
         self.mcu = mcu
-        self.drv = drv
-        self.drv.set_pin_mode_analog_input(SONAR_1_ADC_PIN, 100, self.__sonar_callback)
-        self.drv.set_pin_mode_analog_input(SONAR_2_ADC_PIN, 100, self.__sonar_callback)
-        self.drv.set_pin_mode_analog_input(SONAR_3_ADC_PIN, 100, self.__sonar_callback)
-        self.mcu.set_pin_mode_dht(DHT22_PIN, self.__dht22_callback)
-        self.drv.set_pin_mode_analog_input(LIGHT_SENSOR_PIN, 0, self.__light_sensor_callback)
+        self.mcu.set_pin_mode_analog_input(SONAR_ADC_PIN, 100, self.__sonar_callback)
+        self.mcu.set_pin_mode_dht(DHT22_PIN, self.__dht22_callback) 
+        self.mcu.set_pin_mode_analog_input(LIGHT_SENSOR_PIN, 100, self.__light_sensor_callback)
 
-    def sonar_get_distance(self, id: int) -> float:
-        return self.__sonar_distances[id]
+    def sonar_get_distance(self) -> float:
+        return self.__sonar_distances
 
     def __sonar_callback(self, data):
-        pin = data[1]
         distance = data[2]
-        sonar_id = self.__sonar_adc_pins.index(pin)
-        self.__sonar_distances[sonar_id] = distance
+        self.__sonar_distances = distance
 
     def get_brightness_pct(self) -> float:
         return self.__brightness
@@ -41,7 +34,7 @@ class sensors:
         self.__brightness = (4096-float(data[2]))/4096*100
 
     def __dht22_callback(self, data):
-        humidity = data[3]
-        temperature = data[4]
+        humidity = data[2]
+        temperature = data[3]
         self.__humidity = humidity
         self.__temperature = temperature

utils.py

@@ -0,0 +1,104 @@
+import cv2
+import numpy as np
+
+
+
+def thresholding(img):
+    imgHsv = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
+    lowerWhite = np.array([0,0,180])
+    upperWhite = np.array([179,255,255])
+    maskWhite = cv2.inRange(imgHsv,lowerWhite,upperWhite)
+    return maskWhite
+
+def warpImg(img,points,w,h,inv = False):
+    pts1 = np.float32(points)
+    pts2 = np.float32([[0,0],[w,0],[0,h],[w,h]])
+    if inv:
+        matrix = cv2.getPerspectiveTransform(pts2, pts1)
+    else:
+        matrix = cv2.getPerspectiveTransform(pts1,pts2)
+    imgWarp = cv2.warpPerspective(img,matrix,(w,h))
+    return imgWarp
+ 
+def nothing(a):
+    pass
+ 
+def initializeTrackbars(intialTracbarVals,wT=480, hT=240):
+    cv2.namedWindow("Trackbars")
+    cv2.resizeWindow("Trackbars", 360, 240)
+    cv2.createTrackbar("Width Top", "Trackbars", intialTracbarVals[0],wT//2, nothing)
+    cv2.createTrackbar("Height Top", "Trackbars", intialTracbarVals[1], hT, nothing)
+    cv2.createTrackbar("Width Bottom", "Trackbars", intialTracbarVals[2],wT//2, nothing)
+    cv2.createTrackbar("Height Bottom", "Trackbars", intialTracbarVals[3], hT, nothing)
+ 
+def valTrackbars(wT=480, hT=240):
+    widthTop = cv2.getTrackbarPos("Width Top", "Trackbars")
+    heightTop = cv2.getTrackbarPos("Height Top", "Trackbars")
+    widthBottom = cv2.getTrackbarPos("Width Bottom", "Trackbars")
+    heightBottom = cv2.getTrackbarPos("Height Bottom", "Trackbars")
+    points = np.float32([(widthTop, heightTop), (wT-widthTop, heightTop),
+                      (widthBottom , heightBottom ), (wT-widthBottom, heightBottom)])
+    return points
+ 
+def drawPoints(img,points):
+    for x in range(4):
+        cv2.circle(img,(int(points[x][0]),int(points[x][1])),15,(0,0,255),cv2.FILLED)
+    return img
+
+
+def getHistogram(img,minPer=0.1,display= False,region=1):
+ 
+    if region == 1:
+        histValues = np.sum(img, axis= 0)
+    else:
+        histValues = np.sum(img[img.shape[0]//region:,:], axis = 0)
+    
+ 
+    maxValue = np.max(histValues)
+    minValue = minPer*maxValue
+
+    indexArray = np.where(histValues >= minValue)
+    basePoint = int(np.average(indexArray))
+    #print(basePoint)
+
+    if display:
+        imgHist = np.zeros((img.shape[0],img.shape[1],3),np.uint8)
+        for x,intensity in enumerate(histValues):
+            cv2.line(imgHist,(x,img.shape[0]),(x,img.shape[0]-intensity//255//region),(255,0,255),1)
+            cv2.circle(imgHist,(basePoint,img.shape[0]),20,(0,255,255),cv2.FILLED)
+            
+        return basePoint,imgHist
+ 
+    return basePoint
+
+
+def stackImages(scale,imgArray):
+    rows = len(imgArray)
+    cols = len(imgArray[0])
+    rowsAvailable = isinstance(imgArray[0], list)
+    width = imgArray[0][0].shape[1]
+    height = imgArray[0][0].shape[0]
+    if rowsAvailable:
+        for x in range ( 0, rows):
+            for y in range(0, cols):
+                if imgArray[x][y].shape[:2] == imgArray[0][0].shape [:2]:
+                    imgArray[x][y] = cv2.resize(imgArray[x][y], (0, 0), None, scale, scale)
+                else:
+                    imgArray[x][y] = cv2.resize(imgArray[x][y], (imgArray[0][0].shape[1], imgArray[0][0].shape[0]), None, scale, scale)
+                if len(imgArray[x][y].shape) == 2: imgArray[x][y]= cv2.cvtColor( imgArray[x][y], cv2.COLOR_GRAY2BGR)
+        imageBlank = np.zeros((height, width, 3), np.uint8)
+        hor = [imageBlank]*rows
+        hor_con = [imageBlank]*rows
+        for x in range(0, rows):
+            hor[x] = np.hstack(imgArray[x])
+        ver = np.vstack(hor)
+    else:
+        for x in range(0, rows):
+            if imgArray[x].shape[:2] == imgArray[0].shape[:2]:
+                imgArray[x] = cv2.resize(imgArray[x], (0, 0), None, scale, scale)
+            else:
+                imgArray[x] = cv2.resize(imgArray[x], (imgArray[0].shape[1], imgArray[0].shape[0]), None,scale, scale)
+            if len(imgArray[x].shape) == 2: imgArray[x] = cv2.cvtColor(imgArray[x], cv2.COLOR_GRAY2BGR)
+        hor= np.hstack(imgArray)
+        ver = hor
+    return ver