from time import sleep
import math
from pyjuni.robodog import *
from pyjuni.deflib import *
from pyjuni.jkeyevent import *

height = 60

if __name__ == "__main__":
    robodog = RoboDog()
    jkey = JKeyEvent()  
    sleep(0.5)
    
    robodog.Open()
    while not jkey.isKeyEscPressed():
        [_, pitch] = robodog.get_tilt()
        delta = int(math.tan(math.radians(pitch))*55)
        delta = DefLib.constrain(delta, -30, 30)
        robodog.leg(LEFT_FRONT, height-delta, 0, 50, 50)
        robodog.leg(LEFT_BACK, height+delta, 0, 50, 50)
        robodog.leg(RIGHT_BACK, height+delta, 0, 50, 50)
        robodog.leg(RIGHT_FRONT, height-delta, 0, 50, 50)
        sleep(0.01)

    robodog.Close()