         }
     }
 
+    /*
+     * The main loop calls this over and over just before calling updateMotorState.
+     * This function continues advancing through steps until it finds one that 
+     * reports that it is not done.
+     * Note: activate(MotorState) is called on each next step as it becomes the
+     * current step before looping and asking it whether it is already done.  
+     * This allows activate() to reset default pin conditions, timer start values, etc. 
+     * before isDone() checks to see if the step should be skipped immediately.
+     * Note: Since this keeps looking when a step says it is done, there can
+     * be several steps in a program that only execute in certain conditions.
+     */
     void updateProgramState(MotorState *motorState) {
-        boolean stepChanged = false;
-        // update the index for the current program step when the current step is done    
-        if (programSteps[currentProgramStepIndex]->isDurationComplete()) {
-            stepChanged = true;
+        /*
+         * Keep skipping through steps until one says that it is not done
+         * after it has been activated.
+         */ 
+        while (programSteps[currentProgramStepIndex]->isDone()) {
             currentProgramStepIndex++;
             if (currentProgramStepIndex >= programStepCount) {
                 currentProgramStepIndex = 0;  // loop and repeat program
             }
-        }
-        if (stepChanged || !programStarted) {
-            programStarted = true;
             programSteps[currentProgramStepIndex]->activate(motorState);
         }
     }

        }
    }

    // on full back and forth (4 seconds total)
    programs[0][0]->addProgramStep(new ProgramStep(FORWARD, 255, 2000));
    programs[0][0]->addProgramStep(new ProgramStep(REVERSE, 255, 2000));
    // on lower power back and forth (6 seconds total)
    programs[0][0]->addProgramStep(new ProgramStep(FORWARD, 127, 3000));
    programs[0][0]->addProgramStep(new ProgramStep(REVERSE, 127, 3000));
    // off for a few seconds
    programs[0][0]->addProgramStep(new ProgramStep(BRAKEGND, 0, 2000));
    // back and forth 3 times quick
    programs[0][0]->addProgramStep(new ProgramStep(REVERSE, 127, 250));
    programs[0][0]->addProgramStep(new ProgramStep(FORWARD, 127, 250));
    programs[0][0]->addProgramStep(new ProgramStep(REVERSE, 127, 250));
    programs[0][0]->addProgramStep(new ProgramStep(FORWARD, 127, 250));
    programs[0][0]->addProgramStep(new ProgramStep(REVERSE, 127, 250));
    programs[0][0]->addProgramStep(new ProgramStep(FORWARD, 127, 250));
    programs[0][0]->addProgramStep(new ProgramStep(REVERSE, 127, 250));
    
    programs[0][1]->addProgramStep(new ProgramStep(FORWARD, 200, 500));
    programs[0][1]->addProgramStep(new ProgramStep(BRAKEGND, 0, 3000));

    programs[1][0]->addProgramStep(new ProgramStep(BRAKEGND, 0, 1000));
    programs[1][0]->addProgramStep(new ProgramStep(REVERSE, 200, 200));
    programs[1][0]->addProgramStep(new ProgramStep(BRAKEGND, 0, 1000));
    programs[1][0]->addProgramStep(new ProgramStep(FORWARD, 200, 200));

    programs[1][1]->addProgramStep(new ProgramStep(BRAKEGND, 0, 10000));
    programs[1][1]->addProgramStep(new ProgramStep(FORWARD, 200, 1000));
    
    // Initialize stopped/braked
    stopMotor();

    
    pinMode(PIN_PROGRAM_SWITCH_1, INPUT_PULLUP);
    pinMode(PIN_PROGRAM_SWITCH_2, INPUT_PULLUP);
    pinMode(PIN_PARK_TRIGGER, INPUT_PULLUP);
    pinMode(PIN_PARK_FEEDBACK, INPUT_PULLUP);

    motor1State = new MotorState();
    for (int i=0; i<2; i++) {
