import time

from telemetrix_rpi_pico import telemetrix_rpi_pico
import kuukar.kuukar_motion as kuukar_motion
import kuukar.kuukar_sensors as kuukar_sensors
from kuukar.kuukar_config import SERIAL_MCU
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

mcu = telemetrix_rpi_pico.TelemetrixRpiPico()
motion = kuukar_motion.motion(mcu, None,  None)

motion.drive(30)

def rot_test():
    while True:
        speed = 30
        motion.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
        motion.motor_write(MOTOR_FR_F, MOTOR_FR_R, -speed)
        motion.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
        motion.motor_write(MOTOR_RR_F, MOTOR_RR_R, -speed)
        time.sleep(0.5)
        speed = -speed
        motion.motor_write(MOTOR_FL_F, MOTOR_FL_R, speed)
        motion.motor_write(MOTOR_FR_F, MOTOR_FR_R, -speed)
        motion.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed)
        motion.motor_write(MOTOR_RR_F, MOTOR_RR_R, -speed)
        time.sleep(0.5)