You can call functions but you can't call blocking functions which is what waitforlocking() seems to be.
Instead, you need to program a state machine which is essentially a switch statement on a status variable.
Code:
static int state = WAITING;
switch(state){
case WAITING:
if(locked)
// ask for tool change
state = LOCKED;
break;
case LOCKED:
if(changed)
state = CHANGED;
break;
}
The attached example shows an example state machine and also a function that calculates a moving average. (It would be easy to program this by using a loop but that blocks so there is a convoluted way to calculate this without a loop)
Remember, that the servo thread is essentially a timer interrupt service routine and you cannot block an interrupt becasue all components must be serviced in that 1 millisecond between invocations. Short, sharp and sweet is the way to do this.
