stepper works now

git-svn-id: https://svn.spreadspace.org/avr/trunk@122 aa12f405-d877-488e-9caf-2d797e2a1cc7

1 files changed, 20 insertions, 11 deletions

diff --git a/usb-i2c-sl018/stepper.c b/usb-i2c-sl018/stepper.c
index 673b494..f8f2991 100644
--- a/usb-i2c-sl018/stepper.c
+++ b/usb-i2c-sl018/stepper.c
@@ -56,7 +56,8 @@ uint8_t step_table [] =
 #define STEPPER_OUTPUT_BITMASK (~(0xF << STEPPER_FIRST_BIT ))
 
 volatile uint16_t step_cnt = 0;
-static uint16_t step_cnt_max = LENGTH_STEP_TABLE*2000;
+#define STEP_CNT_STOP (LENGTH_STEP_TABLE*400)
+#define STEP_CNT_OFF  (STEP_CNT_STOP + 125)
 stepper_direction_t step_direction = dir_open;
 
 inline void stop_stepper(void)
@@ -69,17 +70,23 @@ inline void stop_stepper(void)
 
 inline uint8_t handle_step(void)
 {
-  if(step_cnt < step_cnt_max)
-  {
-    step_cnt++;
-    uint8_t stepper_output=step_table[step_cnt % LENGTH_STEP_TABLE];
-    stepper_output<<=STEPPER_FIRST_BIT;
-    STEPPER_PORT = (STEPPER_PORT & STEPPER_OUTPUT_BITMASK ) | stepper_output;
-    return 1;
-  } else {
-    stop_stepper();
+  static uint8_t step_idx = 0;
+  
+  if(step_cnt < STEP_CNT_STOP) {
+    step_idx += (step_direction == dir_open) ? 1 : -1;
+    step_idx %= LENGTH_STEP_TABLE;
+  }
+
+  uint8_t stepper_output = step_table[step_idx];
+  stepper_output <<= STEPPER_FIRST_BIT;
+  STEPPER_PORT = (STEPPER_PORT & STEPPER_OUTPUT_BITMASK ) | stepper_output;
+
+  step_cnt++;
+  if(step_cnt >= STEP_CNT_OFF) {
     return 0;
   }
+
+  return 1;
 }
 
 void init_stepper(void)
@@ -106,5 +113,7 @@ void start_stepper(stepper_direction_t direction)
 
 ISR(TIMER1_COMPA_vect)
 {
-  handle_step();
+  if(!handle_step()) {
+    stop_stepper();
+  }
 }