From 6511984484909d6882bcf89e5d6910e9ff094ed9 Mon Sep 17 00:00:00 2001 From: Siwat Sirichai Date: Wed, 9 Nov 2022 22:59:21 +0700 Subject: [PATCH] implement skew drive --- kuukar.service | 11 +++++++++++ kuukar/kuukar_lcd.py | 7 +++++-- kuukar/kuukar_leds.py | 9 ++++++--- kuukar/kuukar_motion.py | 25 +++++++++++++++++++++++++ 4 files changed, 47 insertions(+), 5 deletions(-) create mode 100644 kuukar.service diff --git a/kuukar.service b/kuukar.service new file mode 100644 index 0000000..b565255 --- /dev/null +++ b/kuukar.service @@ -0,0 +1,11 @@ +[Unit] +After=network.target +Description=KuuKar Primary Application + +[Service] +Type=idle +User=pi +ExecStart=/usr/bin/python3 /home/pi/kuukar-rpi/app.py + +[Install] +WantedBy=multi-user.target \ No newline at end of file diff --git a/kuukar/kuukar_lcd.py b/kuukar/kuukar_lcd.py index bed2bd1..7af9ab1 100644 --- a/kuukar/kuukar_lcd.py +++ b/kuukar/kuukar_lcd.py @@ -38,8 +38,10 @@ class lcd: if page == 1 and component_id == 2 and press_type == '01': if self.nextion.get_attribute("aiTG.val"): print("AI Drive Activated") + self.motion.roam_start() else: print("AI Drive Deactivated") + self.motion.roam_stop() elif page == 3: if component_id == 2 and press_type == '00': x = self.nextion.get_attribute("joystick.x") @@ -47,7 +49,8 @@ class lcd: speed = 100 - (y - 60) / (205 - 60) * 200 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) def _sensor_screen_updator(self): while True: self.nextion.send_command( @@ -57,5 +60,5 @@ class lcd: self.nextion.send_command( f"brightnessTXT.txt=\"{self.sensors.get_brightness_pct()}\"") self.nextion.send_command( - f"sonarTXT.txt=\"{int(self.sensors.sonar_get_distance(0))} / {int(self.sensors.sonar_get_distance(1))} / {int(self.sensors.sonar_get_distance(2))}\"") + 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 931dfe2..e819a12 100644 --- a/kuukar/kuukar_leds.py +++ b/kuukar/kuukar_leds.py @@ -18,9 +18,11 @@ 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 = [5, 5, 25] #Keke Image Color + self.__update_leds() def set_headlights(self, state: bool): - headlight = True + self.headlight = state self.__update_leds() def set_left_signal_led(self, state: bool): @@ -40,11 +42,12 @@ class leds: self.__update_leds() def __update_leds(self): - self.mcu.neopixel_fill(r=self.ambient_light[0], g=self.ambient_light[1], b=self.ambient_light[2]) + self.mcu.neopixel_fill( + r=self.ambient_light[0], g=self.ambient_light[1], b=self.ambient_light[2]) if self.headlight: for led in HEADLIGHT_LEDS: self.mcu.neo_pixel_set_value(led, self.headlight * 255, self.headlight * 255, - self.headlight * 255) + self.headlight * 255) if self.left_signal: for led in LEFT_SIGNAL_LEDS: self.mcu.neo_pixel_set_value(led, 255, 0, 0) diff --git a/kuukar/kuukar_motion.py b/kuukar/kuukar_motion.py index 376c529..6af9846 100644 --- a/kuukar/kuukar_motion.py +++ b/kuukar/kuukar_motion.py @@ -60,6 +60,28 @@ class motion: self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed) self.motor_write(MOTOR_RR_F, MOTOR_RR_R, -speed) + def drive_skew(self, speed: float, dir: float): + if dir > 0: + self.leds.set_right_signal_led(True) + elif dir < 0: + self.leds.set_left_signal_led(True) + + lm_speed = (100+dir/2)*speed/100 + if lm_speed>99: + 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; + + 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 stop(self): self.leds.set_left_signal_led(False) self.leds.set_right_signal_led(False) @@ -72,6 +94,9 @@ class motion: def roam_stop(self): self.roaming = False + def is_roaming(self) -> bool: + return self.roaming + def __roam_looper(self): while True: if self.roaming: