from microbit import *
import math

compass.is_calibrated()
sleep(2000)
# -- COMMUNICATION - cannot be changed
def ispis(tekst):
    dd = len(tekst)
    if dd < 31:
        salji(tekst)
    else:
        salji(tekst[0:30] + "+")
        salji(tekst[30:dd])


def salji(tekst):
    duz = len(tekst)
    buf = bytearray(duz)
    for n in range(0, duz):
        bb = ord(tekst[n : n + 1])
        buf[n] = bb
    try:
        i2c.write(0x11, buf)
        sleep(duz * 4)
    except OSError:
        print("er: salji")

sleep(200)
ispis("CLS")
sleep(100)
#-------------------
pi = 3.1415
x1 = 57
y1 = 24
ispis("COMPASS;0;0")

ispis("CIR;57;24;20;C")
ispis("CIR;57;24;22;C")
ispis("CIR;57;24;1;C;C")
sleep(100)

while True:
    xx = compass.heading()
    n = 360 - xx
    rad = (pi / 180) * n
    y2 = math.sin(rad)*18+y1
    x2 = math.cos(rad)*18+x1
    ispis("LIN;"+str(x1)+";"+str(y1)+";"+str(x2)+";"+str(y2)+";C")
    sleep(100)
    ispis("CIR;57;24;1;C;C")
    sleep(400)        
    ispis("LIN;"+str(x1)+";"+str(y1)+";"+str(x2)+";"+str(y2)+";B")
    sleep(100)