I assume that your motor/vision operations must be complete (after user interaction, waiting for your system to end the task and then consider other user command). Therefore start with some code to detect user command, either:
- event structure
- old state machine, but simplify it introducing a ring menu (or list) with commands (like "GO to location") and the parameters separated.
- avoiding the state machine, introducing an "OK" button to trigger the command
Have this part generating a code for each main operation, thus you can use it as an input for a case structure to manage motor/vision operation.
A mixed solution is to have everything inside an event structure. How good is this solution depends on the amount of code that you need to
duplicate and how data flow is affected.
Hope this helps
p.s.: If you want the user to be able to set commands while the system is "working", use a queue in order to store them for later (when motor/vision equipment is ready to execute)