diff --git a/MainRobotLane.py b/MainRobotLane.py index a48ca5b..e6fa26b 100644 --- a/MainRobotLane.py +++ b/MainRobotLane.py @@ -1,4 +1,3 @@ -from time import perf_counter import cv2 from kuukar.kuukar import kuukar from LaneDetectionModule import getLaneCurve @@ -6,6 +5,7 @@ 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,34 +14,27 @@ 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) @@ -50,3 +43,7 @@ if __name__ == '__main__': speed = float(input("speed: ")) sen = float(input("sen: ")) delay = float(input("delay: ")) + + + + diff --git a/app.py b/app.py index b7ba278..a9f1aef 100644 --- a/app.py +++ b/app.py @@ -1,5 +1,24 @@ -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: diff --git a/kuukar/kuukar.py b/kuukar/kuukar.py index 776fbd2..85a22c9 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, self.motion) + self.cv = kuukar_cv.cv(self.lcd,self.leds) \ No newline at end of file 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..45cc457 100644 --- a/kuukar/kuukar_config.py +++ b/kuukar/kuukar_config.py @@ -31,3 +31,4 @@ 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 f78fa98..1f24c36 100644 --- a/kuukar/kuukar_cv.py +++ b/kuukar/kuukar_cv.py @@ -1,36 +1,34 @@ 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) - 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_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..ca6a3fa 100644 --- a/kuukar/kuukar_environment.py +++ b/kuukar/kuukar_environment.py @@ -8,18 +8,17 @@ 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 @@ -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..9b8b527 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( 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..1094f12 100644 --- a/kuukar/kuukar_motion.py +++ b/kuukar/kuukar_motion.py @@ -71,16 +71,17 @@ 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) @@ -118,22 +119,16 @@ 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)) @@ -141,6 +136,7 @@ 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) @@ -163,12 +159,12 @@ class motion: while True: if self.roaming: self.__roam_handle() - time.sleep(0.1) + time.sleep(0.5) def __roam_handle(self): - if self.sensors.sonar_get_distance() < 900: - self.turn(32, 2600) + if self.sensors.sonar_get_distance()<500: + self.turn(30,2000) self.drive(25) - + time.sleep(0.5) if not self.roaming: - self.stop() + self.stop() \ No newline at end of file