RobotStudio Script

MODULE user (SYSMODULE)

    ! --- PERSISTENT VARIABLES (Controlled by Unity) ---
    PERS jointtarget p_UnityTarget;     ! Target for Unity to set (Manual Control)
    PERS bool b_MoveNow := FALSE;       ! Flag for Unity to trigger a manual move
    ! Playback Control Flags (from SimulationControlPanel)
    PERS bool b_Play := FALSE;          ! Continuous play of a sequence
    PERS bool b_Pause := FALSE;         ! Pause during playback
    PERS bool b_Stop := FALSE;          ! Immediate stop during playback
    PERS bool b_StepForward := FALSE;   ! Step by step forward
    PERS bool b_StepBackward := FALSE;  ! Step by step backward
    PERS bool b_ResetPP := FALSE;       ! Reset Program Pointer (handled by Unity via RWS)
    
    ! SCADA Control Flags (from ScadaControlPanel)
    PERS bool b_SCADAPlay := FALSE; 
    PERS bool b_SCADAPause:= FALSE;
    PERS bool b_SCADAStop := FALSE;     ! Graceful stop after current action/cycle
    PERS bool b_SCADAEStop:= FALSE;     ! IMMEDIATE Stop (Emergency Stop)

    ! --- PROGRAM VARIABLES ---
    CONST num MAX_PATH_POINTS := 4; 
    VAR jointtarget jt_PathPoints{MAX_PATH_POINTS}; 
    PERS num current_path_index := 1; ! Initialize to 1, as InitializePathPoints will set it.

    PROC main()
        TPWrite "Unity SCADA: Initializing...";
        
        InitializePathPoints;
        TPWrite "Unity SCADA: Ready.";

        WHILE TRUE DO
            ! --- Control from HybridControlPanel (Manual Mode) ---
            IF b_MoveNow THEN
                TPWrite "Manual Move flag. Moving...";
                MoveAbsJ p_UnityTarget, v50, z50, tool0;
                b_MoveNow := FALSE; 
                TPWrite "Manual Move done.";
            ENDIF

            ! --- Playback Control from SimulationControlPanel (Sim Sync Mode) ---
            IF b_Play THEN
                TPWrite "Sim Play flag. RunSeq()...";
                RunSimulationSequence;
                b_Play := FALSE;
            ENDIF

            IF b_Pause THEN
                TPWrite "Sim Play Paused."; StopMove;
                WaitUntil b_Pause = FALSE; 
                TPWrite "Sim Resumed.";
            ENDIF

            IF b_Stop THEN
                TPWrite "Sim Play Stop. Stopping.";
                StopMove;
                b_Stop := FALSE;
                TPWrite "Sim Play Stopped.";
            ENDIF

            IF b_StepForward THEN
                TPWrite "Sim Play StepFwd.";
                StepForwardPath; 
                b_StepForward := FALSE; 
            ENDIF
            
            IF b_StepBackward THEN
                TPWrite "Sim Play StepBwd.";
                StepBackwardPath; 
                b_StepBackward := FALSE;
            ENDIF

            IF b_ResetPP THEN
                TPWrite "PP Reset. Robot Home.";
                current_path_index := 1;
                p_UnityTarget := jt_PathPoints{1};
                MoveAbsJ p_UnityTarget, v50, fine \Inpos := inpos100, tool0;
                b_ResetPP := FALSE;
                ExitCycle;
                
            ENDIF

            ! --- SCADA Control Panel Logic ---
            IF b_SCADAPlay THEN
                TPWrite "SCADA Play flag received from main. Calling ScadaProcessCycle...";
                ScadaProcessCycle; 
            ENDIF 

            IF b_SCADAPause THEN
                TPWrite "SCADA Process Paused (main)."; StopMove;
                WaitUntil b_SCADAPause = FALSE; 
                TPWrite "SCADA Process Resumed (main).";
            ENDIF

            IF b_SCADAEStop THEN
                TPWrite "SCADA E-STOP (main)!";
                Stop; 
                b_SCADAEStop := FALSE;
                b_SCADAPlay := FALSE; 
                ExitCycle; 
            ENDIF
            
            WaitTime 0.05; 
        ENDWHILE
    ENDPROC
    

    PROC InitializePathPoints()
        jt_PathPoints{1} := [[0,0,0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];          ! Home
        jt_PathPoints{2} := [[20,20,20,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];         ! Pose 1
        jt_PathPoints{3} := [[-10,-10,-10,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];       
        jt_PathPoints{4} := [[45,-30,60,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];        
        
        
        IF p_UnityTarget = jt_PathPoints{1} THEN 
            TPWrite "Path pts init. Robot Home.";
        ELSE
        p_UnityTarget := jt_PathPoints{1};
        MoveAbsJ p_UnityTarget, v50, fine \Inpos := inpos100, tool0; ! Changed to fine for initial move
        current_path_index := 1;
        ENDIF
        
    ENDPROC
    
     PROC RunSimulationSequence()
        VAR num i;
        TPWrite "SimSeq: ENTERING. Initial current_path_index: " + NumToStr(current_path_index,0);
        
        FOR i FROM current_path_index TO MAX_PATH_POINTS DO 
            TPWrite "SimSeq: Loop iteration " + NumToStr(i,0) + ". Target idx: " + NumToStr(i,0);

            IF b_Stop THEN 
                TPWrite "Sim Stop detected during sequence. Exiting.";
                StopMove;
                current_path_index := 1; 
                b_Stop := FALSE; 
                b_Play := FALSE; 
                RETURN; ! Exit the procedure immediately if stop is pressed
            ENDIF
            IF b_Pause THEN 
                TPWrite "Sim Pause detected during sequence. Pausing.";
                StopMove;
                WaitUntil b_Pause = FALSE; 
                TPWrite "Sim Resuming.";
            ENDIF
            
            p_UnityTarget := jt_PathPoints{i}; 
            TPWrite "SimSeq: Before MoveAbsJ to idx: " + NumToStr(i,0);
            MoveAbsJ p_UnityTarget, v50, fine \Inpos := inpos100, tool0; 
            TPWrite "SimSeq: After MoveAbsJ to idx: " + NumToStr(i,0);
            WaitTime 0.5;
        ENDFOR
        
        TPWrite "SimSeq: Loop complete. Moving to Home position before exit.";
        current_path_index := 1; ! Ensure index is 1 for home position move
        p_UnityTarget := jt_PathPoints{1};
        MoveAbsJ p_UnityTarget, v50, fine \Inpos := inpos100, tool0; ! Move back to Home
        
        TPWrite "SimSeq: Robot at Home. Sequence Complete.";
        b_Play := FALSE; 
    ENDPROC
    
    PROC StepBackwardPath()
        IF current_path_index > 1 THEN
            current_path_index := current_path_index - 1;
            TPWrite "StepBwd: idx " + NumToStr(current_path_index, 0);
            p_UnityTarget := jt_PathPoints{current_path_index}; 
            MoveAbsJ p_UnityTarget, v50, fine \Inpos := inpos100, tool0;
            WaitTime 0.5;
        ELSE
            TPWrite "Bgn of path.";
        ENDIF
    ENDPROC
    
    PROC StepForwardPath()
        IF current_path_index < MAX_PATH_POINTS THEN
            current_path_index := current_path_index + 1;
            TPWrite "StepFwd: idx " + NumToStr(current_path_index, 0);
            p_UnityTarget := jt_PathPoints{current_path_index}; 
            MoveAbsJ p_UnityTarget, v50, fine \Inpos := inpos100, tool0;
            WaitTime 0.1;
        ELSE
            TPWrite "End of path.";
        ENDIF
    ENDPROC
    
    PROC ScadaProcessCycle()
            TPWrite "SCADA Proc: Enter. Init idx " + NumToStr(current_path_index,0);
            WaitTime 0.1; ! Give Prefetcher/Motion System time to prepare
            IF b_SCADAPlay THEN
                TPWrite "SCADA Proc: Play TRUE.";
            ELSE
                TPWrite "SCADA Proc: Play FALSE.";
            ENDIF
    
            WHILE b_SCADAPlay DO 
                TPWrite "SCADA Proc: Top loop. current_path_index for move: " + NumToStr(current_path_index,0);
                IF b_SCADAPlay THEN
                    TPWrite "SCADA Proc: Play still TRUE.";
                ELSE
                    TPWrite "SCADA Proc: Play now FALSE. Exit soon.";
                ENDIF
    
                IF b_SCADAEStop THEN 
                    TPWrite "SCADA Proc: IMMEDIATE E-STOP ACTIVATED. Stopping now!";
                    Stop; 
                    b_SCADAEStop := FALSE; 
                    b_SCADAPlay := FALSE; 
                    current_path_index := 1; 
                    TPWrite "SCADA Proc: E-Stop done. Exit.";
                    RETURN; 
                ENDIF
    
                IF b_SCADAPause THEN
                    TPWrite "SCADA Proc: Pause detected. Pausing. idx " + NumToStr(current_path_index,0);
                    StopMove; 
                    WaitUntil b_SCADAPause = FALSE; 
                    TPWrite "SCADA Proc: Resuming.";
                ENDIF
                
                ! --- MOVE TO CURRENT INDEX FIRST ---
                p_UnityTarget := jt_PathPoints{current_path_index};
                TPWrite("Before MoveAbsJ to idx: " + NumToStr(current_path_index, 0)); 
                MoveAbsJ p_UnityTarget, v50, fine \Inpos := inpos100, tool0; ! Using fine to ensure blocking
                TPWrite("After MoveAbsJ to idx: " + NumToStr(current_path_index, 0)); 
                WaitTime 0.1; ! Small wait after motion completion for good measure
    
                TPWrite "SCADA Proc: After move to idx: " + NumToStr(current_path_index,0);
    
                ! --- THEN INCREMENT FOR NEXT ITERATION ---
                current_path_index := current_path_index + 1;
    
                IF current_path_index > MAX_PATH_POINTS THEN
                    current_path_index := 1; 
                    TPWrite "SCADA Proc: Path looped. Home (1).";
                ENDIF
    
                ! --- GRACEFUL STOP LOGIC (b_SCADAStop) ---
                IF b_SCADAStop AND current_path_index = 1 THEN
                    TPWrite "SCADA Proc: Graceful Stop & Home. Exiting.";
                    p_UnityTarget := jt_PathPoints{current_path_index};
                    MoveAbsJ p_UnityTarget, v50, fine \Inpos := inpos100, tool0; 
                    b_SCADAStop := FALSE; 
                    b_SCADAPlay := FALSE; 
                    ExitCycle; 
                ENDIF
                
                TPWrite "SCADA Proc: End loop iter. Next idx will be: " + NumToStr(current_path_index,0);
                IF b_SCADAPlay THEN
                    TPWrite "SCADA Proc: Play TRUE at iter end.";
                ELSE
                    TPWrite "SCADA Proc: Play FALSE at iter end. Terminating.";
                ENDIF
    
            ENDWHILE
            
            TPWrite "SCADA Proc: WHILE loop exited (Play FALSE).";
            b_SCADAStop := FALSE; 
            current_path_index := 1; 
            TPWrite "SCADA Proc: Proc done. Final idx 1.";
        ENDPROC

ENDMODULE