
// ----------------------------------------------------------------------

void moveEngine() {
  // Main move engine rountines.
  // This code responds to a series of coded commands stored in the moves[]
  // array, which have been pre-loaded by another function. See the Commands.h
  // file for the list of available commands.
  PWR_timeout = 200; // auto-power off set to 2 seconds after sequence
  int zAny; // temp variable
  int zContinue = 1; // temp continuation flag, run by default
  switch (moveTask) {
    case 0:
      // read a command from the array and act accordingly
      moveLast = movePnt; // record current pointer
      if (moveRun > -1) {
        // in single step mode
        if (moveRun > 0) {moveRun--;
        } else {
          // stop reading commands
          zContinue = 0;
          if (moveRpt > 0) {moveRpt = 0; reportValues();}
        }
      }
      if (zContinue == 1) {
        // run as normal interpreting next command
        moveCmd = moves[movePnt];
        switch (moveCmd) {
          case cmdClap0:
            // initiate clapping n times ending closed
            clapCnt = moves[movePnt + 1]; moveSubTask = 0;
            movePnt = movePnt + 2; moveTask = 3; moveInc = 0; break;
          case cmdClap1:
            // initiate clapping n times ending open
            clapCnt = moves[movePnt + 1]; moveSubTask = 0;
            movePnt = movePnt + 2; moveTask = 3; moveInc = 1; break;
          case cmdEnd:
            // end of move commands so switch off engine, power-OFF after 2 seconds
            movePnt = -1; PWR_timeout = 200; break;
          case cmdEndOn:
            // end of move commands so switch off engine, but leave power ON
            movePnt = -1; PWR_timeout = 0; break;
          case cmdFloor:
            // move to the Floor position
            if (!servoEn) {attachServos(0);}
            servoTgt[0] = Floor0; servoTgt[1] = Floor1;
            servoTgt[2] = Floor2; servoTgt[3] = Floor3; 
            movePnt++; moveInvoke();
            moveTask = 1; break;
          case cmdFor:
            // record pointer and count for For...Loop
            moveForLp = moves[movePnt + 1];
            movePnt = movePnt + 2; moveForPnt = movePnt; break;
          case cmdGoNG:
            // load the target values and initiate a move without claw
            if (!servoEn) {attachServos(0);}
            servoTgt[0] = moves[movePnt + 1];
            servoTgt[1] = moves[movePnt + 2];
            servoTgt[2] = moves[movePnt + 3];
            movePnt = movePnt + 4; moveInvoke();
            moveTask = 1; break;
          case cmdGoPos:
            // move to a previously stored location, without claw movement
            zAny = moves[movePnt + 1];
            servoTgt[0] = movePosR[zAny];
            servoTgt[1] = movePosF[zAny];
            servoTgt[2] = movePosV[zAny];
            movePnt = movePnt + 2; moveInvoke();
            moveTask = 1; break;
          case cmdGoPosRnd:
            // move to a random n-m inclusive previously stored location
            // without claw movement
            zAny = random(moves[movePnt + 1], 1 + moves[movePnt + 2]);
            servoTgt[0] = movePosR[zAny];
            servoTgt[1] = movePosF[zAny];
            servoTgt[2] = movePosV[zAny];
            movePnt = movePnt + 3; moveInvoke();
            moveTask = 1; break;
          case cmdGosub_A:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[0]; break;
          case cmdGosub_B:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[1]; break;
          case cmdGosub_C:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[2]; break;
          case cmdGosub_D:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[3]; break;
          case cmdGosub_E:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[4]; break;
          case cmdGosub_F:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[5]; break;
          case cmdGosub_G:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[6]; break;
          case cmdGosub_H:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[7]; break;
          case cmdGosub_I:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[8]; break;
          case cmdGosub_J:
            // branch to Label pointer, having first stored return address
            moveReturn = movePnt + 1; movePnt = moveLabels[9]; break;
          case cmdGoTo:
            // load the target values and initiate a move with claw
            if (!servoEn) {attachServos(0);}
            servoTgt[0] = moves[movePnt + 1];
            servoTgt[1] = moves[movePnt + 2];
            servoTgt[2] = moves[movePnt + 3];
            servoTgt[3] = moves[movePnt + 4];
            movePnt = movePnt + 5; moveInvoke();
            moveTask = 1; break;
          case cmdGoV0:
            // load the target value into V0 and initiate a move
            // claw value is not affected
            if (!servoEn) {attachServos(0);}
            servoTgt[0] = moves[movePnt + 1];
            servoTgt[1] = servoVal[1];
            servoTgt[2] = servoVal[2];
            movePnt = movePnt + 2; moveInvoke();
            moveTask = 1; break;
          case cmdGoV1:
            // load the target value into V1 and initiate a move
            // claw value is not affected
            if (!servoEn) {attachServos(0);}
            servoTgt[0] = servoVal[0];
            servoTgt[1] = moves[movePnt + 1];
            servoTgt[2] = servoVal[2];
            movePnt = movePnt + 2; moveInvoke();
            moveTask = 1; break;
          case cmdGoV2:
            // load the target value into V2 and initiate a move
            // claw value is not affected
            if (!servoEn) {attachServos(0);}
            servoTgt[0] = servoVal[0];
            servoTgt[1] = servoVal[1];
            servoTgt[2] = moves[movePnt + 1];
            movePnt = movePnt + 2; moveInvoke();
            moveTask = 1; break;
          case cmdGrip:
            // close the grippers
            if (!servoEn) {attachServos(0);}
            servoVal[3] = gripClose; servoTgt[3] = servoVal[3];  movePnt++;
            // Serial.print(F("SV3=")); Serial.println(servoVal[3]);
            // printServoVals();
            ValChg = true;
            if (movePause > 0) {moveWait = movePause; moveTask = 99;}
            break;
          case cmdHome:
            // move to the Home position
            if (!servoEn) {attachServos(0);}
            servoTgt[0] = Home0; servoTgt[1] = Home1;
            servoTgt[2] = Home2; servoTgt[3] = Home3 + 503; 
            movePnt++; moveInvoke();
            moveTask = 1; break;
          case cmdJumpTo_A:
            // branch to Label_A pointer
            movePnt = moveLabels[0]; break;
          case cmdJumpTo_B:
            // branch to Label_A pointer
            movePnt = moveLabels[1]; break;
          case cmdJumpTo_C:
            // branch to Label_A pointer
            movePnt = moveLabels[2]; break;
          case cmdJumpTo_D:
            // branch to Label_A pointer
            movePnt = moveLabels[3]; break;
          case cmdJumpTo_E:
            // branch to Label_A pointer
            movePnt = moveLabels[4]; break;
          case cmdJumpTo_F:
            // branch to Label_A pointer
            movePnt = moveLabels[5]; break;
          case cmdJumpTo_G:
            // branch to Label_A pointer
            movePnt = moveLabels[6]; break;
          case cmdJumpTo_H:
            // branch to Label_A pointer
            movePnt = moveLabels[7]; break;
          case cmdJumpTo_I:
            // branch to Label_A pointer
            movePnt = moveLabels[8]; break;
          case cmdJumpTo_J:
            // branch to Label_A pointer
            movePnt = moveLabels[9]; break;
          case cmdLabel_A:
            // store a dynamic label pointer
            moveLabels[0] = movePnt + 1; movePnt++; break;
          case cmdLabel_B:
            // store a dynamic label pointer
            moveLabels[1] = movePnt + 1; movePnt++; break;
          case cmdLabel_C:
            // store a dynamic label pointer
            moveLabels[2] = movePnt + 1; movePnt++; break;
          case cmdLabel_D:
            // store a dynamic label pointer
            moveLabels[3] = movePnt + 1; movePnt++; break;
          case cmdLabel_E:
            // store a dynamic label pointer
            moveLabels[4] = movePnt + 1; movePnt++; break;
          case cmdLabel_F:
            // store a dynamic label pointer
            moveLabels[5] = movePnt + 1; movePnt++; break;
          case cmdLabel_G:
            // store a dynamic label pointer
            moveLabels[6] = movePnt + 1; movePnt++; break;
          case cmdLabel_H:
            // store a dynamic label pointer
            moveLabels[7] = movePnt + 1; movePnt++; break;
          case cmdLabel_I:
            // store a dynamic label pointer
            moveLabels[8] = movePnt + 1; movePnt++; break;
          case cmdLabel_J:
            // store a dynamic label pointer
            moveLabels[9] = movePnt + 1; movePnt++; break;
          case cmdNext:
            // if count not expired repeat For...Next loop
            moveForLp--; movePnt++;
            if (moveForLp > 0) {movePnt = moveForPnt;}
            break;
          case cmdOpen:
            // open the grippers
            if (!servoEn) {attachServos(0);}
            servoVal[3] = gripOpen; servoTgt[3] = servoVal[3]; movePnt++;
            // Serial.print(F("SV3=")); Serial.println(servoVal[3]);
            // printServoVals();
            ValChg = true;
            if (movePause > 0) {moveWait = movePause; moveTask = 99;}
            break;
          case cmdPop:
            // move to remembered position
            servoTgt[0] = moveMem0; servoTgt[1] = moveMem1;
            servoTgt[2] = moveMem2; movePnt++; moveInvoke();
            moveTask = 1; break;
          case cmdPush:
            // remember current position
            moveMem0 = servoVal[0]; moveMem1 = servoVal[1];
            moveMem2 = servoVal[2]; movePnt++; break;
          case cmdReplay:
            // replay the sequence from the beginning
            movePnt = 0; break;
          case cmdReset:
            // move to the RESET position
            if (!servoEn) {attachServos(0);}
            servoTgt[0] = Reset0; servoTgt[1] = Reset1;
            servoTgt[2] = Reset2; servoTgt[3] = Reset3; 
            movePnt++; moveInvoke();
            moveTask = 1; break;
          case cmdReturn:
            // return program control to a previously store line
            movePnt = moveReturn; break;
          case cmdSet0:
            // set a servo 0 value immediately
            if (!servoEn) {attachServos(0);}
            servoVal[0] = moves[movePnt + 1]; servoTgt[0] = servoVal[0];
            // Serial.print(F("SV0=")); Serial.println(servoVal[0]);
            // printServoVals();
            ValChg = true;
            movePnt = movePnt + 2; break;
          case cmdSet1:
            // set a servo 1 value immediately
            if (!servoEn) {attachServos(0);}
            servoVal[1] = moves[movePnt + 1]; servoTgt[1] = servoVal[1];
            // Serial.print(F("SV1=")); Serial.println(servoVal[1]);
            // printServoVals();
            ValChg = true;
            movePnt = movePnt + 2; break;
          case cmdSet2:
            // set a servo 2 value immediately
            if (!servoEn) {attachServos(0);}
            servoVal[2] = moves[movePnt + 1]; servoTgt[2] = servoVal[2];
            setVertMinMax(); // ensure vertical channel is within limits
            // Serial.print(F("SV2=")); Serial.println(servoVal[2]);
            // printServoVals();
            ValChg = true;
            movePnt = movePnt + 2; break;
          case cmdSet3:
            // set a servo 3 value immediately
            if (!servoEn) {attachServos(0);}
            servoVal[3] = moves[movePnt + 1]; servoTgt[3] = servoVal[3];
            // Serial.print(F("SV3=")); Serial.println(servoVal[3]);
            // printServoVals();
            ValChg = true;
            movePnt = movePnt + 2; break;
          case cmdSetPause:
            // set the delay after each move, default = 0
            movePause = moves[movePnt + 1]; movePnt = movePnt + 2; break;
          case cmdSetPos:
            // load servo values from a previously stored location
            if (!servoEn) {attachServos(0);}
            zAny = moves[movePnt + 1];
            servoVal[0] = movePosR[zAny];
            servoVal[1] = movePosF[zAny];
            servoVal[2] = movePosV[zAny];
            setVertMinMax(); // ensure vertical channel is within limits
            // Serial.print(F("SV0=")); Serial.println(servoVal[0]);
            // Serial.print(F("SV1=")); Serial.println(servoVal[1]);
            // Serial.print(F("SV2=")); Serial.println(servoVal[2]);
            // printServoVals();
            ValChg = true;
            movePnt = movePnt + 2; break;
          case cmdSetSpeed:
            // change the move loop period, default = 10000 (10ms)
            // max value is 32,767 as cmd array are integers
            moveInterval = moves[movePnt + 1];
            movePnt = movePnt + 2; break;
          case cmdSleep:
            // turn off servo motors
            detachServos(); movePnt++; break;
          case cmdWait:
            // load the waiting time and start a wait task
            moveWait = moves[movePnt + 1];
            if (moveInterval != moveDefInterval) {
              // loop speed has changed so set delay count accordingly
              moveWait = (moveWait * moveDefInterval)/moveInterval;
            }
            movePnt = movePnt + 2; moveTask = 99; break;
          case cmdWaitRnd:
            // wait for a random number of loops
            moveWait = random(moves[movePnt + 1],moves[movePnt + 2]);
            movePnt = movePnt + 3; moveTask = 99;
            break;
          case cmdWide:
            // open the grippers
            if (!servoEn) {attachServos(0);}
            servoVal[3] = gripWide; servoTgt[3] = servoVal[3]; movePnt++;
            // Serial.print(F("SV3=")); Serial.println(servoVal[3]);
            // printServoVals();
            ValChg = true;
            if (movePause > 0) {moveWait = movePause; moveTask = 99;}
            break;
        } break;
   
      
      case 1:
        // moving towards target values, whilst moving grippers
        // movement is performed in main loop, here we wait for it to end
        if (moveCnt < 1) {
          moveInterval = moveDefInterval;
          moveTask = 0; if (movePause > 0) {moveWait = movePause; moveTask = 99;}
        } break;

      case 3:
        // perform clapping actions
        if (!servoEn) {attachServos(0);}
        switch (moveSubTask) {
          case 0:
            // move jaw gripper
            if (moveInc < 1) {servoVal[3] = gripOpen; servoTgt[3] = servoVal[3];}
            else {servoVal[3] = gripClose; servoTgt[3] = servoVal[3];}
            // printServoVals();
            ValChg = true;
            moveWait = 10; moveSubTask++; break;
          case 1:
            // wait for 90ms
            moveWait--; if (moveWait < 1) {moveSubTask++;}
            break;
          case 2:
            // move jaw gripper and test for last time?
            if (moveInc < 1) {servoVal[3] = gripClose; servoTgt[3] = servoVal[3];}
            else {servoVal[3] = gripOpen; servoTgt[3] = servoVal[3];}
            // Serial.print(F("SV3=")); Serial.println(servoVal[3]);
            // printServoVals();
            ValChg = true;
            clapCnt--; if (clapCnt > 0) {
              moveWait = 10; moveSubTask++;
            } else {
                moveTask = 0; if (movePause > 0) {moveWait = movePause; moveTask = 99;}
            } break;
          case 3:
            // wait for 90ms
            moveWait--; if (moveWait < 1) {moveSubTask = 0;}
            break;
        } break;

      case 99:
        // wait for a predefined number of cycles
        moveWait--; if (moveWait < 1) {
          // end of timeout so set task to read next command
          moveTask = 0;
        } break;
    }
  }
}

// ----------------------------------------------------------------------

void moveGoFloor() {
  // use the move engine to go to the Floor position
  // moveInit(); moveLoad1(cmdFloor); moveLoad1(cmdEndOn); moveStart();
  if (button_BF) return; // button already pressed
  
  moveSTOP();
  servoTgt[0] = Floor0; servoTgt[1] = Floor1; servoTgt[2] = Floor2; servoTgt[3] = Floor3; 
  PrintTx+= "\nGoing to Floor...\n";
  if (APP > 0) {reportServoTgts(); }
  moveInvoke(); PWR_timeout = 1000;
  // if Wii connected, prevent further calls to this function whilst button is down
  if (WiiError == 0) {button_BF = true;}
}

// ----------------------------------------------------------------------

void moveGoHome() {
  // use the move engine to go to the Home position
  // moveInit(); moveLoad1(cmdHome); moveLoad1(cmdEndOn); moveStart();
  if (button_BH) return; // button already pressed
  
  moveSTOP();
  servoTgt[0] = Home0; servoTgt[1] = Home1;
  servoTgt[2] = Home2; servoTgt[3] = Home3 + 503; 
  PrintTx+= "\nGoing Home....\n";
  if (APP > 0) {reportServoTgts();}
  moveInvoke();  PWR_timeout = 1000;
  // if Wii connected, prevent further calls to this function whilst button is down
  if (WiiError == 0) {button_BH = true;}
}

// ----------------------------------------------------------------------

void moveGoRESET() {
  // use the move engine to go to the RESET position
  // moveInit(); moveLoad1(cmdReset); moveLoad1(cmdEnd); moveStart();
  if (button_BS) return; // button already pressed
  
  moveSTOP();
  servoTgt[0] = Reset0; servoTgt[1] = Reset1; servoTgt[2] = Reset2; servoTgt[3] = Reset3; 
  PrintTx+= "\nGoing to Reset...\n";
  if (APP > 0) {reportServoTgts();}
  moveInvoke(); PWR_timeout = 200;
  // if Wii connected, prevent further calls to this function whilst button is down
  if (WiiError == 0) {button_BS = true;}
}

// ----------------------------------------------------------------------

void moveInit() {
  // called before loading and running the Move Engine
  // Initialises all variables to correct default state
  movePnt = 0; // reset the move pointer
  movePause = 0; // zero delay between moves as default
  moveReturn = moveArraySize -1; // store last line as default return line
  
  // clear the move array
  for (anyVal = 0; anyVal < moveArraySize; anyVal++) {moves[anyVal] = cmdEnd;}
  
  // clear the position arrays
  for (anyVal = 0; anyVal < posMax; anyVal++) {
    movePosR[anyVal] = Home0; movePosF[anyVal] = Home1; movePosV[anyVal] = Home2;}
}

// ----------------------------------------------------------------------

void moveInvoke() {
  // called from move or play engine to set the move counter and 
  moveInterval = moveDefInterval;
  moveIntervalInc = 0; moveIntervalMin = moveDefInterval;

  // calculate number of steps needed for the move looking at 3 target values
  // we don't use the claw movement as a determining factor, but set a minimum
  long zVal;
  moveCnt = 5; // absolute minimum number of steps, and for claw only moves
  zVal = abs(servoTgt[0] - servoVal[0]); zVal = (Mult * zVal)/(turntableMax - turntableMin);
  moveCnt = max(moveCnt,int(zVal));
  zVal = abs(servoTgt[1] - servoVal[1]); zVal = (Mult * zVal)/(fwdArmMax - fwdArmMin);
  moveCnt = max(moveCnt,int(zVal));
  zVal = abs(servoTgt[2] - servoVal[2]); zVal = (Mult * zVal)/(vertArmMaxB - vertArmMinA);
  moveCnt = max(moveCnt,int(zVal));
  // Serial.print(F("moveCnt = ")); Serial.println(moveCnt);
}

// ----------------------------------------------------------------------

void moveLoad1(int zCmd) {
  // loads one value into the move engine array and increments pointer
  moves[movePnt] = zCmd; movePnt++;  moves[movePnt] = cmdEnd;
}

// ----------------------------------------------------------------------

void moveLoad2(int zCmd, int zV0) {
  // loads two values into the move engine array and increments pointer
  moves[movePnt] = zCmd; movePnt++;
  moves[movePnt] = zV0; movePnt++; moves[movePnt] = cmdEnd;
}

// ----------------------------------------------------------------------

void moveLoad3(int zCmd, int zV0, int zV1) {
  // loads three values into the move engine array and increments pointer
  moves[movePnt] = zCmd; movePnt++;
  moves[movePnt] = zV0; movePnt++;
  moves[movePnt] = zV1; movePnt++; moves[movePnt] = cmdEnd;
}

// ----------------------------------------------------------------------

void moveLoad4(int zCmd, int zV0, int zV1, int zV2) {
  // loads four values into the move engine array and increments pointer
  moves[movePnt] = zCmd; movePnt++;
  moves[movePnt] = zV0; movePnt++;
  moves[movePnt] = zV1; movePnt++;
  moves[movePnt] = zV2; movePnt++; moves[movePnt] = cmdEnd;
}

// ----------------------------------------------------------------------

void moveLoad5(int zCmd, int zV0, int zV1, int zV2, int zV3) {
  // loads five values into the move engine array and increments pointer
  moves[movePnt] = zCmd; movePnt++;
  moves[movePnt] = zV0; movePnt++;
  moves[movePnt] = zV1; movePnt++;
  moves[movePnt] = zV2; movePnt++;
  moves[movePnt] = zV3; movePnt++; moves[movePnt] = cmdEnd;
}

// ----------------------------------------------------------------------

void moveLoadPosRFV (int zA, int zV0, int zV1, int zV2) {
  // load servo co-ordinates into storage array
  movePosR[zA] = zV0; movePosF[zA] = zV1;  movePosV[zA] = zV2;
}

// ----------------------------------------------------------------------

void moveStart() {
  // initiates playing a move command sequence if valid
  if (moves[0] != cmdEnd) {
    // set initial conditions for move engine
    moveInterval = moveDefInterval; // default speed 10 ms loop
    movePnt = 0; moveRun = -1; moveTask = 0; attachServos(10);
    // search for labels and store them for forward branch jumps/gosubs
    for (anyVal = 0; anyVal < moveArraySize; anyVal++) {
      anyAny = moves[anyVal];
      // look for -ve values -100 to -109
      if ((anyAny <= cmdLabel_A) && (anyAny >= cmdLabel_J)) {
        moveLabels[cmdLabel_A - anyAny] = anyVal + 1; // store next line number
      }
    }
  } else {
    // move cmd array does not look valid so stop engine
    movePnt = -1;
  }
}

// ----------------------------------------------------------------------

void moveSTOP() {
  // immediately stop any auto-movements
  playRun = false;  // stop any Play Engine movement
  movePnt = -1;     // stop any Move Engine movement
  moveCnt = 0;      // stop the servo movement task
}

// ----------------------------------------------------------------------

void moveStopButton() {
  // called when a SW0 or SW1 button is pressed whilst moving
  // we immediately stop any auto-movements and move to the REST position
  moveGoRESET();
  button_BS = false;
  WaitWhilstDwn();  // wait for both buttons to be released
}

// ----------------------------------------------------------------------

void moveToHome() {
  // move immediately to the HOME position
  movePnt = -1; // switch off move engine
  servoVal[0] = Home0; servoVal[1] = Home1;
  servoVal[2] = Home2; servoVal[3] = Home3;
  attachServos(100); delay(200); // note servos remain attached
}

// ----------------------------------------------------------------------

void moveToResetPos() {
  // move immediately to the RESET position
  movePnt = -1; // switch off move engine
  servoVal[0] = Reset0; servoVal[1] = Reset1;
  servoVal[2] = Reset2; servoVal[3] = Reset3;
  attachServos(100); // note servos turn OFF automatically
}

// ----------------------------------------------------------------------

void moveToTest00 () {
  // this is the UTC robots rock workspace, with three platforms and 
  // three machines to operate
  
  // Serial.println(F("Loading moveToTest00..."));
  moveInit(); // always Initialise Move Engine before loading arrays!
  movePause = 50; // set delay after move to 50 loops, default = 0
  
  // load target positions
  moveLoadPosRFV(0, Home0-312, Home1+680, Home2-835); // loading bay, backed off
  moveLoadPosRFV(1, Home0-312, Home1+662, Home2-723); // loading bay, pick up
  moveLoadPosRFV(2, Home0, Home1+217, Home2-642); // centre back, mid height
  moveLoadPosRFV(3, Home0+370, Home1+374, Home2-701); // finishing bay, backed off
  moveLoadPosRFV(4, Home0+370, Home1+696, Home2-771); // finishing bay, drop off
  moveLoadPosRFV(5, Home0-205, Home1+694, Home2-526); // RH low stand
  moveLoadPosRFV(6, Home0-205, Home1+368, Home2-598); // RH low stand, backed off
  moveLoadPosRFV(7, Home0+14, Home1+634, Home2-494); // Ctr mid stand
  moveLoadPosRFV(8, Home0+14, Home1+482, Home2-577); // Ctr mid stand, backed off
  moveLoadPosRFV(9, Home0+246, Home1+570, Home2-459); // LH high stand
  moveLoadPosRFV(10, Home0+246, Home1+354, Home2-511); // LH high stand, backed off
  moveLoadPosRFV(11, Home0-196, Home1+426, Home2-160); // above press leaver
  moveLoadPosRFV(12, Home0-196, Home1+456, Home2-311); // press leaver down
  moveLoadPosRFV(13, Home0-107, Home1+330, Home2-268); // move to right of spin wheel
  moveLoadPosRFV(14, Home0+158, Home1+330, Home2-268); // move to the left of spin wheel
  moveLoadPosRFV(15, Home0+239, Home1+346, Home2-140); // opposite flag pole backed off
  moveLoadPosRFV(16, Home0+239, Home1+512, Home2-63); // opposite flag pole
  moveLoadPosRFV(17, Home0, Home1, Home2); // home for test purposes

  // load move sequence
  moveLoad1(cmdHome); // start at home position
  moveLoad2(cmdGoPos, 0); // loading bay, backed off
  moveLoad2(cmdGoPos, 1); // loading bay, pick up

  moveLoad2(cmdClap1, 5); // clap 5 times to get attention!
  moveLoad2(cmdWait, 450); // wait for 5 seconds to load object
  moveLoad1(cmdGrip); // close jaws to collect object

  // move object to 1st stand
  moveLoad1(cmdGosub_A); // call common subroutine _A

  // place the object on the 1st stand
  moveLoad2(cmdGoPos, 5); // RH low stand
  moveLoad1(cmdOpen); // open jaws to drop off object
  moveLoad2(cmdGoPos, 6); // RH low stand, backed off

  // operate press
  moveLoad1(cmdHome); // home
  moveLoad1(cmdGrip); // close jaws to operate press leaver
  moveLoad2(cmdWait, 50); // wait for 1 seconds

  moveLoad2(cmdFor, 2); // operate the press 2 time
  moveLoad2(cmdGoPos, 11); // above press leaver
  moveLoad2(cmdSetPos, 12); // press leaver down
  moveLoad1(cmdNext); // repeat For... loop
  moveLoad2(cmdGoPos, 11); // above press leaver
  moveLoad2(cmdWait, 50); // wait for 1 seconds
  moveLoad1(cmdWide); // open jaws to release press
  moveLoad2(cmdGoV1, Home1); // back away
  moveLoad1(cmdOpen); // close jaws to normal open position

  moveLoad1(cmdGosub_A); // call common subroutine _A

  // collect the object from the 1st stand
  moveLoad2(cmdGoPos, 6); // RH low stand, backed off
  moveLoad2(cmdGoPos, 5); // RH low stand
  moveLoad1(cmdGrip); // close jaws to collect object

  moveLoad1(cmdGosub_A); // call common subroutine _A

  // place the object on the centre stand
  moveLoad2(cmdGoPos, 7); // Ctr mid stand
  moveLoad1(cmdOpen); // open jaws to drop off object
  moveLoad2(cmdGoPos, 8); // Ctr mid stand, backed off

  moveLoad1(cmdGosub_A); // call common subroutine _A

  // spin the centre wheel
  moveLoad1(cmdHome); // move to Home position
  moveLoad2(cmdFor, 3); // spin wheel 3 times
  moveLoad2(cmdGoPos, 13); // move to right of spin wheel
  moveLoad2(cmdGoPos, 14); // move to left of spin wheel
  moveLoad2(cmdGoV1, Home1+180); // back away
  moveLoad2(cmdGoV0, Home0-107); // swing back to right position
  moveLoad1(cmdNext); // repeat For... loop
  moveLoad1(cmdHome); // move to Home position

  // collect the object from the centre stand
  moveLoad2(cmdGoPos, 8); // Ctr mid stand, backed off
  moveLoad2(cmdGoPos, 7); // Ctr mid stand
  moveLoad1(cmdGrip); // close jaws to collect object

  moveLoad1(cmdGosub_A); // call common subroutine _A

  // place the object on the 3rd stand
  moveLoad2(cmdGoPos, 9); // LH high stand
  moveLoad1(cmdOpen); // open jaws to drop off object
  moveLoad2(cmdGoPos, 10); // LH high stand, backed off

  moveLoad1(cmdGosub_A); // call common subroutine _A
  moveLoad1(cmdHome); // goto home

  // grab the flags
  moveLoad2(cmdSet3, Home3+420); // open jaws for grabbing flags +386
  moveLoad2(cmdGoPos, 15); // oposite flag pole
  moveLoad2(cmdGoPos, 16); // oposite flag pole
  moveLoad2(cmdFor, 4); // grab flags 4 times
  moveLoad2(cmdSet3, Home3-418); // close jaws to grab flags
  moveLoad2(cmdWait, 30); // wait for 3/10 seconds
  moveLoad2(cmdSet3, Home3+420); // open jaws to release flags
  moveLoad2(cmdWait, 30); // wait for 3/10 seconds
  moveLoad1(cmdNext); // repeat For... loop
  moveLoad2(cmdGoPos, 15); // oposite flag pole
  moveLoad1(cmdHome); // goto home

  moveLoad1(cmdGosub_A); // call common subroutine _A

  // collect the object from the 3rd stand
  moveLoad2(cmdGoPos, 10); // LH high stand, backed off
  moveLoad2(cmdGoPos, 9); // LH high stand
  moveLoad1(cmdGrip); // close jaws to collect object

  moveLoad1(cmdGosub_A); // call common subroutine _A

  // place the object in the finishing bay
  moveLoad2(cmdGoPos, 3); // finishing bay, backed off
  moveLoad2(cmdGoPos, 4); // finishing bay, drop off
  moveLoad1(cmdOpen); // open jaws to drop off object
  moveLoad2(cmdGoPos, 3); // finishing bay, backed off
  moveLoad2(cmdWait, 50); // wait for 1 seconds

  // retreat to reset and clap
  moveLoad1(cmdGosub_A); // call common subroutine _A
  moveLoad1(cmdReset); // goto reset position
  moveLoad2(cmdClap0, 3); // clap 3 times to get attention!
  moveLoad1(cmdEnd); // stop at this point

  // common subroutine
  moveLoad1(cmdLabel_A); // give this subroutine a label
  moveLoad2(cmdGoPos, 2); // centre back, mid height
  moveLoad2(cmdWait, 50); // wait for 1/2 second
  moveLoad1(cmdReturn); // return to line after Gosub

  // report the amount of program space used
  reportProgramStats();
}

// ----------------------------------------------------------------------

void moveToTest01() {
  // this is a servo warm-up routine
  
  // Serial.println(F("Loading moveToTest01..."));
  moveInit(); // always Initialise Move Engine before loading arrays!
  movePause = 50; // set delay after move to 50 loops, default = 0
  
  // load target positions
  moveLoadPosRFV(0, Home0+244, Home1+447, Home2+33); // above left-hand machine


  // load move sequence
  moveLoad1(cmdHome); // start at home position
  moveLoad2(cmdGoPos, 0); // above left-hand machine
  moveLoad1(cmdHome); // start at home position
  
  moveLoad1(cmdEnd); // stop at this point

  // report the amount of program space used
  reportProgramStats();
}

// ----------------------------------------------------------------------

void playAbort(int zErr) {
  // play engine has hit an error so abort the play process and flag the
  // error on the screen
  switch(zErr) {
    case ErrFor:    DispTx2$ = "For.. Stack"; break;
    case ErrGosub:  DispTx2$ = "Gosub Stack"; break;
    case ErrNext:   DispTx2$ = "Next Stack"; break;
    case ErrReturn: DispTx2$ = "Return Stack"; break;
    default: DispTx2$ = "Unknown??"; break;
  }
  // display the error message as flashing text
  DispTx1$ = "Play Error";
  DispNow = 1; DispInv = true; DispFlash = 10;  // flash 10 times

  // stop the play engine when the error occurs
  playRun = false;
}

// ----------------------------------------------------------------------

void playAdd() {
  // add the current servo positions to the play memory
  // playPnt ends up pointing at the next free available memory element
  // playCnt tracks the total number of elements stored
  resetRECPause(); // reset the auto-power off counter
  if (button_BA) return;
  if (playCnt == moveArraySize) {playMemLimit(); return;}
  
  // it does not matter where we are in the sequence
  copyMovesUp(playPnt,playPnt + 4);  // this will autoadjust playCnt
  playPnt+= 4;
  grabVals();  // overwrite in case it was a command
  PrintTx+= "playAdd\n";
  saveServoVals();
  button_BA = true;
}

// ----------------------------------------------------------------------

void playAddIncClap() {
  // record a clap by either inserting a clap code command or adding
  // to an existing one
  resetRECPause(); // reset the auto-power off counter
  if (button_BY) return;  // block button held condition
  
  // check to see if this is add a point or add a delay
  if (moves[playPnt] != cmdPlayClap) {
      // we have not yet created a clap command so insert one
      moves[playPnt] = cmdPlayClap;  // insert the command code
      moves[playPnt+1] = 1;          // set initial value
  } else {
    // already a clap cmd so inc up to 10 claps
    if (moves[playPnt+1] < 10) {moves[playPnt+1]++;}
  }
  // Serial.println("playClapCmd");
  // Serial.print("Claps="); Serial.println(moves[playPnt+1]);

  // now simulate a single clap
  if (!servoEn) {attachServos(0);} // in case auto-power off has disabled them
  anyVal = servoVal[3];
  for (int zP = 1; zP <= moves[playPnt+1]; zP++) {
    servoVal[3] = gripClose; servoInst[3].writeMicroseconds(servoVal[3]);
    delay(120);
    servoVal[3] = anyVal; servoInst[3].writeMicroseconds(servoVal[3]);
    delay(120);
  } SyncTimers();
  button_BY = true;
}

// ----------------------------------------------------------------------

void playAddIncDelay() {
  // make the current point a delay command
  // if already a delay command then increment it
  // if button held down then progressively increase the value
  // function called every 20ms, first delay is 500ms, then 200ms increments
  resetRECPause(); // reset the auto-power off counter
  button_BB_Cnt++; if (button_BB_Cnt > 25) {
     // button held down for > 500ms
    button_BB = false;
    button_BB_Cnt = 15; // shorter delay after initial pause
  }
  if (button_BB) return;
  
  button_BB = true;
  
  // check to see if this is add a point or add a delay
  // Serial.println("playDelayCmd");
  if (moves[playPnt] != cmdPlayDelay) {
      // we have not yet created a delay code command so insert one
      moves[playPnt] = cmdPlayDelay;  // insert the command code
      moves[playPnt+1] = 10;          // set initial value
  } else {
    // already a delay so inc 100ms up to 9.99 seconds
    if (moves[playPnt+1] < 990) {moves[playPnt+1] += 10;}
    // Serial.print("Delay="); Serial.println(moves[playPnt+1]);
  }
}

// ----------------------------------------------------------------------

void playAppend() {
  // append a position to the end of the sequence but don't move the pointer
  resetRECPause(); // reset the auto-power off counter
  if (button_BA) return;
  if (playCnt >= (moveArraySize - 1)) {return;} // can't exceed the memory space
  
  PrintTx+= "playAppend\n";
  moves[playCnt] = servoVal[0] - Home0;
  moves[playCnt+1] = servoVal[1] - Home1;
  moves[playCnt+2] = servoVal[2] - Home2;
  moves[playCnt+3] = servoVal[3] - Home3;
  playCnt+= 4; button_BA = true;
}

// ----------------------------------------------------------------------

void playButton() {
  // the 'Play/Pause' button is being pressed, we come here every 20ms
  // Play will start/restart from the current position or stop/pause
  // Serial.println(F("Play button"));
  resetRECPause();
  button_BDU_Cnt++; if (button_BDU_Cnt > 30) {
    // average button press is 14 - 20 counts (280 - 400ms)
    playRepeat = true;
    Display_Text2x16("REPEAT","PLAY MODE"); DispNOP = false; DispDel = 30;
  }
  if (button_BDU) {return;}
  
  if (playRun) {
    // currently playing so pause it
    // however the current move to target will complete
    playRun = false;
  } else {
    // not playing so start it if something is stored in memory
    // as reset position is stored in first element it will at least be played
    // by default Reset position is played, then the rest
    // Serial.print(F("playPnt = ")); Serial.println(playPnt);
    // Serial.print(F("playCnt = ")); Serial.println(playCnt);
    if ((playPnt > 0) && (playPnt == (playCnt - 4))) {
      // at end of sequence so restart it
      playPnt = 0;
    }
    playTask = 0; playRun = true;
  } button_BDU = true;
}

// ----------------------------------------------------------------------

void playContinuously() {
  // invoked by the move trainer app to play the contents of the memory continuously  
  playCnt = MemMax * 4; playPnt = 0; playTask = 0;
  playRepeat = true; playRun = true;
  // PrintTx+= "playOnce\n";
  // PrintTx+= "playCnt" + String(playCnt) + "\n";
}

// ----------------------------------------------------------------------

void playDecClap() {
  // decrement the number of claps in a clap command to 0
  // below 1 then insert current position
  resetRECPause(); // reset the auto-power off counter
  if (button_BY) return;  // block button held condition
  
  // check to see if this is a clap command
  if (moves[playPnt] == cmdPlayClap) {
    // this is a clap command
    if (moves[playPnt+1] > 1) {
      moves[playPnt+1]--;
      // now simulate a single clap
      if (!servoEn) {attachServos(0);} // in case auto-power off has disabled them
      anyVal = servoVal[3];
      for (int zP = 1; zP <= moves[playPnt+1]; zP++) {
        servoVal[3] = gripClose; servoInst[3].writeMicroseconds(servoVal[3]);
        delay(120);
        servoVal[3] = anyVal; servoInst[3].writeMicroseconds(servoVal[3]);
        delay(120);
      } SyncTimers();
    } else {grabTgts();}  // load the current position
  }
  PrintTx+= "playClapCmd\n";
  PrintTx+= "Claps=" + String(moves[playPnt+1]) + "\n";
  button_BY = true;
}

// ----------------------------------------------------------------------

void playDecDelay() {
  // if a delay command then decrement it to 100ms
  // after that return it to being the current position
  resetRECPause(); // reset the auto-power off counter
  button_BB_Cnt++; if (button_BB_Cnt > 25) {
     // button held down for > 500ms
    button_BB = false;
    button_BB_Cnt = 15; // shorter delay after initial pause
  }
  if (button_BB) return;
  
  // check to see if this a delay command
  // Serial.println("playDelayCmd");
  if (moves[playPnt] == cmdPlayDelay) {
    // this is a delay command
    if (moves[playPnt+1] > 10) {moves[playPnt+1] -= 10;}
    else {grabTgts();}  // load the current position
    // Serial.print("Delay="); Serial.println(moves[playPnt+1]);
  }
  button_BB = true;
}

// ----------------------------------------------------------------------

void playDelete() {
  // delete the current servo positions from the play memory
  // move to the previous position
  // playCnt tracks the total number of elements stored
  resetRECPause();
  if (button_BA) return;
  
  button_BA = true; // stop the action repeating whilst button is down
  if (playCnt == 4) {
    // only one entry so set it to the REST position
    moves[0] = Reset0; moves[1] = Reset1; moves[2] = Reset2; moves[3] = Reset3;
  } else {
    // remove the values by copying down values from above
    for (int zP = playPnt; zP < playCnt; zP++) {
      moves[zP] = moves[zP + 4];
    }
    if (playCnt > 4) {playCnt -= 4;}  // reduce the play count if more than one row
    if (playPnt > 0) {playPnt -= 4;}  // point at previous position if not at 1st row
  }
  // now play the previous position if there was one
  playTask = 2; playRun = true; // play one move only
}

// ----------------------------------------------------------------------

void playEngine() {
  // this code plays a sequence of stored points
  // if playRepeat == true continuous repeat play is actioned
  int zL,zN,zU;

  // PrintTx+= "playEngine\n";
  // PrintTx+= "playTask = " + String(playTask) + "\n";
  PWR_timeout = 1000; // auto-power off set to 10 seconds after sequence
  switch (playTask) {
    case 0: // main playEngine task
      // set servo targets from the current point and run until end
      playLoad();
      if (playPnt == 0) {moveInterval = moveDefInterval; Mult = 100;}  // reset speed to default value
      // ensure that the loadFlag is == 0 for new servo positions
      if (loadFlag < 1) {
        // new servo targets loaded
        // PrintTx+= "playPnt" + String(playPnt) + "\n";
        // PrintTx+= "playCnt" + String(playCnt) + "\n";
        playInvoke(); playTask = 99; playTaskNext = 1;
      } else {
        // if loadFlag > 0 then this is a coded command, so decode it
        playTask = 10; playTaskNext = 1;
      } break;
    case 1:
      // check to see if we have got to the last element
      if (playPnt >= (playCnt - 4)) {
        if (playRepeat) {
          playPnt = 0;  // reset the play pointer and restart the cycle
        } else {playRun = false;} // end playing the sequence
      }
      else {playPnt += 4;} // move onto the next position
      playTask = 0; break;


    case 2: // this is the single stepping feature
      // set servo targets from the current point and only do one move
      playLoad();
      // ensure that the loadFlag is == 0 for new servo positions
      if (loadFlag < 1) {
        // new servo targets loaded
        playInvoke(); playTask = 99; playTaskNext = 3;
      } else {
        // if loadFlag > 0 then this is a coded command so decode it
        playTask = 10; playTaskNext = 3;
      }
      playPause = 0; // an inter-move pause is not necessary as we are stopping after this move
      break;
    case 3:
      // check to see if we have got to the last element
      playTask = 0; playRun = false; // end the sequence
      break;


    //###################################################################
    //
    // respond to commands
    //
    //###################################################################
    case 10: // here we decode the coded command in loadFlag
      // Serial.print(F("loadFlag = ")); Serial.println(loadFlag);
      switch (loadFlag) {
        case cmdPlayAddPnt: // this is an add pint[] command
          playAddPnt[moves[playPnt + 1]] = moves[playPnt + 2];
          if (!APP) {PrintTx+= "AddPnt " + String(moves[playPnt + 1]) + "\t" + String(moves[playPnt + 2]) + "\n";}
          playPnt+= 4;          // move onto the next line
          playTask = 0; break;  // load a move from the new pointer
          
        case cmdPlayClap: // this is a clap n times command
          clapCnt = moves[playPnt + 1]; // get the number of claps
          if (!APP) {PrintTx+= "Claps=" + String(clapCnt) + "\n";}
          playTask = 200; clapTaskNext = playTaskNext;// do claps then goto the next task
          break;
          
        case cmdPlayClapRnd: // this is a random clap min - max times command
          zL = min(moves[playPnt + 1],moves[playPnt + 2]);  // 1st clap reference
          zU = max(moves[playPnt + 1],moves[playPnt + 2]);  // 2nd clap reference
          zN = random(zL,(zU + 1));  // get random clap value
          clapCnt = zN; // get the number of claps
          if (!APP) {PrintTx+= "Claps=" + String(clapCnt) + "\n";}
          playTask = 200; clapTaskNext = playTaskNext;// do claps then goto the next task
          break;
          
        case cmdPlayDelay: // this is a delay in 100ms steps
          playPause = moves[playPnt + 1]; // get the size of the pause
          if (!APP) {PrintTx+= "Delay=" + String(playPause * 10) + "ms\n";}
          playTask = 100; // pause then goto the next task
          break;
          
        case cmdPlayDelRnd: // this is a random delay in 100ms steps
          zL = min(moves[playPnt + 1],moves[playPnt + 2]);  // 1st playAdPnt[] reference
          zU = max(moves[playPnt + 1],moves[playPnt + 2]);  // 2nd playAdPnt[] reference
          zN = random(zL,(zU + 1));  // get random delay value
          playPause = zN; // get the size of the pause
          if (!APP) {PrintTx+= "DelRnd=" + String(playPause * 10) + "ms\n";}
          playTask = 100; // pause then goto the next task
          break;
          
        case cmdPlayFor: // this is a For... command so push stack and number
          if (forPnt < 9) {
            forPnt++;                               // inc stack pointer
            forStack[forPnt] = playPnt + 4;         // store next line pointer
            numStack[forPnt] = moves[playPnt + 1];  // store times number
          } else {playAbort(ErrFor); return;}       // stack overflow error 
          playPnt+= 4;          // move onto the next line
          if (!APP) {PrintTx+= "For " + String(moves[playPnt + 1]) + "\n";}
          playTask = 0; break;  // load a move from the new pointer

        case cmdPlayForRnd: // this is a random For... command so push stack and number
          zL = min(moves[playPnt + 1],moves[playPnt + 2]);  // 1st playAdPnt[] reference
          zU = max(moves[playPnt + 1],moves[playPnt + 2]);  // 2nd playAdPnt[] reference
          zN = random(zL,(zU + 1));  // get random delay value
          if (forPnt < 9) {
            forPnt++;                         // inc stack pointer
            forStack[forPnt] = playPnt + 4;   // store next line pointer
            numStack[forPnt] = zN;            // store random times number
          } else {playAbort(ErrFor); return;} // stack overflow error 
          playPnt+= 4;          // move onto the next line
          if (!APP) {PrintTx+= "ForRnd " + String(zN) + "\n";}
          playTask = 0; break;  // load a move from the new pointer
          
        case cmdPlayGosub: // this is a Gosub command so push stack and go there
          if (gosubPnt < (gosubDepth - 1)) {
            gosubPnt++;                             // inc stack pointer
            gosubStack[gosubPnt] = playPnt + 4;     // store next line pointer
          } else {playAbort(ErrGosub); return;}     // stack overflow error 
          playPnt = MemWidth * moves[playPnt + 1];  // get the new line pointer
          if (!APP) {PrintTx+= "Gosub " + String(moves[playPnt + 1]) + "\n";}
          playTask = 0; break;  // load a move from the new pointer

        case cmdPlayGoto: // this is a Goto command so go there
          playPnt = MemWidth * moves[playPnt + 1];  // get the new line pointer
          if (!APP) {PrintTx+= "Goto " + String(moves[playPnt + 1]) + "\n";}
          playTask = 0; break;  // load a move from the new pointer
          
        case cmdPlayGsbRnd: // this is a Gosub via random AddPnt[] command
          if (gosubPnt < (gosubDepth - 1)) {
            gosubPnt++;                             // inc stack pointer
            gosubStack[gosubPnt] = playPnt + 4;     // store next line pointer
          } else {playAbort(ErrGosub); return;}     // stack overflow error 
          zL = min(moves[playPnt + 1],moves[playPnt + 2]);  // 1st playAdPnt[] reference
          zU = max(moves[playPnt + 1],moves[playPnt + 2]);  // 2nd playAdPnt[] reference
          zN = zL;
          if (zL != zU) {
  label_Redo0:
            zN = random(zL,(zU + 1));        // get random playAddPnt[] pointer
            if (zN == RndLast) {goto label_Redo0;}
          }
          RndLast = zN;
          playPnt = MemWidth * playAddPnt[zN];  // get the new line pointer
          if (!APP) {PrintTx+= "GosubRnd " + String(zN) + "\t" + String(playAddPnt[zN]) + "\n";}
          playTask = 0; break;  // load a move from the new pointer
          
        case cmdPlayGtoRnd: // this is a Goto via random AddPnt[] command
          zL = min(moves[playPnt + 1],moves[playPnt + 2]);  // 1st playAdPnt[] reference
          zU = max(moves[playPnt + 1],moves[playPnt + 2]);  // 2nd playAdPnt[] reference
          zN = zL;
          if (zL != zU) {
  label_Redo1:
            zN = random(zL,(zU + 1));        // get random playAddPnt[] pointer
            if (zL == RndLast) {goto label_Redo1;}
          }
          RndLast = zN;
          playPnt = MemWidth * playAddPnt[zN];  // get the new line pointer
          if (!APP) {PrintTx+= "GotoRnd " + String(zN) + "\t" + String(playAddPnt[zN]) + "\n";}
          playTask = 0; break;  // load a move from the new pointer
          
        case cmdPlayNext: // this is a Next command so pop times value
          if (forPnt >= 0) {
            numStack[forPnt]--;           // decrement the times number
            if (numStack[forPnt] > 0) {
              // still more times to do
              playPnt = forStack[forPnt]; // pop the For.. + 1 line number
            } else {
              forPnt--;                   // times complete, so decrement the stack pointer
              playPnt+= 4;                // move onto the next line
              if (playPnt >= playCnt) {playStopApp(); return;}
            }
          } else {playAbort(ErrNext); return;} // stack empty error 
          if (!APP) {PrintTx+= "Next\n";}
          playTask = 0; break;  // load a move from the new pointer

        case cmdPlayNull: // this is a Null command so skip it
          if (!APP) {PrintTx+= "Null\n";}
          playTask = playTaskNext; break;
          
        case cmdPlayPause: // set the intermove pause, 0 - 100, = 0ms - 1000ms;
          playPauseDef = moves[playPnt + 1]; playPause = playPauseDef; 
          if (!APP) {PrintTx+= "Pause=" + String(playPause) + "\n";}
          playTask = playTaskNext; break;
        
        case cmdPlayReturn: // this is a Gosub Return command so push stack and go there
          if (gosubPnt >= 0) {
            playPnt = gosubStack[gosubPnt]; // pop the line number
            gosubPnt--;}                    // decrement the stack pointer
          else {playAbort(ErrReturn); return;} // stack empty error 
          if (!APP) {PrintTx+= "Return\n";}
          playTask = 0; break;  // load a move from the new pointer
          
        case cmdPlaySpeed: // this is an 'Speed' % command
          playSpeed = moves[playPnt + 1]; // get the speed value
          Mult = 10000/playSpeed;
          if (!APP) {PrintTx+= "Speed=" + String(playSpeed) + "%\n";}
          playTask = playTaskNext; break; // continue to next task
          
        case cmdPlaySpeedRnd: // this is a random speed min - max % command
          zL = min(moves[playPnt + 1],moves[playPnt + 2]);  // 1st speed reference
          zU = max(moves[playPnt + 1],moves[playPnt + 2]);  // 2nd speed reference
          zN = random(zL,(zU + 1));  // get random speed value
          playSpeed = zN; // get the random speed value
          Mult = 10000/playSpeed;
          if (!APP) {PrintTx+= "Speed=" + String(playSpeed) + "%\n";}
          playTask = playTaskNext; break; // continue to next task

        default:
          // we should never get here, but just in case, handle it
          playTask = 1; break;
      } break;


    case 99:
      // wait for the servo movement to complete, then return to a defined task
      if ((moveCnt < 1) && !ValChg) {
        ValChg = true;  // update servos once more
        if (playPause > 0) {playTask++;} // include an inter-move pause
        else {playTask = playTaskNext;} // don't include an inter-move pause
      } break;
    case 100: // this is also a pause task called from other tasks
      // inter-move pause before next move
      if (playPause < 1) {playTask = playTaskNext;}
      playPause--;
      // Serial.println(playPause);
      break;

    case 200: // this provide the clap n times function
      // we determine the open/closed start condition and end in that state
      if (servoVal[3] < (gripClose + 50)) {
        // jaws are already closed so change process to open first
        playTask = 205;
        // PrintTx += "Clap from closed\n";
      } else {
        // jaws are open to start with
        playTask = 201;
        // PrintTx += "Clap from open\n";
      } break;
    case 201:
      // jaws are open to start with so close the jaws
      servoVal[3] = gripClose; servoInst[3].writeMicroseconds(servoVal[3]);
      if (!APP) {printServoVals();}
      playWait = 12; playTask = 210; playTaskNext = 202;
      break;
    case 202: // open the jaws, returning them to the previous open position
      servoVal[3] = LSV3; servoInst[3].writeMicroseconds(servoVal[3]);
      if (!APP) {printServoVals();}
      playWait = 12; playTask = 210; playTaskNext = 203;
      break;
    case 203: // reduce clapCnt and see if we have finished
      clapCnt--;
      if (clapCnt < 1) {playTask = clapTaskNext;} // finished clapping
      else {playTask = 201;} // clap again
      break;
    case 205:
      // open the jaws
      servoVal[3] = gripOpen; servoInst[3].writeMicroseconds(servoVal[3]);
      if (!APP) {printServoVals();}
      playWait = 12; playTask = 210; playTaskNext = 206;
      break;
    case 206: // close the jaws, returning them to the start closed position
      servoVal[3] = LSV3; servoInst[3].writeMicroseconds(servoVal[3]);
      if (!APP) {printServoVals();}
      playWait = 12; playTask = 210; playTaskNext = 207;
      break;
    case 207: // reduce clapCnt and see if we have finished
      clapCnt--;
      if (clapCnt < 1) {playTask = clapTaskNext;} // finished clapping
      else {playTask = 205;} // clap again
      break;
    case 210: // this is an n cycle wait function, returning to playTaskNext
      playWait--; if (playWait < 1) {playTask = playTaskNext;}
      break;
  }
}

// ----------------------------------------------------------------------

void playInit() {
  // called when entering Record mode
  // initialises all variables to correct default state
  PrintTx+= "\nplayInit\n";
  moveSTOP(); // stop any Move Engine movement
  playRun = false; // stop any Play Engine movement
  playPause = playPauseDef; // inter-move pause in 10ms loops; hence 100ms
  playPnt = 0; // reset the play pointer
  playTask = 0; // clear the playEngine task pointer
  
  // clear the move array by filling it with zeros
  // this effectively sets all values to the Home position
  for (anyVal = 0; anyVal < moveArraySize; anyVal++) {moves[anyVal] = 0;}

  // put the RESET position in the first element
  moves[playPnt] = Reset0 - Home0;
  moves[playPnt+1] = Reset1 - Home1;
  moves[playPnt+2] = Reset2 - Home2;
  moves[playPnt+3] = Reset3 - Home3;
  playCnt = 4; snapshot[0] = 5000;
}

// ----------------------------------------------------------------------

void playInvoke() {
  // called from play engine to set the move counter and 
  moveInterval = moveDefInterval;
  moveIntervalInc = 0; moveIntervalMin = moveDefInterval;

  // calculate number of steps needed for the move looking at 3 target values
  // the maximum figure is used
  long zVal;
  moveCnt = 5; // absolute minimum number of steps, and for claw only moves
  zVal = abs(servoTgt[0] - servoVal[0]); zVal = (Mult * zVal)/(turntableMax - turntableMin);
  moveCnt = max(moveCnt,int(zVal));
  zVal = abs(servoTgt[1] - servoVal[1]); zVal = (Mult * zVal)/(fwdArmMax - fwdArmMin);
  moveCnt = max(moveCnt,int(zVal));
  zVal = abs(servoTgt[2] - servoVal[2]); zVal = (Mult * zVal)/(vertArmMaxB - vertArmMinA);
  moveCnt = max(moveCnt,int(zVal));
  // Serial.print("moveCnt = "); Serial.println(moveCnt);
  if (!APP) {printServoVals();}
}

// ----------------------------------------------------------------------

void playLoad() {
  // transfer values from memory into servo targets if a valid move
  // or load the coded command
  // note values are stored relative to 'Home' positions, so those values
  // need to be added to the target position
  // Serial.println("targets loaded");
  playPause = playPauseDef; // set default inter-move pause to 100ms on every load
  if (moves[playPnt] < 5000) {
    // this is a valid move so load target values
    loadFlag = 0; // indicate that servo values have been loaded
    // LSV0 = servoTgt[0]; LSV1 = servoTgt[1]; LSV2 = servoTgt[2]; LSV3 = servoTgt[3];
    servoTgt[0] = moves[playPnt] + Home0;
    servoTgt[1] = moves[playPnt+1] + Home1;
    servoTgt[2] = moves[playPnt+2] + Home2;
    servoTgt[3] = moves[playPnt+3] + Home3;
    // check target limits for rogue values
    resetServoLimits();
    for (int zP = 0; zP < 4; zP++) {
      if (servoTgt[zP] > servoMax[zP]) {servoTgt[zP] = servoMax[zP];}
      if (servoTgt[zP] < servoMin[zP]) {servoTgt[zP] = servoMin[zP];}
    }
    // remember the current target positions which will be moved to
    // as these can be used by other functions like playAdd
    LSV0 = servoTgt[0]; LSV1 = servoTgt[1]; LSV2 = servoTgt[2]; LSV3 = servoTgt[3];
  } else {
    // moves[playPnt] is > turntableMax, so it must be a coded command
    loadFlag = moves[playPnt]; // grab the coded command
  }
  // reportPlayTgts();
  // PrintTx+="playLoad\n";
  if (APP) {PrintTx+= "MT" + String(playPnt/MemWidth) + "\n";}
}

// ----------------------------------------------------------------------

void playMemLimit() {
  // called when memory limit is reached
  Display_Text2x16("MEMORY","LIMIT");
  DispNOP = false; DispDel = 50;
}

// ----------------------------------------------------------------------

void playMemLoad(int zV0,int zV1,int zV2,int zV3) {
  // loads the values passed into moves[] memory and increments the counter
  // values store are relative to calibrated 'Home' positions
  if (playCnt >= moveArraySize) {return;} // don't overfill
  moves[playCnt] = zV0; playCnt++;
  moves[playCnt] = zV1; playCnt++;
  moves[playCnt] = zV2; playCnt++;
  moves[playCnt] = zV3; playCnt++;
}

// ----------------------------------------------------------------------

void playNext() {
  // play the next move in the sequence
  resetRECPause();
  if (button_BDR) {return;}
  
  if (playPnt == (playCnt - 4)) {playPnt = 0;} // at the last position
  else {playPnt += 4;} // increment the pointer
  playTask = 2; playRun = true; // play one move only
  button_BDR = true;
}

// ----------------------------------------------------------------------

void playOnce() {
  // invoked by the move trainer app to play the contents of the memory  
  playCnt = MemMax * 4; playPnt = 0; playTask = 0;
  playRepeat = false; playRun = true;
  // PrintTx+= "playOnce\n";
  // PrintTx+= "playCnt" + String(playCnt) + "\n";
}

// ----------------------------------------------------------------------

void playOverwrite() {
  // overwrite the current servo positions to the play memory
  // playPnt is pointing at the current 'played' memory element
  // playCnt tracks the total number of elements stored
  resetRECPause();
  if (button_BB) return;
  button_BB = true;  
  if (playPnt == 0) {
    // playPnt is at the start of the sequence
    if (playCnt == 4) {
      // an empty sequence so add the new value to the end and extend the count
      // this insertion can only occur once
      playPnt += 4; playCnt += 4;
    } else {
      // we can't overwrite the Reset position
      return;
    }
  }
  // now add in the new values
  PrintTx+= "Overwriting data\n";
  moves[playPnt] = servoVal[0] - Home0;
  moves[playPnt+1] = servoVal[1] - Home1;
  moves[playPnt+2] = servoVal[2] - Home2;
  moves[playPnt+3] = servoVal[3] - Home3;
}

// ----------------------------------------------------------------------

void playPausePlay() {
  // invoked by the move trainer app to either play/pause the contents of the memory  
  // if continuous playRepeat == true then this will also apply
  playCnt = MemMax * 4;
  if (playRun) {
    // currently playing so pause it
    // however the current move to target will complete
    playRun = false;
  } else {
    // not playing so start it if something is stored in memory
    // as reset position is stored in first element it will at least be played
    // by default Reset position is played, then the rest
    if ((playPnt > 0) && (playPnt == (playCnt - 4))) {
      // at end of sequence so restart it
      playPnt = 0;
    }
    playTask = 0; playRun = true;
  }
  // PrintTx+= "playPause\n";
  // PrintTx+= "playCnt" + String(playCnt) + "\n";
}

// ----------------------------------------------------------------------

void playPrevious() {
  // play the previous move in the sequence
  resetRECPause();
  if (button_BDL) {return;}
  playPnt -= 4; // decrement pointer
  if (playPnt < 0) {playPnt = playCnt - 4;} // fallen off the bottom
  playTask = 2; playRun = true; // play one move only
  button_BDL = true;
}

// ----------------------------------------------------------------------

void playRow() {
  // play a single move MemRow target position as determined by the user
  // scrolling through the Move Trainer list
  // branch instructions like Goto, Gosub, etc are ignored
  resetRECPause();
  
  playPnt = MemRow * MemWidth;
  // list of commands to be ignored
  if (moves[playPnt] == cmdPlayAddPnt) {return;}
  if (moves[playPnt] == cmdPlayDelay) {return;}
  if (moves[playPnt] == cmdPlayDelRnd) {return;}
  if (moves[playPnt] == cmdPlayFor) {return;}
  if (moves[playPnt] == cmdPlayForRnd) {return;}
  if (moves[playPnt] == cmdPlayGoto) {return;}
  if (moves[playPnt] == cmdPlayGosub) {return;}
  if (moves[playPnt] == cmdPlayGsbRnd) {return;}
  if (moves[playPnt] == cmdPlayGtoRnd) {return;}
  if (moves[playPnt] == cmdPlayNext) {return;}
  if (moves[playPnt] == cmdPlayReturn) {return;}
  if (moves[playPnt] == cmdPlayPause) {return;}
  
  playTask = 2; playRun = true; // play one move only
}

// ----------------------------------------------------------------------

void playSnapshot() {
  // record the current position as a snapshot position
  if (button_BX) {return;}
  
  snapshot[0] = servoVal[0]; snapshot[1] = servoVal[1];
  snapshot[2] = servoVal[2]; snapshot[3] = servoVal[3];
  // Serial.println(F("playSnapshot"));
  button_BX = true;
}

// ----------------------------------------------------------------------

void playSpeedDwn() {
  // make the current point a speed command
  // if already a speed command then increment it
  // if button held down then progressively increase the value
  // function called every 20ms, first speed is 90%, then -5% increments to 10%
  resetRECPause(); // reset the auto-power off counter
  button_BDL_Cnt++; if (button_BDL_Cnt > 25) {
     // button held down for > 500ms
    button_BDL = false;
    button_BDL_Cnt = 15; // shorter delay after initial pause
  }
  if (button_BDL) return;
  
  button_BDL = true;
  
  // check to see if this is add a point or add a delay
  // Serial.println("cmdPlaySpeed");
  if (moves[playPnt] != cmdPlaySpeed) {
      // we have not yet created a speed code command so insert one
      moves[playPnt] = cmdPlaySpeed;  // insert the command code
      moves[playPnt+1] = 90;          // set initial value of 90%
  } else {
    // already a delay so inc 100ms up to 9.99 seconds
    if (moves[playPnt+1] > 10) {moves[playPnt+1] -= 5;}
    // Serial.print("Speed="); Serial.println(moves[playPnt+1]);
  }
}

// ----------------------------------------------------------------------

void playSpeedUp() {
  // make the current point a speed command
  // if already a speed command then decrement it
  // if button held down then progressively decrease the value
  // function called every 20ms, first speed is 110%, then +5% increments to 300%
  resetRECPause(); // reset the auto-power off counter
  button_BDR_Cnt++; if (button_BDR_Cnt > 25) {
     // button held down for > 500ms
    button_BDR = false;
    button_BDR_Cnt = 15; // shorter delay after initial pause
  }
  if (button_BDR) return;
  
  button_BDR = true;
  
  // check to see if this is add a point or add a delay
  // Serial.println("cmdPlaySpeed");
  if (moves[playPnt] != cmdPlaySpeed) {
      // we have not yet created a speed code command so insert one
      moves[playPnt] = cmdPlaySpeed;  // insert the command code
      moves[playPnt+1] = 110;         // set initial value of 110%
  } else {
    // already a delay so inc 100ms up to 9.99 seconds
    if (moves[playPnt+1] < 300) {moves[playPnt+1] += 5;}
    // Serial.print("Speed="); Serial.println(moves[playPnt+1]);
  }
}

// ----------------------------------------------------------------------

void playStop() {
  // stop the current sequence and return to the reset position  
  if (button_BDD) {return;}
  
  playPnt = 0;        // reset pointer to the start of the sequence
  playRepeat = false; // clear the repeat flag
  PWR_timeout = 200; playTask = 2; playRun = true;
  button_BDD = true;
}

// ----------------------------------------------------------------------

void playStopApp() {
  // invoked by the Move Trainer app
  // stop the current sequence and return to the reset position  
  playPnt = 0;        // reset pointer to the start of the sequence
  playRepeat = false; // clear the repeat flag
  PWR_timeout = 200;
  // load the reset position into memory
  // we over-write memory, but the App will reload it as needed
  moves[0] = Reset0 - Home0;
  moves[1] = Reset1 - Home1;
  moves[2] = Reset2 - Home2;
  moves[3] = Reset3 - Home3;
  playCnt = 4; playTask = 2; playRun = true;
}

// ----------------------------------------------------------------------

void playToSnapshot() {
  // return to the previous snapshot position
  if (button_BX) {return;}
  if (snapshot[0] >= 5000) {return;} // no snapshot position recorded

  // store snapshot values as current position
  moves[playPnt] = snapshot[0] - Home0;
  moves[playPnt+1] = snapshot[1] - Home1;
  moves[playPnt+2] = snapshot[2] - Home2;
  moves[playPnt+3] = snapshot[3] - Home3;
  
  // move to snapshot position
  moveSTOP();
  servoTgt[0] = snapshot[0]; servoTgt[1] = snapshot[1];
  servoTgt[2] = snapshot[2]; servoTgt[3] = snapshot[3];
  // Serial.println("playToSnapshot");
  moveInvoke(); PWR_timeout = 200;
  button_BX = true;
}

// ----------------------------------------------------------------------

void playXcuteBDD() {
  // a routine initiated by the Down digital pad button
  if (button_BDD) {return;}
  PrintTx+= "playXcuteBDD\n";
  playInit(); // clear flags and moves[] memory

  // now load the play values into memory
  playMemLoad(5012,10,0,0);       // inter-move pause = 100 ms
  playMemLoad(0,-545,0,0);        // RESET
  playMemLoad(0,453,-751,780);    // Drop forward + jaws open
  playMemLoad(5002,3,0,0);        // 3 claps from open
  playMemLoad(562,621,-589,0);    // Swing left + jaws closed
  playMemLoad(562,621,-589,555);  // Open jaws
  playMemLoad(562,621,-589,-591); // Close jaws
  playMemLoad(562,85,315,0);      // Rise up
  playMemLoad(-662,85,315,0);     // Swing right
  playMemLoad(-662,621,-589,525); // Drop down + jaws open
  playMemLoad(-662,621,-589,-546);// Close jaws
  playMemLoad(22,453,-751,636);   // Swing centre + jaws open
  playMemLoad(5002,1,0,0);        // 1 clap from open
  playMemLoad(22,453,-751,-393);  // Close jaws
  playMemLoad(0,-545,0,0);        // RESET

  // now play the contents of the memory
  PWR_timeout = 200; playRun = true;
  button_BDD = true;
}

// ----------------------------------------------------------------------

void playXcuteBDL() {
  // a routine initiated by the Left digital pad button
  if (button_BDL) {return;}
  PrintTx+= "playXcuteBDL\n";
  playInit(); // clear flags and moves[] memory

  // now load the play values into memory
  playMemLoad(0,-545,0,0);        // RESET
  playMemLoad(230,245,-212,620);  // 
  playMemLoad(5002,3,0,0);        // 
  playMemLoad(0,-545,0,0);        // RESET

  // now play the contents of the memory
  PWR_timeout = 200; playRun = true;
  button_BDL = true;
}

// ----------------------------------------------------------------------

void playXcuteBDR() {
  // a routine initiated by the Right digital pad button
  if (button_BDR) {return;}
  PrintTx+= "playXcuteBDR\n";
  playInit(); // clear flags and moves[] memory

  // now load the play values into memory
  playMemLoad(0,-545,0,0);        // RESET
  playMemLoad(-226,129,-309,658); // 
  playMemLoad(5002,1,0,0);        // 
  playMemLoad(0,-545,0,0);        // RESET

  // now play the contents of the memory
  PWR_timeout = 200; playRun = true;
  button_BDR = true;
}

// ----------------------------------------------------------------------

void playXcuteBDU() {
  // a routine initiated by the Up digital pad button
  if (button_BDU) {return;}
  PrintTx+= "playXcuteBDU\n";
  playInit(); // clear flags and moves[] memory

  // now load the play values into memory
  playMemLoad(0,-545,0,0);        // RESET
  playMemLoad(0,127,-250,620);    // 
  playMemLoad(5002,2,0,0);        // 
  playMemLoad(0,-545,0,0);        // RESET

  // now play the contents of the memory
  PWR_timeout = 200; playRun = true;
  button_BDU = true;
}

// ----------------------------------------------------------------------

void playXcuteDemo0() {
  // a routine initiated by the LT + SELECT buttons
  // this is the card game 5-station demo
  if (button_SEL) {return;}
  
  PrintTx+= "playXcuteDemo0\n";
  playInit();   // clear flags and moves[] memory
  playCnt = 0;  // zero the count for loading new data
  Display_Text2x16("SELECT","DEMO"); DispNOP = false; DispDel = 50; DispCnt = 0;

  // now load the play values into memory
  // Record memory count = 512
  // Home0 = 1500
  // Home1 = 1318
  // Home2 = 1922
  // Home3 = 1109
  playMemLoad(5012,10,0,0);       // inter-move pause = 100 ms
  playMemLoad(0,-545,0,0);        // REST
  playMemLoad(5001,200,0,0);      // pause for 2.0 sec
  playMemLoad(303,286,-814,417);  // Moves to start 1
  playMemLoad(5002,3,0,0);        // 3 claps from open
  playMemLoad(5001,500,0,0);      // pause for 5.0 sec
  playMemLoad(303,598,-780,417);  // Moves to piece
  playMemLoad(303,598,-780,0);    // Grabs piece
  playMemLoad(303,418,-672,0);    // Picks up piece
  
  playMemLoad(5000,7,0,0);        // Stand 1 - Stamp
  playMemLoad(201,358,-699,0);    // Aligns with stand 1
  playMemLoad(201,436,-384,0);    // Moves to stand 1
  playMemLoad(201,619,-561,0);    // Place on stand
  playMemLoad(5001,10,0,0);       // pause for 100 ms
  playMemLoad(201,619,-561,384);  // Open jaws
  playMemLoad(201,457,-741,384);  // Back away
  playMemLoad(186,376,-234,513);  // Move to stamp stand
  playMemLoad(186,520,-108,513);  // Reach forward
  playMemLoad(5002,3,0,0);        // 3 claps from open
  playMemLoad(5001,20,0,0);       // pause for 200 ms
  playMemLoad(186,283,-333,513);  // Back away
  playMemLoad(186,454,-732,513);  // Drop to opposite piece
  playMemLoad(186,604,-537,513);  // Move to piece
  playMemLoad(186,604,-537,0);    // Grap piece
  playMemLoad(186,442,-399,0);    // Pick up piece
  playMemLoad(186,61,-528,0);     // Back away
  
  playMemLoad(5000,7,0,0);        // Stand 2 - Laser stand
  playMemLoad(105,61,-528,0);     // Swing right
  playMemLoad(105,493,-423,0);    // Move over stand 2
  playMemLoad(105,619,-561,0);    // Place on stand
  playMemLoad(5001,10,0,0);       // pause for 100 ms
  playMemLoad(105,619,-561,348);  // Open jaws
  playMemLoad(105,361,-705,348);  // Back away
  playMemLoad(105,307,-402,348);  // Move up
  playMemLoad(105,571,-243,348);  // Move to laser stand
  playMemLoad(99,571,-174,-312);  // Close jaws
  playMemLoad(5001,180,0,0);      // pause for 1.8 sec
  playMemLoad(99,571,-174,348);   // Open jaws
  playMemLoad(105,307,-402,348);  // Back away
  playMemLoad(105,454,-732,513);  // Drop down
  playMemLoad(105,604,-537,513);  // Move to piece
  playMemLoad(105,604,-537,0);    // Grap piece
  playMemLoad(105,442,-399,0);    // Pick up piece
  playMemLoad(105,61,-528,0);     // Back away
  
  playMemLoad(5000,7,0,0);        // Stand 3 - Drill stand
  playMemLoad(0,280,-726,0);      // Swing right
  playMemLoad(0,493,-423,0);      // Move over stand 3
  playMemLoad(0,619,-561,0);      // Place on stand
  playMemLoad(5001,10,0,0);       // pause for 100 ms
  playMemLoad(0,619,-561,348);    // Open jaws
  playMemLoad(0,361,-705,348);    // Back away
  playMemLoad(5007,3,0,0);        // do the following 3 times
  playMemLoad(0,247,-288,348);    // Move opposite paddle wheel
  playMemLoad(-129,247,-288,348); // Move to right side
  playMemLoad(5003,150,0,0);      // 150% default speed
  playMemLoad(-129,367,-207,126); // Reach forward
  playMemLoad(114,367,-207,126);  // Swing left quickly
  playMemLoad(5003,100,0,0);      // 100% default speed
  playMemLoad(114,211,-288,126);  // Move back
  playMemLoad(5008,1,0,0);        // repeat the For...Next loop
  playMemLoad(0,247,-288,348);    // Move opposite paddle wheel
  playMemLoad(0,361,-705,348);    // Move down
  playMemLoad(0,610,-534,348);    // Move to piece
  playMemLoad(0,610,-534,0);      // Grap piece
  playMemLoad(0,436,-420,0);      // Pick up piece
  playMemLoad(0,205,-570,0);      // Back away
  
  playMemLoad(5000,7,0,0);        // Stand 4 - Paint Booth
  playMemLoad(-96,61,-528,0);     // Swing right
  playMemLoad(-96,493,-423,0);    // Move over stand 2
  playMemLoad(-96,619,-561,0);    // Place on stand
  playMemLoad(5001,10,0,0);       // pause for 100 ms
  playMemLoad(-126,619,-561,348); // Open jaws
  playMemLoad(-96,361,-705,348);  // Back away
  playMemLoad(-96,307,-402,348);  // Move up
  playMemLoad(-117,583,-201,270); // Move to laser stand
  playMemLoad(-96,571,-174,-312); // Close jaws
  playMemLoad(5001,300,0,0);      // pause for 3.0 sec
  playMemLoad(-96,571,-174,348);  // Open jaws
  playMemLoad(-96,307,-402,348);  // Back away
  playMemLoad(-96,454,-732,513);  // Drop down
  playMemLoad(-96,604,-537,513);  // Move to piece
  playMemLoad(-96,604,-537,0);    // Grap piece
  playMemLoad(-96,442,-399,0);    // Pick up piece
  playMemLoad(-96,61,-528,0);     // Back away
  
  playMemLoad(5000,7,0,0);        // Stand 5 - Press
  playMemLoad(-207,205,-570,0);   // Swing right
  playMemLoad(-207,514,-462,0);   // Move over stand 5
  playMemLoad(-207,619,-561,0);   // Place on stand
  playMemLoad(-236,607,-549,336); // Open jaws
  playMemLoad(-236,421,-669,336); // Back away
  playMemLoad(-212,217,-306,0);   // Move opposite press
  playMemLoad(5007,3,0,0);        // do the following 3 times
  playMemLoad(5003,150,0,0);      // 150% default speed
  playMemLoad(-212,436,-123,0);   // Move above press handle
  playMemLoad(-212,466,-330,0);   // Press handle down
  playMemLoad(5003,100,0,0);      // 100% default speed
  playMemLoad(5008,1,0,0);        // repeat the For...Next loop
  playMemLoad(-212,160,-258,303); // Back away
  playMemLoad(-236,550,-615,303); // Move down
  playMemLoad(-236,607,-549,303); // Move to piece
  playMemLoad(-236,607,-549,0);   // Grab piece
  playMemLoad(-236,427,-399,0);   // Pick up piece
  playMemLoad(-236,217,-711,0);   // Back away
  
  playMemLoad(5000,7,0,0);        // Finished goods
  playMemLoad(-318,217,-711,0);   // Swing right
  playMemLoad(-318,451,-732,0);   // Move to Finished Goods
  playMemLoad(-318,601,-807,0);   // Place on stand
  playMemLoad(-318,601,-807,399); // Open jaws
  playMemLoad(-318,325,-826,399); // Back away
  playMemLoad(5000,7,0,0);        // Centre
  playMemLoad(0,-143,278,399);    // Move centre high
  playMemLoad(5002,3,0,0);        // 3 claps from open
  playMemLoad(0,-143,278,0);      // Close jaws
  playMemLoad(0,-545,0,0);        // REST
  playMemLoad(5001,200,0,0);      // pause for 2.0 sec
  
  playMemLoad(5000,7,0,0);        // Collect from finished
  playMemLoad(-354,-545,0,0);     // Swing right
  playMemLoad(-354,511,-881,411); // Move to Finished Goods
  playMemLoad(-348,628,-794,411); // Move to piece
  playMemLoad(-348,628,-794,0);   // Grab piece
  playMemLoad(-348,439,-707,0);   // Pick up piece
  playMemLoad(0,160,-674,0);      // Move back centre
  
  playMemLoad(5000,7,0,0);        // Loading Bay
  playMemLoad(303,439,-707,0);    // Swing left to stand 1
  playMemLoad(303,580,-803,0);    // Put piece on stand
  playMemLoad(303,580,-803,354);  // Open jaws
  playMemLoad(303,328,-827,354);  // Back away
  playMemLoad(5004,3,0,0);        // go to line 3 next

  // Recorded 128 element(s). 25% Max total = 500
  // End

  // now play the contents of the memory
  PWR_timeout = 200; playRun = true;
  button_SEL = true;
}

// ----------------------------------------------------------------------

void playXcuteDemo1() {
  // a routine initiated by the LT + HOME buttons
  if (button_HME) {return;}
  
  PrintTx+= "playXcuteDemo1\n";
  playInit();   // clear flags and moves[] memory
  playCnt = 0;  // zero the count for loading new data
  Display_Text2x16("HOME","DEMO"); DispNOP = false; DispDel = 50; DispCnt = 0;

  // now load the play values into memory
  // Record memory count = 540
  // Home0 = 1500
  // Home1 = 1318
  // Home2 = 1922
  // Home3 = 1109
  playMemLoad(5012,10,10,0);      // inter-move pause = 100 ms
  playMemLoad(5009,0,36,0);       // pointer[0] is set to line 36
  playMemLoad(5009,1,54,0);       // pointer[1] is set to line 54
  playMemLoad(5009,2,73,0);       // pointer[2] is set to line 73
  playMemLoad(5009,3,96,0);       // pointer[3] is set to line 96
  playMemLoad(5009,4,115,0);      // pointer[4] is set to line 115
  playMemLoad(0,-545,0,0);        // REST
  playMemLoad(5001,200,0,0);      // pause for 2.0 sec
  playMemLoad(303,286,-814,417);  // Moves to start 1
  playMemLoad(5002,3,0,0);        // 3 claps
  playMemLoad(5001,500,0,0);      // pause for 5.0 sec
  playMemLoad(303,598,-780,417);  // Moves to piece
  playMemLoad(303,598,-780,0);    // Grabs piece
  playMemLoad(303,418,-672,0);    // Picks up piece
  playMemLoad(5014,2,5,0);        // random For...2 - 5 times
  playMemLoad(5010,0,4,0);        // gosub via random Pnt[0 - 4]
  playMemLoad(5008,1,1,0);        // repeat the For...Next loop
  playMemLoad(-318,451,-732,0);   // Move to Finished Goods
  playMemLoad(-318,601,-807,0);   // Place on pallet
  playMemLoad(-318,601,-807,399); // Open jaws
  playMemLoad(-318,325,-826,399); // Back away
  playMemLoad(0,-545,0,399);      // Rest back jaws open
  playMemLoad(5002,3,0,0);        // 3 claps
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,-545,0,-606);     // Close jaws
  playMemLoad(5013,100,300,0);    // rnd pause 1.0 sec - 3.0 sec
  playMemLoad(-318,325,-826,399); // Aligns with finished goods
  playMemLoad(-318,601,-807,399); // Moves to piece
  playMemLoad(-318,601,-807,-603);// Closes jaws
  playMemLoad(-318,451,-732,0);   // Picks up piece
  playMemLoad(303,451,-732,0);    // Swing left to loading
  playMemLoad(303,610,-822,0);    // Lower piece
  playMemLoad(303,610,-822,417);  // Open jaws
  playMemLoad(303,361,-753,417);  // Back away
  playMemLoad(5004,6,0,0);        // go to line 6 next

  playMemLoad(5000,3,0,0);        // 

  playMemLoad(5000,7,0,0);        // Stand 1 - Stamp Stand
  playMemLoad(190,358,-699,0);    // Aligns with stand 1
  playMemLoad(190,436,-384,0);    // Moves to stand 1
  playMemLoad(190,619,-561,0);    // Place on stand
  playMemLoad(5001,10,0,0);       // pause for 100 ms
  playMemLoad(190,619,-561,384);  // Open jaws
  playMemLoad(190,457,-741,384);  // Back away
  playMemLoad(186,376,-234,513);  // Move to flag stand
  playMemLoad(186,520,-108,513);  // Reach forward
  playMemLoad(5015,1,4,0);        // random clap...1 - 4 times
  playMemLoad(5001,20,0,0);       // pause for 200 ms
  playMemLoad(190,283,-333,513);  // Back away
  playMemLoad(190,454,-732,513);  // Drop to opposite piece
  playMemLoad(190,604,-537,513);  // Move to piece
  playMemLoad(190,604,-537,0);    // Grap piece
  playMemLoad(190,442,-399,0);    // Pick up piece
  playMemLoad(190,358,-699,0);    // Back away
  playMemLoad(5006,30,0,0);       // return Gosub line + 1

  playMemLoad(5000,7,0,0);        // Stand 2 - Laser Stand
  playMemLoad(100,358,-699,0);    // Aligns with stand 2
  playMemLoad(100,436,-384,0);    // Moves to stand 2
  playMemLoad(100,619,-561,0);    // Place on stand
  playMemLoad(5001,10,0,0);       // pause for 100 ms
  playMemLoad(100,619,-561,384);  // Open jaws
  playMemLoad(100,457,-741,384);  // Back away
  playMemLoad(92,274,-426,384);   // Move up
  playMemLoad(92,577,-204,345);   // Reach for Laser Stand
  playMemLoad(92,577,-246,-231);  // Grip Laser Stand
  playMemLoad(5001,100,0,0);      // pause for 1.0 sec
  playMemLoad(100,577,-246,327);  // Release
  playMemLoad(100,283,-453,384);  // Back away
  playMemLoad(100,457,-741,384);  // Drop down
  playMemLoad(100,604,-537,513);  // Move to piece
  playMemLoad(100,604,-537,0);    // Grap piece
  playMemLoad(100,442,-399,0);    // Pick up piece
  playMemLoad(100,358,-699,0);    // Back away
  playMemLoad(5006,30,0,0);       // return Gosub line + 1

  playMemLoad(5000,7,0,0);        // Stand 3 - Drill Stand
  playMemLoad(0,358,-699,0);      // Aligns with stand 3
  playMemLoad(0,493,-423,0);      // Move over stand 3
  playMemLoad(0,619,-561,0);      // Place on stand
  playMemLoad(5001,10,0,0);       // pause for 100 ms
  playMemLoad(0,619,-561,348);    // Open jaws
  playMemLoad(0,361,-705,348);    // Back away
  playMemLoad(5014,1,4,0);        // random For...1 - 4 times
  playMemLoad(0,247,-288,348);    // Move opposite paddle wheel
  playMemLoad(-129,247,-288,348); // Move to right side
  playMemLoad(5003,150,0,0);      // 150% of default speed
  playMemLoad(-129,367,-207,126); // Reach forward
  playMemLoad(114,367,-207,126);  // Swing left quickly
  playMemLoad(5003,100,0,0);      // 100% of default speed
  playMemLoad(114,211,-288,126);  // Move back
  playMemLoad(5008,1,0,0);        // repeat the For...Next loop
  playMemLoad(0,247,-288,348);    // Move opposite paddle wheel
  playMemLoad(0,361,-705,348);    // Move down
  playMemLoad(0,610,-534,348);    // Move to piece
  playMemLoad(0,610,-534,0);      // Grap piece
  playMemLoad(0,436,-420,0);      // Pick up piece
  playMemLoad(0,358,-699,0);      // Back away
  playMemLoad(5006,53,0,0);       // return Gosub line + 1

  playMemLoad(5000,7,0,0);        // Stand 4 - Paint Stand
  playMemLoad(-116,358,-699,0);   // Aligns with stand 4
  playMemLoad(-116,436,-384,0);   // Moves to stand 4
  playMemLoad(-116,619,-561,0);   // Place on stand
  playMemLoad(5001,10,0,0);       // pause for 100 ms
  playMemLoad(-116,619,-561,384); // Open jaws
  playMemLoad(-116,457,-741,384); // Back away
  playMemLoad(-116,325,-486,384); // Move up
  playMemLoad(-116,604,-177,225); // Reach for Paint Stand
  playMemLoad(-116,604,-177,-315);// Close jaws
  playMemLoad(5013,200,300,0);    // rnd pause 2.0 sec - 3.0 sec
  playMemLoad(-116,604,-177,225); // Open jaws
  playMemLoad(-116,268,-501,225); // Back away
  playMemLoad(-116,487,-795,225); // Move down
  playMemLoad(-116,604,-537,513); // Move to piece
  playMemLoad(-116,604,-537,0);   // Grap piece
  playMemLoad(-116,442,-399,0);   // Pick up piece
  playMemLoad(-116,358,-699,0);   // Back away
  playMemLoad(5006,30,0,0);       // return Gosub line + 1

  playMemLoad(5000,7,0,0);        // Stand 5 - Press Stand
  playMemLoad(-210,358,-699,0);   // Aligns with stand 5
  playMemLoad(-210,514,-462,0);   // Move opposite paddle wheel
  playMemLoad(-210,619,-561,0);   // Place on stand
  playMemLoad(-210,607,-549,336); // Open jaws
  playMemLoad(-210,421,-669,336); // Back away
  playMemLoad(-212,217,-306,0);   // Move opposite press
  playMemLoad(5014,1,4,0);        // random For...1 - 4 times
  playMemLoad(5003,150,0,0);      // 150% of default speed
  playMemLoad(-210,436,-123,0);   // Move above press handle
  playMemLoad(-210,466,-330,0);   // Press handle down
  playMemLoad(5003,100,0,0);      // 100% of default speed
  playMemLoad(5008,1,0,0);        // repeat the For...Next loop
  playMemLoad(-210,160,-258,303); // Back away
  playMemLoad(-210,550,-615,303); // 3 claps from open
  playMemLoad(-210,607,-549,303); // Move to piece
  playMemLoad(-210,607,-549,0);   // Grab piece
  playMemLoad(-210,427,-399,0);   // Back away
  playMemLoad(-210,358,-699,0);   // Back away
  playMemLoad(5006,72,0,0);       // return Gosub line + 1

  // Recorded 135 element(s). 27% Max total = 500
  // End

  // now play the contents of the memory
  PWR_timeout = 200; playRun = true;
  button_HME = true;
}

// ----------------------------------------------------------------------

void playXcuteDemo2() {
  // a routine initiated by the LT + START buttons
  if (button_SRT) {return;}
  
  PrintTx+= "playXcuteDemo2\n";
  playInit();   // clear flags and moves[] memory
  playCnt = 0;  // zero the count for loading new data
  Display_Text2x16("START","DEMO"); DispNOP = false; DispDel = 50; DispCnt = 0;

  // now load the play values into memory
  // Record memory count = 180
  // Home0 = 1500
  // Home1 = 1318
  // Home2 = 1922
  // Home3 = 1109
  playMemLoad(0,-545,0,-591);     // At REST
  playMemLoad(311,628,-1174,-327);// Loading Bay
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,-545,0,-591);     // At REST
  playMemLoad(207,601,-675,-591); // Stand 1
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,-545,0,0);        // At REST
  playMemLoad(109,601,-675,-591); // Stand 2
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,-545,0,0);        // At REST
  playMemLoad(12,601,-675,-591);  // Stand 3
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,-545,0,0);        // At REST
  playMemLoad(-113,601,-675,-591);// Stand 4
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,-545,0,0);        // At REST
  playMemLoad(-215,601,-675,-591);// Stand 5
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,-545,0,0);        // At REST
  playMemLoad(-323,628,-1174,-327);// Finished goods
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,0,0,503);         // At READY
  playMemLoad(0,0,0,-607);        // Close jaws
  playMemLoad(-215,391,-210,-591);// Press machine
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,0,0,503);         // At READY
  playMemLoad(0,0,0,-148);        // Cloase jaws
  playMemLoad(-121,493,-222,0);   // Paint Booth
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,0,0,503);         // At READY
  playMemLoad(0,0,0,-142);        // Close jaws
  playMemLoad(12,301,-216,-591);  // Drill Machine
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,0,0,503);         // At READY
  playMemLoad(0,0,0,-361);        // Close jaws
  playMemLoad(106,451,-234,-591); // Laser cutter
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,0,0,503);         // At READY
  playMemLoad(0,0,0,-607);        // Close jaws
  playMemLoad(207,409,-96,-591);  // 
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(0,0,0,503);         // 
  playMemLoad(0,-545,0,0);        // 
  playMemLoad(5001,50,0,0);       // pause for 500 ms
  playMemLoad(5002,3,0,0);        // 3 claps

  // Recorded 45 element(s). 9% Max total = 500
  // End

  // now play the contents of the memory
  PWR_timeout = 200; playRun = true;
  button_SRT = true;
}

// ----------------------------------------------------------------------

void REC_ON() {
  // enter RECORD mode
  // move to RESET and begin recording mode
  PrintTx+= "\nEntering RECORD mode.\n";
  playInit(); // clear memory after recording mode
  REC = true; Display_Text2x16("RECORD","MODE"); DispNOP = false; DispDel = 30;
  loopWiiWait(); moveGoRESET();
}

// ----------------------------------------------------------------------

void REC_toggle() {
  // toggle RECORD mode
  // if buttons held for > 2 seconds clear memory contents
  if (REC) {
    // move to RESET and end recording mode
    PrintTx+= "\nExiting RECORD mode.\n";
    REC = false; Display_Text2x16("NORMAL","MODE"); DispNOP = false; DispDel = 30;
  } else {
    // move to RESET and begin recording mode
    PrintTx+= "\nEntering RECORD mode.\n";
    REC = true; Display_Text2x16("RECORD","MODE"); DispNOP = false; DispDel = 30;
  }
  TimeNow = millis(); // grab the current time
  loopWiiWait();
  if ((millis() - TimeNow) > 2000) {
    // buttons held for > 2 seconds
    playInit(); // clear memory after recording mode
    Display_Text2x16("MEMORY","CLEARED"); DispNOP = false; DispDel = 30;
  }
  moveGoRESET();
}

// ----------------------------------------------------------------------

void reportProgramStats() {
  // report the amount of program space used
  PrintTx+= "Loaded " + String(movePnt);
  PrintTx+= " commands. " + String((movePnt * 100)/moveArraySize);
  PrintTx+= "% Max total = " + String(moveArraySize) + "\n";
}

// ----------------------------------------------------------------------

void resetRECPause() {
  // called to reset the auto power time-out in REC mode, when servos are
  // moved and/or buttons are being pressed
  // this prevents the robot from retracting unexpectedly
  PWR_timeout = 2000; // timeout set to 20 seconds
}
