Example: full robot control sequence

The following Logic Program demonstrates a complete control sequence: checking prerequisites, recovering from safety faults, powering on, releasing brakes, running the main program, and powering off. The main program loops on sync() while playing.

See Control API, Control methods, and State query methods for method descriptions and return values.

handle = control_api_factory(source="my_logic_program")
sleepTime = 3
pollInterval = 1

while True:
    ctrl = handle.is_control_allowed()
    if ctrl != 1:
        textmsg("Not in remote mode, waiting...")
        sleep(2)
    else:
        safety = handle.robot_safety_status()
        mode = handle.robot_mode()

        if safety == 8 or safety == 9:
            textmsg("Detected safety fault (", safety, "), restarting safety...")
            status = handle.restart_safety()
            textmsg("restart_safety: ", status)
            sleep(sleepTime)

            attempts = 0
            while attempts < 10:
                safety = handle.robot_safety_status()
                if safety == 1:
                    break
                end
                textmsg("Waiting for safety normal, currently: ", safety)
                sleep(sleepTime)
                attempts = attempts + 1
            end

        elif safety == 3:
            textmsg("Detected protective stop, unlocking...")
            status = handle.unlock_protective_stop()
            textmsg("unlock_protective_stop: ", status)
            sleep(sleepTime)

            attempts = 0
            while attempts < 10:
                safety = handle.robot_safety_status()
                if safety == 1:
                    break
                end
                textmsg("Waiting for safety normal, currently: ", safety)
                sleep(sleepTime)
                attempts = attempts + 1
            end

        end

        safety = handle.robot_safety_status()
        mode = handle.robot_mode()

        if safety == 1:
            if mode == 3:
                status = handle.arm_power_on()
                textmsg("arm_power_on: ", status)
                sleep(sleepTime)

                attempts = 0
                while attempts < 10:
                    mode = handle.robot_mode()
                    if mode == 5:
                        break
                    end
                    sleep(sleepTime)
                    attempts = attempts + 1
                end
            end

            if mode == 5:
                status = handle.arm_brake_release()
                textmsg("arm_brake_release: ", status)
                sleep(sleepTime)

                attempts = 0
                while attempts < 10:
                    mode = handle.robot_mode()
                    if mode == 7:
                        break
                    end
                    sleep(sleepTime)
                    attempts = attempts + 1
                end
            end

            if mode == 7:
                pstate = handle.robot_program_state()
                if pstate != 2:
                    status = handle.program_start()
                    textmsg("program_start: ", status)
                    sleep(sleepTime)
                end

                pstate = handle.robot_program_state()
                if pstate == 2:
                    handle.program_pause()
                    textmsg("program_pause sent")
                    sleep(sleepTime)

                    pstate = handle.robot_program_state()
                    if pstate == 4:
                        handle.program_start()
                        textmsg("program_start (resume) sent")
                        sleep(sleepTime)
                        handle.program_stop()
                        sleep(sleepTime)
                    end
                end
            end

            status = handle.arm_power_off()
            textmsg("arm_power_off: ", status)
            sleep(sleepTime)
        end
    end

    sleep(pollInterval)
end