diff --git a/ColorPicker.py b/ColorPicker.py deleted file mode 100644 index 777dfd6..0000000 --- a/ColorPicker.py +++ /dev/null @@ -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() diff --git a/KeyPressModule.py b/KeyPressModule.py deleted file mode 100644 index a1a40f3..0000000 --- a/KeyPressModule.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/LaneDetectionModule.py b/LaneDetectionModule.py deleted file mode 100644 index b36a416..0000000 --- a/LaneDetectionModule.py +++ /dev/null @@ -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) - - - - - diff --git a/MainRobotLane.py b/MainRobotLane.py deleted file mode 100644 index a48ca5b..0000000 --- a/MainRobotLane.py +++ /dev/null @@ -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: ")) diff --git a/RobotMain2.py b/RobotMain2.py deleted file mode 100644 index a93a1e7..0000000 --- a/RobotMain2.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/WebcamModule.py b/WebcamModule.py deleted file mode 100644 index 0a56138..0000000 --- a/WebcamModule.py +++ /dev/null @@ -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) \ No newline at end of file diff --git a/app.py b/app.py index b7ba278..8f463ca 100644 --- a/app.py +++ b/app.py @@ -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) diff --git a/classes_OLIPY.png b/classes_OLIPY.png deleted file mode 100644 index 3c51a9c..0000000 Binary files a/classes_OLIPY.png and /dev/null differ diff --git a/kuukar/kuukar.py b/kuukar/kuukar.py index 776fbd2..4c622a5 100644 --- a/kuukar/kuukar.py +++ b/kuukar/kuukar.py @@ -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) diff --git a/kuukar/kuukar_collision.py b/kuukar/kuukar_collision.py index 1d3a11d..3a423eb 100644 --- a/kuukar/kuukar_collision.py +++ b/kuukar/kuukar_collision.py @@ -3,18 +3,15 @@ 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") \ No newline at end of file diff --git a/kuukar/kuukar_config.py b/kuukar/kuukar_config.py index 32d2dc8..4e5f565 100644 --- a/kuukar/kuukar_config.py +++ b/kuukar/kuukar_config.py @@ -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 diff --git a/kuukar/kuukar_cv.py b/kuukar/kuukar_cv.py index f78fa98..fc2e8cc 100644 --- a/kuukar/kuukar_cv.py +++ b/kuukar/kuukar_cv.py @@ -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,20 +16,18 @@ 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 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 diff --git a/kuukar/kuukar_environment.py b/kuukar/kuukar_environment.py index a57a571..586b3ef 100644 --- a/kuukar/kuukar_environment.py +++ b/kuukar/kuukar_environment.py @@ -8,25 +8,24 @@ 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_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 @@ -35,4 +34,4 @@ class environment: self.leds.set_headlights(True) else: self.leds.set_headlights(False) - sleep(1) + sleep(1) \ No newline at end of file diff --git a/kuukar/kuukar_lcd.py b/kuukar/kuukar_lcd.py index f8963e6..49ce19b 100644 --- a/kuukar/kuukar_lcd.py +++ b/kuukar/kuukar_lcd.py @@ -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) diff --git a/kuukar/kuukar_leds.py b/kuukar/kuukar_leds.py index d281d77..27d46ef 100644 --- a/kuukar/kuukar_leds.py +++ b/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): diff --git a/kuukar/kuukar_motion.py b/kuukar/kuukar_motion.py index 6e628fd..fc29a1c 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, -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 - if not self.roaming: + 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() \ No newline at end of file diff --git a/kuukar/kuukar_sensors.py b/kuukar/kuukar_sensors.py index ad163ce..797030b 100644 --- a/kuukar/kuukar_sensors.py +++ b/kuukar/kuukar_sensors.py @@ -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.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 = 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) - 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 diff --git a/kuukar/kuukar_voice.py.disabled b/kuukar/kuukar_voice.py similarity index 100% rename from kuukar/kuukar_voice.py.disabled rename to kuukar/kuukar_voice.py diff --git a/packages_OLIPY.png b/packages_OLIPY.png deleted file mode 100644 index 6369cce..0000000 Binary files a/packages_OLIPY.png and /dev/null differ diff --git a/utils.py b/utils.py deleted file mode 100644 index 04175ac..0000000 --- a/utils.py +++ /dev/null @@ -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 diff --git a/vid1.mp4 b/vid1.mp4 deleted file mode 100644 index 88c5042..0000000 Binary files a/vid1.mp4 and /dev/null differ