diff --git a/kuukar_config.py b/kuukar_config.py index 1b55413..d801b0a 100644 --- a/kuukar_config.py +++ b/kuukar_config.py @@ -1,4 +1,12 @@ SERIAL_DRIVER_BOARD = "/dev/serial/by-id/..." + +SONAR_1_TRIG_PIN = 0 +SONAR_1_ECHO_PIN = 1 +SONAR_2_TRIG_PIN = 2 +SONAR_2_ECHO_PIN = 3 +SONAR_3_TRIG_PIN = 4 +SONAR_3_ECHO_PIN = 5 + MOTOR_FL_F = 6 MOTOR_FL_R = 7 MOTOR_FR_F = 8 @@ -8,8 +16,14 @@ MOTOR_RL_R = 11 MOTOR_RR_F = 12 MOTOR_RR_R = 13 +# Full Speed 90 Degress Turn Time +TURN_TIME_FS_90DEG_MS = 1500 + SERIAL_AUX_BOARD = "/dev/serial/by-id/..." + COLLISION_DETECTOR_PIN = 2 + LEDS_DATA_PIN = 3 +LEDS_NUM = 15 + DHT22_PIN = 5 -LEDS_NUM = 15 \ No newline at end of file diff --git a/kuukar_leds.py b/kuukar_leds.py index 18803f0..3734d41 100644 --- a/kuukar_leds.py +++ b/kuukar_leds.py @@ -6,5 +6,5 @@ class leds: self.aux_board = aux_board self.aux_board.set_pin_mode_neopixel(LEDS_DATA_PIN,LEDS_NUM) - def flash(r: int,g: int,b: int,duration_ms: int): + def flash(self, r: int,g: int,b: int,duration_ms: int): pass \ No newline at end of file diff --git a/kuukar_motion.py b/kuukar_motion.py index c763493..ae85b87 100644 --- a/kuukar_motion.py +++ b/kuukar_motion.py @@ -1,4 +1,5 @@ -from kuukar_config import MOTOR_FL_F, MOTOR_FL_R, MOTOR_FR_F, MOTOR_FR_R, MOTOR_RL_F, MOTOR_RL_R, MOTOR_RR_F, MOTOR_RR_R +from random import randint +from kuukar_config import MOTOR_FL_F, MOTOR_FL_R, MOTOR_FR_F, MOTOR_FR_R, MOTOR_RL_F, MOTOR_RL_R, MOTOR_RR_F, MOTOR_RR_R, TURN_TIME_FS_90DEG_MS import time from kuukar_leds import leds from kuukar_sensors import sensors @@ -57,5 +58,31 @@ class motion: self.__roam_handle__() def __roam_handle__(self): - #TODO Implement Roaming Function - pass \ No newline at end of file + self.drive(50) + f_dist = self.sensors.sonar_get_distance(2) + if f_dist > 0 and f_dist < 20: # Close to collision, turn the vehicle + l_dist = self.sensors.sonar_get_distance + r_dist = self.sensors.sonar_get_distance + if l_dist < 20: #Left side is blocked + if r_dist < 20: #Both side are blocked, do a full 180deg turn + self.turn(100,2*TURN_TIME_FS_90DEG_MS) + self.drive(50) + time.sleep(3) + else: + #Right Side is Free, turn right + self.turn(100,TURN_TIME_FS_90DEG_MS) + self.drive(50) + time.sleep(3) + elif r_dist < 20: #Right side is blocked, left side is free, turn left + self.turn(-100,TURN_TIME_FS_90DEG_MS) + else: #Both Side are free, randomize a direction + right: bool = randint(0,1) + if right: + self.turn(100,TURN_TIME_FS_90DEG_MS) + self.drive(50) + time.sleep(3) + else: + self.turn(-100,TURN_TIME_FS_90DEG_MS) + self.drive(50) + time.sleep(3) + time.sleep(0.5) \ No newline at end of file diff --git a/kuukar_sensors.py b/kuukar_sensors.py index abc6c23..62adbaa 100644 --- a/kuukar_sensors.py +++ b/kuukar_sensors.py @@ -1,5 +1,43 @@ from telemetrix_rpi_pico import telemetrix_rpi_pico + +from kuukar_config import DHT22_PIN, SONAR_1_ECHO_PIN, SONAR_1_TRIG_PIN, SONAR_2_ECHO_PIN, SONAR_2_TRIG_PIN, SONAR_3_ECHO_PIN, SONAR_3_TRIG_PIN + + class sensors: + + __sonar_trig_pins__ = [SONAR_1_TRIG_PIN, SONAR_2_TRIG_PIN, SONAR_3_TRIG_PIN] + __sonar_distances__ = [0, 0, 0] + __temperature__ = 0 + __humidity__ = 0 + def __init__(self, aux_board: telemetrix_rpi_pico.TelemetrixRpiPico, driver_board: telemetrix_rpi_pico.TelemetrixRpiPico) -> None: self.aux = aux_board - self.driver = driver_board \ No newline at end of file + self.driver = driver_board + self.driver.set_pin_mode_sonar( + SONAR_1_TRIG_PIN, SONAR_1_ECHO_PIN, self.__sonar_callback__) + self.driver.set_pin_mode_sonar( + SONAR_2_TRIG_PIN, SONAR_2_ECHO_PIN, self.__sonar_callback__) + self.driver.set_pin_mode_sonar( + SONAR_3_TRIG_PIN, SONAR_3_ECHO_PIN, self.__sonar_callback__) + self.driver.set_pin_mode_dht(DHT22_PIN, self.__dht22_callback__) + + def sonar_get_distance(self, id: int) -> float: + return self.__sonar_distances__[id] + + def __sonar_callback__(self, data): + pin = data[1] + distance = data[2] + sonar_id = self.sonar_pins.index(pin) + self.__sonar_distances__[sonar_id] = distance + + def get_temperature(self) -> float: + return self.__temperature__ + + def get_humidity_pct(self) -> float: + return self.__humidity__ + + def __dht22_callback__(self,data): + humidity = data[3] + temperature = data[4] + self.__humidity__ = humidity + self.__temperature__ = temperature \ No newline at end of file