project-KuuKar
/
kuukar-rpi
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: project-KuuKar:lite
Branches Tags
project-KuuKar:main
project-KuuKar:lite
..
compare: project-KuuKar:main
Branches Tags
project-KuuKar:lite
project-KuuKar:main

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
Show Stats Expand all files Collapse all files

55
ColorPicker.py
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()

27
KeyPressModule.py
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()

103
LaneDetectionModule.py
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)

52
MainRobotLane.py
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: "))

22
RobotMain2.py
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()

14
WebcamModule.py
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 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() car = kuukar()
while True: while True:
temp = car.sensors.get_temperature() temp = car.sensors.get_temperature()
humid = car.sensors.get_humidity_pct() humid = car.sensors.get_humidity_pct()
dist = car.sensors.sonar_get_distance() dist1 = car.sensors.sonar_get_distance(0)
print(f'temp: {temp}, humidity: {humid}, dist: {dist}') 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) time.sleep(1)

BIN
classes_OLIPY.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 242 KiB

10
kuukar/kuukar.py
View File

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

5
kuukar/kuukar_collision.py
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_lcd import lcd
from kuukar.kuukar_leds import leds from kuukar.kuukar_leds import leds
class collision: class collision:
def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico, lcd: lcd, leds: leds) -> None: def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico, lcd: lcd, leds: leds) -> None:
self.mcu = mcu self.mcu = mcu
self.lcd = lcd self.lcd = lcd
self.leds = leds self.leds = leds
if COLLISION_ENABLE: if COLLISION_ENABLE:
self.mcu.set_pin_mode_digital_input( self.mcu.set_pin_mode_digital_input(COLLISION_DETECTOR_PIN, self.collision_handle)
COLLISION_DETECTOR_PIN, self.collision_handle)
def collision_handle(self, data): def collision_handle(self, data):
val = data[2] val = data[2]

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

9
kuukar/kuukar_cv.py
View File

@ -1,14 +1,13 @@
from kuukar.kuukar_lcd import lcd from kuukar.kuukar_lcd import lcd
from kuukar.kuukar_leds import leds from kuukar.kuukar_leds import leds
from kuukar.kuukar_motion import motion
import threading import threading
import multiprocessing import multiprocessing
import cv2 import cv2
class cv: class cv:
def __init__(self, lcd: lcd, leds: leds, motion: motion) -> None: def __init__(self, lcd: lcd, leds: leds) -> None:
self.motion = motion
self.lcd = lcd self.lcd = lcd
self.leds = leds self.leds = leds
self.camera = cv2.VideoCapture(0) self.camera = cv2.VideoCapture(0)
@ -17,9 +16,7 @@ class cv:
def _cv_loop(self): def _cv_loop(self):
while True: while True:
img = self.capture_image() img = self.capture_image()
stop_signs = self.detect_sign(img) stop_sign = self.detect_sign(img)
if len(stop_signs) >= 1: # Stop Sign Found~!
self.motion.stop()
def get_road_curve(self, frame): def get_road_curve(self, frame):
pass pass

7
kuukar/kuukar_environment.py
View File

@ -8,7 +8,6 @@ from kuukar.kuukar_sensors import sensors
class environment: class environment:
t_alerted = False t_alerted = False
h_alerted = False h_alerted = False
def __init__(self, lcd: lcd, leds: leds, sensors: sensors) -> None: def __init__(self, lcd: lcd, leds: leds, sensors: sensors) -> None:
self.lcd = lcd self.lcd = lcd
self.leds = leds self.leds = leds
@ -19,14 +18,14 @@ class environment:
while True: while True:
if self.sensors.get_temperature() > 38.0: if self.sensors.get_temperature() > 38.0:
if not self.t_alerted: if not self.t_alerted:
self.lcd.play_video("keke_hurt") #self.lcd.play_video("keke_died")
self.t_alerted = True self.t_alerted = True
else: else:
self.t_alerted = False 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: if not self.h_alerted:
self.lcd.play_video("kanon_hurt") #self.lcd.play_video("keke_cute_noise")
self.h_alerted = True self.h_alerted = True
else: else:
self.h_alerted = False self.h_alerted = False

3
kuukar/kuukar_lcd.py
View File

@ -52,7 +52,6 @@ class lcd:
print(f"Setting speed to {speed} and turn angle to {turn}") print(f"Setting speed to {speed} and turn angle to {turn}")
if not self.motion.is_roaming(): if not self.motion.is_roaming():
self.motion.drive_skew(speed,turn) self.motion.drive_skew(speed,turn)
def _sensor_screen_updator(self): def _sensor_screen_updator(self):
while True: while True:
self.nextion.send_command( self.nextion.send_command(
@ -62,5 +61,5 @@ class lcd:
self.nextion.send_command( self.nextion.send_command(
f"brightnessTXT.txt=\"{self.sensors.get_brightness_pct()}\"") f"brightnessTXT.txt=\"{self.sensors.get_brightness_pct()}\"")
self.nextion.send_command( 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) sleep(1)

109
kuukar/kuukar_motion.py
View File

@ -11,8 +11,9 @@ class motion:
roaming = False 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.mcu = mcu
self.drv = drv
self.leds = leds self.leds = leds
self.sensors = sensors self.sensors = sensors
self.roam_thread = threading.Thread(target=self.__roam_looper) 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, 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] MOTOR_FR_R, MOTOR_RL_F, MOTOR_RL_R, MOTOR_RR_F, MOTOR_RR_R]
for drive_pin in drive_pins: 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): def motor_write(self, forward_pin: int, reverse_pin: int, speed: int):
if speed < 0: if speed < 0:
self.mcu.pwm_write(forward_pin, 0) self.drv.pwm_write(forward_pin, 0)
self.mcu.pwm_write(reverse_pin, -int(speed)) self.drv.pwm_write(reverse_pin, -int(speed))
elif speed >= 0: elif speed >= 0:
self.mcu.pwm_write(forward_pin, int(speed)) self.drv.pwm_write(forward_pin, int(speed))
self.mcu.pwm_write(reverse_pin, 0) self.drv.pwm_write(reverse_pin, 0)
def drive(self, speed: int): def drive(self, speed: int):
self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed) self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
@ -71,6 +72,7 @@ class motion:
self.leds.set_right_signal_led(False) self.leds.set_right_signal_led(False)
self.leds.set_left_signal_led(True) self.leds.set_left_signal_led(True)
lm_speed = (100+dir/2)*speed/100 lm_speed = (100+dir/2)*speed/100
if lm_speed>99: if lm_speed>99:
lm_speed = 99 lm_speed = 99
@ -80,67 +82,13 @@ class motion:
if rm_speed > 99: if rm_speed > 99:
rm_speed = 99 rm_speed = 99
elif 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_FL_F, MOTOR_FL_R, lm_speed)
self.motor_write(MOTOR_FR_F, MOTOR_FR_R, rm_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_RL_F, MOTOR_RL_R, lm_speed)
self.motor_write(MOTOR_RR_F, MOTOR_RR_R, rm_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): def stop(self):
self.leds.set_left_signal_led(False) self.leds.set_left_signal_led(False)
self.leds.set_right_signal_led(False) self.leds.set_right_signal_led(False)
@ -163,12 +111,43 @@ class motion:
while True: while True:
if self.roaming: if self.roaming:
self.__roam_handle() self.__roam_handle()
time.sleep(0.1) time.sleep(0.3)
def __roam_handle(self): def __roam_handle(self):
if self.sensors.sonar_get_distance() < 900: sensitivity = 35
self.turn(32, 2600) turn_speed = 40
self.drive(25) 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: if not self.roaming:
self.stop() self.stop()

27
kuukar/kuukar_sensors.py
View File

@ -1,25 +1,32 @@
from telemetrix_rpi_pico import telemetrix_rpi_pico 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: 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 __temperature = 0
__humidity = 0 __humidity = 0
__brightness = 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 = 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_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: def sonar_get_distance(self, id: int) -> float:
return self.__sonar_distances return self.__sonar_distances[id]
def __sonar_callback(self, data): def __sonar_callback(self, data):
pin = data[1]
distance = data[2] 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: def get_brightness_pct(self) -> float:
return self.__brightness return self.__brightness
@ -34,7 +41,7 @@ class sensors:
self.__brightness = (4096-float(data[2]))/4096*100 self.__brightness = (4096-float(data[2]))/4096*100
def __dht22_callback(self, data): def __dht22_callback(self, data):
humidity = data[2] humidity = data[3]
temperature = data[3] temperature = data[4]
self.__humidity = humidity self.__humidity = humidity
self.__temperature = temperature self.__temperature = temperature

0
kuukar/kuukar_voice.py.disabled → kuukar/kuukar_voice.py
View File

BIN
packages_OLIPY.png
View File

Binary file not shown.
Side by Side

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
View File

Binary file not shown.