 #define CS_THRESHOLD 600
 #define UPDATE_INTERVAL 10
 
+//#define PWM_SPEED_5 13
+//#define PWM_SPEED_10 26
+//#define PWM_SPEED_15 38
+//#define PWM_SPEED_20 51
+//#define PWM_SPEED_25 64
+//#define PWM_SPEED_30 77
+//#define PWM_SPEED_35 89
+//#define PWM_SPEED_40 102
+//#define PWM_SPEED_45 115
+//#define PWM_SPEED_50 127
+//#define PWM_SPEED_55 140
+//#define PWM_SPEED_60 153
+//#define PWM_SPEED_65 166
+//#define PWM_SPEED_70 179
+//#define PWM_SPEED_75 191
+//#define PWM_SPEED_80 204
+//#define PWM_SPEED_85 217
+//#define PWM_SPEED_90 230
+//#define PWM_SPEED_95 242
+//#define PWM_SPEED_100 255
+
 /*  TE466 VNH2SP30 pin definitions */
 // INA: digital/logic-level output HIGH for FORWARD, LOW for REVERSE
 // direct connection to TE466 - INA

        }
    }

    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;
            currentProgramStepIndex++;
            if (currentProgramStepIndex >= programStepCount) {
                currentProgramStepIndex = 0;  // loop and repeat program
            }
        }
        if (stepChanged || !programStarted) {
            programStarted = true;
            programSteps[currentProgramStepIndex]->activate(motorState);
        }
    }

    
    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++) {
