import time
from machine import I2C, Pin, ADC
from time import sleep
from pico_i2c_lcd import I2cLcd
from servo import Servo
time.sleep(0.1) # Wait for USB to become ready

# LCD i2C
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
I2C_ADDR = i2c.scan()[0]
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)
servo_ = Servo(pin_id=27)
pot_1 = ADC(28)
pot_2 = ADC(26)
    

def map_value(value, in_min, in_max, out_min, out_max):
    return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


last_string = ""
while True:
  angle = int(map_value(pot_1.read_u16(), 0, 65535, 0, 180))
  multiplier = map_value(pot_2.read_u16(), 0, 65535, 0, 1)

  angle *= multiplier
  servo_.write(angle)
  angle = int(angle)

  if angle > 180:
    angle = 180

  text_string = "Angle: " + str(int(angle)) + "\nMultiplier: " + str(round(multiplier, 1))
  if last_string != text_string:
    lcd.clear()
    lcd.putstr(text_string)
    last_string = text_string

  time.sleep(1/60)