# L_19mai - Control servo directie Aqua Drone.
# Aplicatia MIT App Inventor  trimte start 
# pentru viraje. Acestea dureaza 2 secunde dupa care se revine la
# mers inainte
# -----Conexiuni -------
# Servo - GP13 fizic pin 17
# Vcc de la servo se conecteaza la Pico pin 40 (5V USB)
#-----------------------
import machine
from machine import Pin, UART, PWM
import time

#-----LED RGB ------------
red=Pin(6,Pin.OUT)
green=Pin(7,Pin.OUT)
blue=Pin(8,Pin.OUT)

servo = PWM(Pin(13), freq=50, duty_u16=32)      # pwm servo 
red.value(0)    # oprit
green.value(0)  # desactiv
blue.value(0)


servo.duty_u16(0) # pwm oprit
                    
ser = UART(1, baudrate=9600, tx=Pin(4), rx=Pin(5)) # RP2040 
 
while 1:
    c=ser.read(1)
    
    if c == b'1':    # viraj dreapta
        
        red.value(1) # aprind rosu
        
        servo.duty_u16(2600)
        
        time.sleep(0.3)
        red.value(0)
        time.sleep(2)
        servo.duty_u16(3700) # revine la mers inainte
        
    if c == b'2':    # mers inainte
        
        green.value(1) # aprind rosu
        
        servo.duty_u16(3700)
        time.sleep(0.3)
        green.value(0)
       
        
    if c == b'3':    # viraj stanga
        
        blue.value(1) # aprind rosu
        
        servo.duty_u16(4800)
        time.sleep(0.3)
        blue.value(0)
        time.sleep(2)
        servo.duty_u16(3700) # revine la mers inainte