61 lines
2.0 KiB
Python
61 lines
2.0 KiB
Python
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
|
|
import time
|
|
from kuukar_leds import leds
|
|
from kuukar_sensors import sensors
|
|
from telemetrix_rpi_pico import telemetrix_rpi_pico
|
|
import threading
|
|
|
|
class motion:
|
|
|
|
roaming = False
|
|
|
|
def __init__(self, driver_board: telemetrix_rpi_pico.TelemetrixRpiPico, leds: leds, sensors: sensors) -> None:
|
|
self.driver = driver_board
|
|
self.leds = leds
|
|
self.sensors = sensors
|
|
self.roam_thread = threading.Thread(target = self.roam_loop)
|
|
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.driver.set_pin_mode_pwm_output(drive_pin)
|
|
|
|
def motor_write(self, forward_pin: int,reverse_pin: int, speed: int):
|
|
if speed < 0:
|
|
self.driver.pwm_write(forward_pin,0)
|
|
self.driver.pwm_write(reverse_pin,-speed)
|
|
elif speed >= 0:
|
|
self.driver.pwm_write(forward_pin,speed)
|
|
self.driver.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):
|
|
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)
|
|
self.stop()
|
|
|
|
def stop(self):
|
|
self.drive(0)
|
|
|
|
def roam_start(self):
|
|
self.roaming = True
|
|
|
|
def roam_stop(self):
|
|
self.roaming = False
|
|
|
|
def __roam_looper__(self):
|
|
while True:
|
|
if self.roaming:
|
|
self.__roam_handle__()
|
|
|
|
def __roam_handle__(self):
|
|
#TODO Implement Roaming Function
|
|
pass |