diff --git a/Firmware/include/StepGen3.h b/Firmware/include/StepGen3.h
index 739fb50..12f555d 100755
--- a/Firmware/include/StepGen3.h
+++ b/Firmware/include/StepGen3.h
@@ -75,7 +75,7 @@ public:
     char *ctrl_type[MAX_CHAN] = {0};                                                                                   // control type ("p"pos or "v"vel) for up to 16 channels
     volatile int user_step_type[MAX_CYCLE] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // lookup table for user-defined step type
 
-    stepgen_t *stepgen_array;
+    stepgen_t *stepgen_array = 0;
 
     volatile int num_chan = 0;  // number of step generators configured */
     volatile long periodns;     // makepulses function period in nanosec */
@@ -88,6 +88,7 @@ public:
     volatile double recip_dt;   // recprocal of period, avoids divides */
 
     StepGen3(void);
+    void test(double pos_cmd);
     int rtapi_app_main();
     int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode);
     void make_pulses(void *arg, long period);
@@ -139,4 +140,10 @@ private:
     };
 };
 
+// For the example
+#define BASE_PERIOD 50000
+#define SERVO_PERIOD 1000000
+#define JOINT_0_STEPGEN_MAXACCEL 520.0
+#define JOINT_0_SCALE 200
+
 #endif
diff --git a/Firmware/src/StepGen3.cpp b/Firmware/src/StepGen3.cpp
index c0ec18b..df6491e 100755
--- a/Firmware/src/StepGen3.cpp
+++ b/Firmware/src/StepGen3.cpp
@@ -1080,10 +1080,6 @@ CONTROL StepGen3::parse_ctrl_type(const char *ctrl)
     return INVALID;
 }
 
-#define BASE_PERIOD 50000
-#define SERVO_PERIOD 1000000
-#define JOINT_0_STEPGEN_MAXACCEL 520.0
-#define JOINT_0_SCALE 200
 StepGen3::StepGen3(void)
 {
 
@@ -1099,24 +1095,15 @@ StepGen3::StepGen3(void)
     step->enable = 1;
     step->pos_scale = JOINT_0_SCALE;
     step->maxaccel = JOINT_0_STEPGEN_MAXACCEL;
+}
 
-    for (int servo_thread = 0; servo_thread < 100; servo_thread++)
-    {
-        step->pos_cmd = servo_thread * 1;
-        update_pos(step, SERVO_PERIOD);
-        update_freq(step, SERVO_PERIOD);
-        for (int base_thread = 0; base_thread < 20; base_thread++)
-        {
-            make_pulses(stepgen_array, BASE_PERIOD);
-            digitalWrite(PA6, step->phase[DIR_PIN] ? LOW : HIGH);
-            digitalWrite(PA7, step->phase[STEP_PIN] ? LOW : HIGH);
-            delay(50);
-            // printf("pos_cmd=%f\n", step->pos_cmd);
-            // printf("pos_fb=%f\n", step->pos_fb);
-            // printf("pickoff=%d accum=%lld addval=%d ", 1 << 28, step->accum, step->addval);
-            // printf("dir=%d step=%d\n", step->phase[DIR_PIN], step->phase[STEP_PIN]);
-        }
-    }
+void StepGen3::test(double pos_cmd)
+{
+    stepgen_t *step;
+    step = &(stepgen_array[0]);
+    step->pos_cmd = pos_cmd;
+    update_pos(step, SERVO_PERIOD);
+    update_freq(step, SERVO_PERIOD);
 }
 
 int StepGen3::rtapi_app_main()
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index e452dfa..33ee6db 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -103,6 +103,21 @@ static esc_cfg_t config =
 
 volatile byte serveIRQ = 0;
 #endif
+
+StepGen3 *Step = 0;
+
+void basePeriodCB(void)
+{
+   if (Step && Step->stepgen_array)
+   {
+      Step->make_pulses(Step->stepgen_array, BASE_PERIOD);
+      stepgen_t *step;
+      step = &(Step->stepgen_array[0]);
+      digitalWrite(PA6, step->phase[DIR_PIN] ? LOW : HIGH);
+      digitalWrite(PA7, step->phase[STEP_PIN] ? LOW : HIGH);
+   }
+}
+
 void setup(void)
 {
    Serial1.begin(115200);
@@ -114,12 +129,20 @@ void setup(void)
    pinMode(PA7, OUTPUT);
    digitalWrite(PA6, HIGH);
    digitalWrite(PA7, HIGH);
+   Step = new StepGen3;
+
+   HardwareTimer *MyTim = new HardwareTimer(TIM3);
+   MyTim->setOverflow(BASE_PERIOD, MICROSEC_FORMAT);
+   MyTim->attachInterrupt(basePeriodCB);
+   MyTim->resume();
 }
 
+double pos = 0;
 void loop(void)
 {
-   StepGen3 Step;
-
+   Step->test(pos);
+   pos += 1;
+   delay(1000);
 #if SGT
    uint64_t dTime;
    if (serveIRQ)