167 lines
5.9 KiB
Python
167 lines
5.9 KiB
Python
from random import randint
|
|
from kuukar.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.kuukar_leds import leds
|
|
from kuukar.kuukar_sensors import sensors
|
|
from telemetrix_rpi_pico import telemetrix_rpi_pico
|
|
import threading
|
|
|
|
|
|
class motion:
|
|
|
|
roaming = False
|
|
|
|
def __init__(self, mcu: telemetrix_rpi_pico.TelemetrixRpiPico, leds: leds, sensors: sensors) -> None:
|
|
self.mcu = mcu
|
|
self.leds = leds
|
|
self.sensors = sensors
|
|
self.roam_thread = threading.Thread(target=self.__roam_looper)
|
|
self.roam_thread.start()
|
|
|
|
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)
|
|
|
|
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))
|
|
elif speed >= 0:
|
|
self.mcu.pwm_write(forward_pin, int(speed))
|
|
self.mcu.pwm_write(reverse_pin, 0)
|
|
|
|
def drive(self, speed: int):
|
|
self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
|
|
self.motor_write(MOTOR_FR_F, MOTOR_FR_R, speed)
|
|
self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
|
|
self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)
|
|
|
|
def turn(self, speed: int, duration: float):
|
|
if speed > 0:
|
|
self.leds.set_left_signal_led(False)
|
|
self.leds.set_right_signal_led(True)
|
|
elif speed < 0:
|
|
self.leds.set_left_signal_led(True)
|
|
self.leds.set_right_signal_led(False)
|
|
self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
|
|
self.motor_write(MOTOR_FR_F, MOTOR_FR_R, -speed)
|
|
self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
|
|
self.motor_write(MOTOR_RR_F, MOTOR_RR_R, -speed)
|
|
time.sleep(duration/1000.0)
|
|
self.stop()
|
|
|
|
def turn_cont(self, speed: int):
|
|
if speed > 0:
|
|
self.leds.set_right_signal_led(True)
|
|
self.leds.set_left_signal_led(False)
|
|
elif speed < 0:
|
|
self.leds.set_right_signal_led(False)
|
|
self.leds.set_left_signal_led(True)
|
|
self.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
|
|
self.motor_write(MOTOR_FR_F, MOTOR_FR_R, -speed)
|
|
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)
|
|
self.leds.set_left_signal_led(False)
|
|
elif dir < 0:
|
|
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:
|
|
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 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)
|
|
|
|
|
|
|
|
def stop(self):
|
|
self.leds.set_left_signal_led(False)
|
|
self.leds.set_right_signal_led(False)
|
|
self.leds.set_reverse_led(False)
|
|
self.drive(0)
|
|
|
|
def roam_start(self):
|
|
self.drive(25)
|
|
self.roaming = True
|
|
|
|
def roam_stop(self):
|
|
self.roaming = False
|
|
time.sleep(0.5)
|
|
self.stop()
|
|
|
|
def is_roaming(self) -> bool:
|
|
return self.roaming
|
|
|
|
def __roam_looper(self):
|
|
while True:
|
|
if self.roaming:
|
|
self.__roam_handle()
|
|
time.sleep(0.3)
|
|
|
|
def __roam_handle(self):
|
|
time.sleep(1)
|
|
if not self.roaming:
|
|
self.stop() |