pausing function and continue from a certain point in a robot movement - c++

Question

i have a question on how to program a certain sequence for my robot.
lets say if i would to program to make it run from position a to b, i have a sensor attach to it that should it detect x, it would perform an action called y at the position where it detect x, where action y doesnt change its position.
i would like the robot to continue from where it left after performing action y to go towards b. however i do not know how to pause the sequence from a to b and continue from where it left off after performing action y. i am controlling only the motor of the wheels and its timing so i can only set the speed of the wheels for a certain time.
is there a pause function in general( not sleep) in c++ and to continue running its lines of code from where it paused?
for now i do know how to reset its action but thats not what i want.
example( making the robot move from a to b in 10 seconds, detect object x at 3 seconds, do action y at position when t =3 seconds, continue motion for remaining 7 seconds after action y has been done)

Answer 1

You can try to use some event (message) driving architecture like the following pseudo code:
vector<action> action_sequence={do_a,do_b,do_c};
int i=0;
while(1)
{
e = WaitForMessage();
switch(e.code)
{
case do_action:
action_sequence[i].run();//perform an action
...//some code to have a scheduler thread to send next
//do_action message in certain seconds to this thread.
i++;
default:
...
}
}

Answer 2

The answer would depend on your code, are you using windows messaging, are you using thread, etc. Assuming that you are using neither, just linear code you could implement your own sleep function which is passed a function by the caller which is used to access if the sleep should be preempted. If preempted, then the function returns the time left so the action can be continued later.
This allows linear code to handle your situation. I knocked up a sample. Will explain the bits.
typedef bool (*preempt)(void);
DWORD timedPreemptableAction (DWORD msTime, preempt fn)
{
DWORD startTick = GetTickCount();
DWORD endTick = startTick + msTime;
DWORD currentTick;
do
{
currentTick = GetTickCount();
}
while (fn () == false && currentTick < endTick);
return currentTick > endTick ? 0 : endTick-currentTick;
}
The key function above, obtains start time in milliseconds, and will not exit until the timeout expires - or the user provided function returns true.
This user provided function could poll input devices such as a keyboard press etc. For now to match your question, I have added a user function which returns true, after 3 seconds:
DWORD startToTurnTicks = 0;
bool startToTurn (void)
{
bool preempt = false;
// TODO Implement method of preemption. For now
// just use a static timer, yep a little hacky
//
// 0 = uninitialized
// 1 = complete, no more interruptions
// >1 = starting tick count
if (startToTurnTicks == 0)
{
startToTurnTicks = GetTickCount();
}
else
{
if (startToTurnTicks != 1)
{
if ((startToTurnTicks + 3000) < GetTickCount())
{
startToTurnTicks = 1;
preempt = true;
}
}
}
return preempt;
}
Now we have a function which waits for N time and can exit, and a user function which will return true after 3 seconds, now the main call:
bool neverPreempt (void)
{
return false;
}
int main (void)
{
int appResult = 0;
DWORD moveForwardTime = 1000*10;
DWORD turnTime = 1000*3;
DWORD startTicks = GetTickCount();
printf ("forward : %d seconds in\n",
(GetTickCount()-startTicks)/1000);
moveForwardTime = timedPreemptableAction (moveForwardTime, &startToTurn);
printf ("turn : %d seconds in\n",
(GetTickCount()-startTicks)/1000);
turnTime = timedPreemptableAction (turnTime, &neverPreempt);
printf ("turn complete : %d seconds in\n",
(GetTickCount()-startTicks)/1000);
if (moveForwardTime > 0)
{
printf ("forward again : %d seconds in\n",
(GetTickCount()-startTicks)/1000);
moveForwardTime = timedPreemptableAction (moveForwardTime, &neverPreempt);
printf ("forward complete : %d seconds in\n",
(GetTickCount()-startTicks)/1000);
}
return appResult;
}
In the main code you see timedPreemptableAction is called 3 times. The first time we pass the user function which turns true after 3 seconds. This first call exits after three seconds returning 7 seconds left. The output from the app returns:
f:\projects\cmake_test\build\Debug>main
forward : 0 seconds in
turn : 3 seconds in
turn complete : 6 seconds in
forward again : 6 seconds in
forward complete : 13 seconds in
Started to move forward #0 seconds, "paused" #3 seconds, restored #6 and finished #13.
0->3 + 6->13 = 10 seconds.