git.rot13.org Git - simavr/blob - examples/board_reprap/src/stepper.c

   1 /*
   2         stepper.c
   3
   4         Copyright 2008-2012 Michel Pollet <buserror@gmail.com>
   5
   6         This file is part of simavr.
   7
   8         simavr is free software: you can redistribute it and/or modify
   9         it under the terms of the GNU General Public License as published by
  10         the Free Software Foundation, either version 3 of the License, or
  11         (at your option) any later version.
  12
  13         simavr is distributed in the hope that it will be useful,
  14         but WITHOUT ANY WARRANTY; without even the implied warranty of
  15         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16         GNU General Public License for more details.
  17
  18         You should have received a copy of the GNU General Public License
  19         along with simavr.  If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include <stdlib.h>
  23 #include <string.h>
  24 #include <stdio.h>
  25
  26 #include "sim_avr.h"
  27 #include "sim_time.h"
  28 #include "stepper.h"
  29
  30 static avr_cycle_count_t
  31 stepper_update_timer(
  32                 struct avr_t * avr,
  33         avr_cycle_count_t when,
  34         void * param)
  35 {
  36         stepper_p p = (stepper_p)param;
  37         union {
  38                 float f;
  39                 uint32_t i;
  40         } m = { .f = p->position / p->steps_per_mm };
  41 //      printf("%s (%s) %3.4f\n", __func__, p->name, m.f);
  42         avr_raise_irq(p->irq + IRQ_STEPPER_POSITION_OUT, m.i);
  43         avr_raise_irq(p->irq + IRQ_STEPPER_ENDSTOP_OUT, p->position == p->endstop);
  44         return when + p->timer_period;
  45 }
  46
  47 static void
  48 stepper_dir_hook(
  49                 struct avr_irq_t * irq,
  50                 uint32_t value,
  51                 void * param )
  52 {
  53         stepper_p p = (stepper_p)param;
  54         printf("%s (%s) %d\n", __func__, p->name, value);
  55         p->dir = !!value;
  56 }
  57
  58 static void
  59 stepper_enable_hook(
  60                 struct avr_irq_t * irq,
  61                 uint32_t value,
  62                 void * param )
  63 {
  64         stepper_p p = (stepper_p)param;
  65         p->enable = !!value;
  66         printf("%s (%s) %d pos %.4f\n", __func__, p->name,
  67                         p->enable != 0, p->position / p->steps_per_mm);
  68         avr_raise_irq(p->irq + IRQ_STEPPER_ENDSTOP_OUT, p->position == p->endstop);
  69 }
  70
  71 static void
  72 stepper_step_hook(
  73                 struct avr_irq_t * irq,
  74                 uint32_t value,
  75                 void * param )
  76 {
  77         stepper_p p = (stepper_p)param;
  78         if (!p->enable)
  79                 return;
  80         if (value)
  81                 return;
  82         p->position += !p->dir && p->position == 0 ? 0 : p->dir ? 1 : -1;
  83         if (p->endstop && p->position < p->endstop)
  84                 p->position = p->endstop;
  85         if (p->max_position > 0 && p->position > p->max_position)
  86                 p->position = p->max_position;
  87 }
  88
  89 static const char * irq_names[IRQ_STEPPER_COUNT] = {
  90         [IRQ_STEPPER_DIR_IN] = "1<stepper.direction",
  91         [IRQ_STEPPER_STEP_IN] = "1>stepper.step",
  92         [IRQ_STEPPER_ENABLE_IN] = "1<stepper.enable",
  93         [IRQ_STEPPER_POSITION_OUT] = "32<stepper.position",
  94         [IRQ_STEPPER_ENDSTOP_OUT] = "1<stepper.endstop",
  95 };
  96
  97 void
  98 stepper_init(
  99                 struct avr_t * avr,
 100                 stepper_p p,
 101                 char * name,
 102                 float steps_per_mm,
 103                 float start_position,
 104                 float max_position,
 105                 float endstop_position)
 106 {
 107         p->avr = avr;
 108         strcpy(p->name, name);
 109         p->irq = avr_alloc_irq(&avr->irq_pool, 0, IRQ_STEPPER_COUNT, irq_names);
 110         avr_irq_register_notify(p->irq + IRQ_STEPPER_DIR_IN, stepper_dir_hook, p);
 111         avr_irq_register_notify(p->irq + IRQ_STEPPER_STEP_IN, stepper_step_hook, p);
 112         avr_irq_register_notify(p->irq + IRQ_STEPPER_ENABLE_IN, stepper_enable_hook, p);
 113
 114         p->steps_per_mm = steps_per_mm;
 115         p->position = start_position * p->steps_per_mm;
 116         p->max_position = max_position * p->steps_per_mm;
 117         p->endstop = endstop_position >= 0 ? endstop_position * p->steps_per_mm : 0;
 118 }
 119
 120 void
 121 stepper_connect(
 122                 stepper_p p,
 123                 avr_irq_t *     step,
 124                 avr_irq_t *     dir,
 125                 avr_irq_t *     enable,
 126                 avr_irq_t *     endstop,
 127                 uint16_t flags)
 128 {
 129         avr_connect_irq(step, p->irq + IRQ_STEPPER_STEP_IN);
 130         avr_connect_irq(dir, p->irq + IRQ_STEPPER_DIR_IN);
 131         avr_connect_irq(enable, p->irq + IRQ_STEPPER_ENABLE_IN);
 132         p->irq[IRQ_STEPPER_ENDSTOP_OUT].flags |= IRQ_STEPPER_POSITION_OUT;
 133         p->irq[IRQ_STEPPER_ENDSTOP_OUT].flags |= IRQ_FLAG_FILTERED;
 134         if (endstop) {
 135                 avr_connect_irq(p->irq + IRQ_STEPPER_ENDSTOP_OUT, endstop);
 136                 if (flags & stepper_endstop_inverted)
 137                         p->irq[IRQ_STEPPER_ENDSTOP_OUT].flags |= IRQ_FLAG_NOT;
 138         }
 139         p->timer_period = avr_usec_to_cycles(p->avr, 100000 / 1000); // 1ms
 140         avr_cycle_timer_register(p->avr, p->timer_period, stepper_update_timer, p);
 141 }
 142
 143 float
 144 stepper_get_position_mm(
 145                 stepper_p p)
 146 {
 147         return p->position / p->steps_per_mm;
 148 }
 149