from telemetrix_rpi_pico import telemetrix_rpi_pico
import time
import kuukar.kuukar_config as config

global mcu,drv,distance
mcu = telemetrix_rpi_pico.TelemetrixRpiPico("COM4")
drv = telemetrix_rpi_pico.TelemetrixRpiPico("COM20")

distance = 0
drv.set_pin_mode_pwm_output(config.MOTOR_RR_F)
drv.pwm_write(config.MOTOR_RR_F,30)

def sonarcallback(data):
    global mcu,drv,distance
    pin = data[1]
    distance = data[2]
        
mcu.set_pin_mode_sonar(config.SONAR_2_TRIG_PIN,config.SONAR_2_ECHO_PIN, sonarcallback)

while True:
    print(distance)
    if distance < 20.0 and distance != 0:
        drv.pwm_write(config.MOTOR_RR_F,0)
        print("STOP")
        time.sleep(1)
        drv.pwm_write(config.MOTOR_RR_F,30)