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 ? -1 : 1;
  83         if (p->position < 0)
  84                 p->position = 0;
  85         if (p->endstop && p->position < p->endstop)
  86                 p->position = p->endstop;
  87         if (p->max_position > 0 && p->position > p->max_position)
  88                 p->position = p->max_position;
  89 }
  90
  91 static const char * irq_names[IRQ_STEPPER_COUNT] = {
  92         [IRQ_STEPPER_DIR_IN] = "1<stepper.direction",
  93         [IRQ_STEPPER_STEP_IN] = "1>stepper.step",
  94         [IRQ_STEPPER_ENABLE_IN] = "1<stepper.enable",
  95         [IRQ_STEPPER_POSITION_OUT] = "32<stepper.position",
  96         [IRQ_STEPPER_ENDSTOP_OUT] = "1<stepper.endstop",
  97 };
  98
  99 void
 100 stepper_init(
 101                 struct avr_t * avr,
 102                 stepper_p p,
 103                 char * name,
 104                 float steps_per_mm,
 105                 float start_position,
 106                 float max_position,
 107                 float endstop_position)
 108 {
 109         p->avr = avr;
 110         strcpy(p->name, name);
 111         p->irq = avr_alloc_irq(&avr->irq_pool, 0, IRQ_STEPPER_COUNT, irq_names);
 112         avr_irq_register_notify(p->irq + IRQ_STEPPER_DIR_IN, stepper_dir_hook, p);
 113         avr_irq_register_notify(p->irq + IRQ_STEPPER_STEP_IN, stepper_step_hook, p);
 114         avr_irq_register_notify(p->irq + IRQ_STEPPER_ENABLE_IN, stepper_enable_hook, p);
 115
 116         p->steps_per_mm = steps_per_mm;
 117         p->position = start_position * p->steps_per_mm;
 118         p->max_position = max_position * p->steps_per_mm;
 119         p->endstop = endstop_position >= 0 ? endstop_position * p->steps_per_mm : 0;
 120 }
 121
 122 void
 123 stepper_connect(
 124                 stepper_p p,
 125                 avr_irq_t *     step,
 126                 avr_irq_t *     dir,
 127                 avr_irq_t *     enable,
 128                 avr_irq_t *     endstop,
 129                 uint16_t flags)
 130 {
 131         avr_connect_irq(step, p->irq + IRQ_STEPPER_STEP_IN);
 132         avr_connect_irq(dir, p->irq + IRQ_STEPPER_DIR_IN);
 133         avr_connect_irq(enable, p->irq + IRQ_STEPPER_ENABLE_IN);
 134         p->irq[IRQ_STEPPER_ENDSTOP_OUT].flags |= IRQ_STEPPER_POSITION_OUT;
 135         p->irq[IRQ_STEPPER_ENDSTOP_OUT].flags |= IRQ_FLAG_FILTERED;
 136         if (endstop) {
 137                 avr_connect_irq(p->irq + IRQ_STEPPER_ENDSTOP_OUT, endstop);
 138                 if (flags & stepper_endstop_inverted)
 139                         p->irq[IRQ_STEPPER_ENDSTOP_OUT].flags |= IRQ_FLAG_NOT;
 140         }
 141         p->timer_period = avr_usec_to_cycles(p->avr, 100000 / 1000); // 1ms
 142         avr_cycle_timer_register(p->avr, p->timer_period, stepper_update_timer, p);
 143 }