#!/usr/bin/python3

from pymycobot.mycobot import MyCobot
from pymycobot import PI_PORT, PI_BAUD
import time

mycobot = MyCobot(PI_PORT, PI_BAUD)

def homePosition():
    mycobot.send_angles([0, -45, -90, 90, 0, 0], 90)
    time.sleep(2.5)
    print("Home Position : ")
    print(mycobot.get_coords())
    
def zeroPosition():
    mycobot.send_angles([0, 0, 0, 0, 0, 0], 100)
    time.sleep(2.5)
    print("Zero Position : ")
    print(mycobot.get_coords())

def forwardKinematics():
    mycobot.send_angles([0, 90, 0, 0, 0, 0], 100)
    time.sleep(2.5)
    print(mycobot.get_coords())
def inverseKinematics():
    mycobot.send_coords([160, 0, 200, 0, 0, 0], 100, 0)
    time.sleep(2.5)
    print(mycobot.get_angles())

def mobileTest00():
    mycobot.send_coords([120, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([140, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([160, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([180, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([200, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([210, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([220, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([230, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([240, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([250, 0, 150, 0, 180, 0], 100, 0)
    time.sleep(0.5)
    print(mycobot.get_angles())


def mobileTest01():
    mycobot.send_coords([120, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([125, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([130, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([135, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([140, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([145, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([150, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([155, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([160, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([165, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([170, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([175, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([180, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([185, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([190, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([195, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([200, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([205, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([210, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([215, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([220, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([225, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([230, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([235, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([240, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([245, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([250, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([255, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    mycobot.send_coords([260, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(0.5)
    print(mycobot.get_angles())
    
    

if __name__ == '__main__':
#     zeroPosition()
#     time.sleep(2)
#     homePosition()
#     time.sleep(2)
#     forwardKinematics()
#     time.sleep(5)
#     inverseKinematics()
#     time.sleep(2)
    mobileTest01()
    time.sleep(2)
#     mycobot.send_coords([180, 0, 150, 0, 180, 0], 100, 0)
#     time.sleep(2.5)
#     print(mycobot.get_angles())
#     zeroPosition()