Yes, your state diagram captures exactly what I've suggested. Obviously, your guards in the substates have side effects, which UML purists will criticize. But, still this is a valid UML statechart, as far as I can tell.
The statechart shows all the behavior and it will do what you want when you code it for QEP, obeying the QEP rule for coding guards. The main point here is that you don't need to invent any new notation or coding rules.