From b3fcfd2f32c0b51cf795d0e367401c0ddec671d9 Mon Sep 17 00:00:00 2001
From: Michel Pollet <buserror@gmail.com>
Date: Wed, 18 Apr 2012 10:56:57 +0100
Subject: [PATCH] stepper: Keep position in step numbers, not mm

More precise, also use a cycle timer to send status positions in mm

Signed-off-by: Michel Pollet <buserror@gmail.com>
---
 examples/board_reprap/src/stepper.c | 41 ++++++++++++++++++++---------
 examples/board_reprap/src/stepper.h | 13 ++++-----
 2 files changed, 36 insertions(+), 18 deletions(-)

diff --git a/examples/board_reprap/src/stepper.c b/examples/board_reprap/src/stepper.c
index aedd675..832b1f2 100644
--- a/examples/board_reprap/src/stepper.c
+++ b/examples/board_reprap/src/stepper.c
@@ -24,8 +24,26 @@
 #include <stdio.h>
 
 #include "sim_avr.h"
+#include "sim_time.h"
 #include "stepper.h"
 
+static avr_cycle_count_t
+stepper_update_timer(
+		struct avr_t * avr,
+        avr_cycle_count_t when,
+        void * param)
+{
+	stepper_p p = (stepper_p)param;
+	union {
+		float f;
+		uint32_t i;
+	} m = { .f = p->position / p->steps_per_mm };
+	printf("%s (%s) %3.4f\n", __func__, p->name, m.f);
+	avr_raise_irq(p->irq + IRQ_STEPPER_POSITION_OUT, m.i);
+	avr_raise_irq(p->irq + IRQ_STEPPER_ENDSTOP_OUT, p->position == p->endstop);
+	return when + p->timer_period;
+}
+
 static void
 stepper_dir_hook(
 		struct avr_irq_t * irq,
@@ -45,7 +63,8 @@ stepper_enable_hook(
 {
 	stepper_p p = (stepper_p)param;
 	p->enable = !!value;
-	printf("%s (%s) %d pos %.4f\n", __func__, p->name, p->enable != 0, p->position);
+	printf("%s (%s) %d pos %.4f\n", __func__, p->name,
+			p->enable != 0, p->position / p->steps_per_mm);
 	avr_raise_irq(p->irq + IRQ_STEPPER_ENDSTOP_OUT, p->position == p->endstop);
 }
 
@@ -60,18 +79,13 @@ stepper_step_hook(
 		return;
 	if (value)
 		return;
-	p->position += (p->dir ? -1.0 : 1.0) / p->steps_per_mm;
+	p->position += p->dir ? -1 : 1;
 	if (p->position < 0)
 		p->position = 0;
+	if (p->endstop && p->position < p->endstop)
+		p->position = p->endstop;
 	if (p->max_position > 0 && p->position > p->max_position)
 		p->position = p->max_position;
-	printf("%s (%s) %.4f\n", __func__, p->name, p->position);
-	union {
-		float f;
-		uint32_t i;
-	} m = { .f = p->position };
-	avr_raise_irq(p->irq + IRQ_STEPPER_POSITION_OUT, m.i);
-	avr_raise_irq(p->irq + IRQ_STEPPER_ENDSTOP_OUT, p->position == p->endstop);
 }
 
 static const char * irq_names[IRQ_STEPPER_COUNT] = {
@@ -100,9 +114,9 @@ stepper_init(
 	avr_irq_register_notify(p->irq + IRQ_STEPPER_ENABLE_IN, stepper_enable_hook, p);
 
 	p->steps_per_mm = steps_per_mm;
-	p->position = start_position;
-	p->max_position = max_position;
-	p->endstop = endstop_position;
+	p->position = start_position * p->steps_per_mm;
+	p->max_position = max_position * p->steps_per_mm;
+	p->endstop = endstop_position >= 0 ? endstop_position * p->steps_per_mm : 0;
 }
 
 void
@@ -117,10 +131,13 @@ stepper_connect(
 	avr_connect_irq(step, p->irq + IRQ_STEPPER_STEP_IN);
 	avr_connect_irq(dir, p->irq + IRQ_STEPPER_DIR_IN);
 	avr_connect_irq(enable, p->irq + IRQ_STEPPER_ENABLE_IN);
+	p->irq[IRQ_STEPPER_ENDSTOP_OUT].flags |= IRQ_STEPPER_POSITION_OUT;
 	p->irq[IRQ_STEPPER_ENDSTOP_OUT].flags |= IRQ_FLAG_FILTERED;
 	if (endstop) {
 		avr_connect_irq(p->irq + IRQ_STEPPER_ENDSTOP_OUT, endstop);
 		if (flags & stepper_endstop_inverted)
 			p->irq[IRQ_STEPPER_ENDSTOP_OUT].flags |= IRQ_FLAG_NOT;
 	}
+	p->timer_period = avr_usec_to_cycles(p->avr, 100000 / 1000); // 1ms
+	avr_cycle_timer_register(p->avr, p->timer_period, stepper_update_timer, p);
 }
diff --git a/examples/board_reprap/src/stepper.h b/examples/board_reprap/src/stepper.h
index fce47a1..d8f8867 100644
--- a/examples/board_reprap/src/stepper.h
+++ b/examples/board_reprap/src/stepper.h
@@ -40,9 +40,10 @@ typedef struct stepper_t {
 	char name[32];
 	int enable : 1, dir : 1, trace : 1;
 	double steps_per_mm;
-	double position;
-	double max_position;
-	double endstop;
+	uint64_t	position;	// in steps
+	uint64_t max_position;
+	uint64_t endstop;
+	avr_cycle_count_t timer_period;
 } stepper_t, *stepper_p;
 
 void
@@ -51,9 +52,9 @@ stepper_init(
 		stepper_p p,
 		char * name,
 		float steps_per_mm,
-		float start_position,
-		float max_position,
-		float endstop_position);
+		float start_position,		// mm
+		float max_position,			// mm
+		float endstop_position);	// mm
 
 enum {
 	stepper_endstop_inverted = (1 << 0),
-- 
2.39.5