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


if __name__ == "__main__":
    leg_height = 0
    rb_vertical = 0
    rb_horizontal = 0
    bodyRotation = 0
    rotSum = 0
    robodog = RoboDog()
    jkey = JKeyEvent()  

    robodog.Open()

    runfile = robodog.rb_runfile("color_detect.py")
    print(runfile)
    sleep(0.5)
    robodog.leg_bend(60, 60, 60,60)
    while (not jkey.isKeyEscPressed()) and robodog.is_rb_alive():
        rb_horizontal = robodog.get_rb_data(0)
        rb_vertical = robodog.get_rb_data(1)
        if rb_horizontal == -127:
           rotSum = robodog.get_rotation()
        else:
            rotSum += (rb_horizontal/15)
        robodog.rotate(int(rotSum), 100)
        if rb_vertical == -127:
           leg_height = 0
        else:
            leg_height -= (rb_vertical/15)
        leg_height = DefLib.constrain(leg_height, -20, 20)
        robodog.leg_bend(int(60+leg_height), int(60+leg_height), int(60-leg_height), int(60-leg_height))

        sleep(0.1)

    robodog.Close()