From cc8b01d5749279e47c1b3bd5ad2d17971e6f91cf Mon Sep 17 00:00:00 2001 From: Siwat Sirichai Date: Tue, 13 Dec 2022 21:49:17 +0700 Subject: [PATCH] finalize code --- MainRobotLane.py | 35 +++++++++++++++++++---------------- app.py | 21 +-------------------- kuukar/kuukar.py | 2 +- kuukar/kuukar_collision.py | 9 ++++++--- kuukar/kuukar_config.py | 1 - kuukar/kuukar_cv.py | 22 ++++++++++++---------- kuukar/kuukar_environment.py | 7 ++++--- kuukar/kuukar_lcd.py | 3 ++- kuukar/kuukar_leds.py | 2 +- kuukar/kuukar_motion.py | 36 ++++++++++++++++++++---------------- 10 files changed, 66 insertions(+), 72 deletions(-) diff --git a/MainRobotLane.py b/MainRobotLane.py index e6fa26b..a48ca5b 100644 --- a/MainRobotLane.py +++ b/MainRobotLane.py @@ -1,3 +1,4 @@ +from time import perf_counter import cv2 from kuukar.kuukar import kuukar from LaneDetectionModule import getLaneCurve @@ -5,7 +6,6 @@ from utils import initializeTrackbars import WebcamModule from threading import Thread global speed, sen, car, delay -from time import perf_counter sen = 1 speed = 0 delay = 0.15 @@ -14,27 +14,34 @@ 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 + 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) + 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) @@ -43,7 +50,3 @@ if __name__ == '__main__': speed = float(input("speed: ")) sen = float(input("sen: ")) delay = float(input("delay: ")) - - - - diff --git a/app.py b/app.py index a9f1aef..b7ba278 100644 --- a/app.py +++ b/app.py @@ -1,24 +1,5 @@ -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: diff --git a/kuukar/kuukar.py b/kuukar/kuukar.py index 85a22c9..776fbd2 100644 --- a/kuukar/kuukar.py +++ b/kuukar/kuukar.py @@ -22,4 +22,4 @@ class kuukar: 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) \ No newline at end of file + self.cv = kuukar_cv.cv(self.lcd, self.leds, self.motion) diff --git a/kuukar/kuukar_collision.py b/kuukar/kuukar_collision.py index 3a423eb..1d3a11d 100644 --- a/kuukar/kuukar_collision.py +++ b/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") \ No newline at end of file + self.lcd.play_video("keke_hurt") diff --git a/kuukar/kuukar_config.py b/kuukar/kuukar_config.py index 45cc457..32d2dc8 100644 --- a/kuukar/kuukar_config.py +++ b/kuukar/kuukar_config.py @@ -31,4 +31,3 @@ REVERSE_LEDS = [11, 10, 9] LIGHT_SENSOR_PIN = 0 DHT22_PIN = 3 - diff --git a/kuukar/kuukar_cv.py b/kuukar/kuukar_cv.py index 1f24c36..f78fa98 100644 --- a/kuukar/kuukar_cv.py +++ b/kuukar/kuukar_cv.py @@ -1,34 +1,36 @@ 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) - #threading.Thread(target=self._cv_loop).start() + self.camera = cv2.VideoCapture(0) + threading.Thread(target=self._cv_loop).start() 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 - diff --git a/kuukar/kuukar_environment.py b/kuukar/kuukar_environment.py index ca6a3fa..a57a571 100644 --- a/kuukar/kuukar_environment.py +++ b/kuukar/kuukar_environment.py @@ -8,17 +8,18 @@ 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 @@ -34,4 +35,4 @@ class environment: self.leds.set_headlights(True) else: self.leds.set_headlights(False) - sleep(1) \ No newline at end of file + sleep(1) diff --git a/kuukar/kuukar_lcd.py b/kuukar/kuukar_lcd.py index 9b8b527..f8963e6 100644 --- a/kuukar/kuukar_lcd.py +++ b/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( diff --git a/kuukar/kuukar_leds.py b/kuukar/kuukar_leds.py index 27d46ef..d281d77 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 8cd00f8..6e628fd 100644 --- a/kuukar/kuukar_motion.py +++ b/kuukar/kuukar_motion.py @@ -71,17 +71,16 @@ 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) @@ -119,16 +118,22 @@ class motion: ''' def skt_drive(self, speed: float, turn: float, t=0): - speed *=100 - turn *=100 + 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 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)) @@ -136,7 +141,6 @@ class motion: #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) @@ -162,9 +166,9 @@ class motion: time.sleep(0.1) def __roam_handle(self): - if self.sensors.sonar_get_distance()<900: - self.turn(32,2600) + if self.sensors.sonar_get_distance() < 900: + self.turn(32, 2600) self.drive(25) if not self.roaming: - self.stop() \ No newline at end of file + self.stop()