Compare commits

..

1 Commits
lite ... main

Author SHA1 Message Date
Siwat Sirichai 52a5f3d8a8 implement kuukar_cv 2022-12-11 11:18:06 +07:00
21 changed files with 129 additions and 500 deletions

View File

@ -1,55 +0,0 @@
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()

View File

@ -1,27 +0,0 @@
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()

View File

@ -1,103 +0,0 @@
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)

View File

@ -1,52 +0,0 @@
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: "))

View File

@ -1,22 +0,0 @@
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()

View File

@ -1,14 +0,0 @@
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)

28
app.py
View File

@ -1,10 +1,32 @@
from kuukar.kuukar import kuukar
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)
car = kuukar()
while True:
temp = car.sensors.get_temperature()
humid = car.sensors.get_humidity_pct()
dist = car.sensors.sonar_get_distance()
print(f'temp: {temp}, humidity: {humid}, dist: {dist}')
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}')
time.sleep(1)

Binary file not shown.

Before

Width:  |  Height:  |  Size: 242 KiB

View File

@ -6,20 +6,22 @@ import kuukar.kuukar_lcd as kuukar_lcd
import kuukar.kuukar_sensors as kuukar_sensors
import kuukar.kuukar_environment as kuukar_environment
import kuukar.kuukar_cv as kuukar_cv
from kuukar.kuukar_config import SERIAL_MCU
from kuukar.kuukar_config import SERIAL_MCU, SERIAL_DRV
class kuukar:
def __init__(self) -> None:
self.mcu = telemetrix_rpi_pico.TelemetrixRpiPico(
com_port=SERIAL_MCU, sleep_tune=0.001)
self.sensors = kuukar_sensors.sensors(self.mcu)
self.drv = telemetrix_rpi_pico.TelemetrixRpiPico(
com_port=SERIAL_DRV, sleep_tune=0.001)
self.sensors = kuukar_sensors.sensors(self.mcu, self.drv)
self.leds = kuukar_leds.leds(self.mcu)
self.motion = kuukar_motion.motion(
self.mcu, self.leds, self.sensors)
self.mcu, self.drv, 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)
self.cv = kuukar_cv.cv(self.lcd, self.leds)

View File

@ -3,16 +3,13 @@ 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]

View File

@ -1,15 +1,17 @@
SERIAL_MCU = "/dev/serial/by-id/usb-Raspberry_Pi_Pico_E6609CB2D3846033-if00"
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_LCD = "/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0"
# Sonar Pin
SONAR_ADC_PIN = 1
# HC Sonar Pins
SONAR_1_ADC_PIN = 0
SONAR_2_ADC_PIN = 1
SONAR_3_ADC_PIN = 2
# Motor Pins
MOTOR_FL_F = 6
MOTOR_FL_R = 7
MOTOR_FR_F = 9
MOTOR_FR_R = 8
MOTOR_FL_F = 7
MOTOR_FL_R = 6
MOTOR_FR_F = 8
MOTOR_FR_R = 9
MOTOR_RL_F = 10
MOTOR_RL_R = 11
MOTOR_RR_F = 13
@ -18,6 +20,8 @@ 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
@ -28,6 +32,6 @@ LEFT_SIGNAL_LEDS = [4, 5, 6, 7]
RIGHT_SIGNAL_LEDS = [0, 1, 14, 15]
REVERSE_LEDS = [11, 10, 9]
LIGHT_SENSOR_PIN = 0
LIGHT_SENSOR_PIN = 2
DHT22_PIN = 3
DHT22_PIN = 14

View File

@ -1,14 +1,13 @@
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, motion: motion) -> None:
self.motion = motion
def __init__(self, lcd: lcd, leds: leds) -> None:
self.lcd = lcd
self.leds = leds
self.camera = cv2.VideoCapture(0)
@ -17,9 +16,7 @@ class cv:
def _cv_loop(self):
while True:
img = self.capture_image()
stop_signs = self.detect_sign(img)
if len(stop_signs) >= 1: # Stop Sign Found~!
self.motion.stop()
stop_sign = self.detect_sign(img)
def get_road_curve(self, frame):
pass
@ -31,6 +28,6 @@ class cv:
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

View File

@ -8,7 +8,6 @@ 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
@ -19,14 +18,14 @@ class environment:
while True:
if self.sensors.get_temperature() > 38.0:
if not self.t_alerted:
self.lcd.play_video("keke_hurt")
#self.lcd.play_video("keke_died")
self.t_alerted = True
else:
self.t_alerted = False
if self.sensors.get_humidity_pct() > 80.0:
if self.sensors.get_humidity_pct() > 60.0:
if not self.h_alerted:
self.lcd.play_video("kanon_hurt")
#self.lcd.play_video("keke_cute_noise")
self.h_alerted = True
else:
self.h_alerted = False

View File

@ -51,8 +51,7 @@ 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(
@ -62,5 +61,5 @@ class lcd:
self.nextion.send_command(
f"brightnessTXT.txt=\"{self.sensors.get_brightness_pct()}\"")
self.nextion.send_command(
f"sonarTXT.txt=\"{self.sensors.sonar_get_distance()}\"")
f"sonarTXT.txt=\"{int(self.sensors.sonar_get_distance(2))} / {int(self.sensors.sonar_get_distance(1))} / {int(self.sensors.sonar_get_distance(0))}\"")
sleep(1)

View File

@ -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):

View File

@ -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, -int(speed))
self.drv.pwm_write(forward_pin, 0)
self.drv.pwm_write(reverse_pin, -int(speed))
elif speed >= 0:
self.mcu.pwm_write(forward_pin, int(speed))
self.mcu.pwm_write(reverse_pin, 0)
self.drv.pwm_write(forward_pin, int(speed))
self.drv.pwm_write(reverse_pin, 0)
def drive(self, speed: int):
self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
@ -71,76 +72,23 @@ 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)
@ -163,12 +111,43 @@ class motion:
while True:
if self.roaming:
self.__roam_handle()
time.sleep(0.1)
time.sleep(0.3)
def __roam_handle(self):
if self.sensors.sonar_get_distance() < 900:
self.turn(32, 2600)
self.drive(25)
sensitivity = 35
turn_speed = 40
drive_speed = 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()

View File

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

Binary file not shown.

Before

Width:  |  Height:  |  Size: 38 KiB

104
utils.py
View File

@ -1,104 +0,0 @@
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

BIN
vid1.mp4

Binary file not shown.