26 lines
725 B
Python
26 lines
725 B
Python
|
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)
|