How to optimize program about "control-the-start-and-stop-of-the-robot"

Hi every one:

I try to write a script to simulate emergency stop.

I refer to the post as bellowed.

I found a problem that the current taget would be skipped when the robot resumes startup.
If the current taget of the MainRoutine, contains such as “Wain IN[101] == True”, it would skipped too.

I tried to use HeartBeat (Enable HeartBeat ,Set HeartbeatTime = 1000) , but it just stop the robot . If I stopped it at 10 seconds and start at 100 second
it didn’t start at 10 seconds, it started at the equivalent of 100s, which is a kind of like warm-up function.

My idea is to write a script, when the emergency stop is triggered.

  1. Copy the statement being executed, and then Paste into the current position immediately.
  2. After the resumption , delete the statement inserted before.

But I do not know how to do that, I hope your help,