main.py - Module_A - AndreyNovikov/Champ

main.py
78 строк3 KB
rizz
ЧModule_A_Update
12 авг 2025, 13:09
12 авг 2025, 13:09014a979
from motion.core import RobotControl, Waypoint, InterpreterStates, LedLamp
import time
from math import *
import datetime

def main():
    lamp = LedLamp("192.168.2.101")
    robot = RobotControl("192.168.2.100")

    if robot.connect():
        lamp.setLamp("1111")

        robot.engage()

        point1 = Waypoint([0.5, -0.5,  0.3, pi/2, 0.0, pi])
        robot.moveToPointL([point1])

        while not(robot.getActualStateOut() is InterpreterStates.PROGRAM_IS_DONE.value):
            now = datetime.datetime.now()
            print(f"{now} - Robot is move")
            lamp.setLamp("0100")
            time.sleep(2.0)
            robot.play()

            if (robot.getActualStateOut() is InterpreterStates.PROGRAM_STOP_S.value):
                now = datetime.datetime.now()
                print(f"{now} - Stop")
                lamp.setLamp("0001")
                break
            elif (robot.getActualStateOut() is InterpreterStates.PROGRAM_PAUSE_S.value):
                now = datetime.datetime.now()
                print(f"{now} - Pause")
                lamp.setLamp("0010")
            else:
                print("Robot in point")
                robot.toolOFF()
                time.sleep(0.25)
            
            lamp.setLamp("1000")

        # if robot.moveToStart():
        #     lamp.setLamp("1000")

            # robot.manualCartMode()
            # print(robot.getActualStateOut(), robot.getRobotMode(), robot.getRobotState())
            
            # robot.manualJointMode()
            # print(robot.getActualStateOut(), robot.getRobotMode(), robot.getRobotState())

            # robot.moveToPointJ([Waypoint([radians(0.0), radians(0.0), radians(70.0), radians(0.0), radians(90.0), radians(0.0)])])
            # print(robot.getActualStateOut(), robot.getRobotMode(), robot.getRobotState())

            # robot.moveToPointJ([Waypoint([radians(0.0), radians(0.0), radians(70.0), radians(0.0), radians(90.0), radians(0.0)])])
            
            # while not(robot.getActualStateOut() is InterpreterStates.PROGRAM_IS_DONE.value):
            #     print("Robot is move")
            #     lamp.setLamp("0100")
            #     time.sleep(2.0)
            #     robot.pause()
            #     time.sleep(1.0)
            #     robot.play()
            # else:
            #     print("Robot in point")
            #     robot.toolON()
            #     time.sleep(0.25)
            
        # robot.manualCartMode()
        # robot.setCartesianVelocity([1.0, 0.0, 0.0, 0.0, 0.0, 0.0])
        # time.sleep(1.0)

        # robot.manualJointMode()
        # robot.setJointVelocity([-1.0, 0.0, 0.0, 0.0, 0.0, 0.0])
        # time.sleep(1.0)
    else:
        lamp.setLamp("0000")

if __name__ == "__main__":
    main()