1  /********************************************************************
  2  * Description: modmath.c
  3  *   Assorted modulo arithmetic functions for HAL.
  4  *
  5  *   This HAL component has just one function at the moment: closest_dir
  6  *   this component takes a "actual" input and a "desired" input, and
  7  *   has one parameter: the "base" (or, number of elements).  It outputs
  8  *   an up or down bit, depending on which direction has the shortest path
  9  *   from the actual to the desired input.
 10  *
 11  *   Each individual type of block is invoked by a parameter on
 12  *   the insmdo command line.  Each parameter is of the form:
 13  *   <blockname>=<number_of_blocks>
 14  *
 15  *   For example, mod_dir=2 installs two "closest direction" components.
 16  *
 17  *   List of functions currently implemented:
 18  *      mod_dir: tells the direction to the closest neighbor, for sequences that roll over
 19  *
 20  *   Eventually, there should be more functions
 21  *
 22  *********************************************************************
 23  *
 24  * Author: Stephen Wille Padnos  (swpadnos AT sourceforge DOT net)
 25  *       Based on a work by John Kasunich (jmkasunich AT att DOT net)
 26  * License: GPL Version 2
 27  * Created on: 2006/5/18
 28  * System: Linux
 29  *
 30  * Copyright (c) 2006 All rights reserved.
 31  *
 32  ********************************************************************/
 33  
 34  #include "rtapi.h"		/* RTAPI realtime OS API */
 35  #include "rtapi_app.h"		/* RTAPI realtime module decls */
 36  #include "hal.h"		/* HAL public API decls */
 37  
 38  #include "rtapi_math.h"
 39  
 40  /* module information */
 41  MODULE_AUTHOR("Stephen Wille Padnos");
 42  MODULE_DESCRIPTION("Modulo math blocks for EMC HAL");
 43  MODULE_LICENSE("GPL");
 44  static int mod_dir = 0;	/* number of mod_dirs */
 45  RTAPI_MP_INT(mod_dir, "Modulo direction blocks");
 46  
 47  /***********************************************************************
 48  *                STRUCTURES AND GLOBAL VARIABLES                       *
 49  ************************************************************************/
 50  
 51  /** These structures contain the runtime data for a single block. */
 52  
 53  typedef struct {
 54      hal_bit_t *up;		/* output pin: go up to get to the desired position */
 55      hal_bit_t *down;		/* output pin: go down to get to the desired position */
 56      hal_bit_t *on_target;	/* output pin: go at desired position */
 57      hal_s32_t *actual;		/* input pin: actual position */
 58      hal_s32_t *desired;		/* input pin: desired position */
 59      hal_s32_t *max_num;		/* input/output pin: highest value to allow */
 60      hal_s32_t *min_num;		/* input/output pin: lowest value to allow */
 61      hal_bit_t *wrap;		/* input/output pin: set true if the array is circular, false if linear */
 62  } mod_dir_t;
 63  
 64  /* other globals */
 65  static int comp_id;		/* component ID */
 66  
 67  /***********************************************************************
 68  *                  LOCAL FUNCTION DECLARATIONS                         *
 69  ************************************************************************/
 70  
 71  static int export_mod_dir(int num);
 72  
 73  static void mod_dir_funct(void *arg, long period);
 74  
 75  
 76  /***********************************************************************
 77  *                       INIT AND EXIT CODE                             *
 78  ************************************************************************/
 79  
 80  int rtapi_app_main(void)
 81  {
 82      int n;
 83  
 84      /* connect to the HAL */
 85      comp_id = hal_init("modmath");
 86      if (comp_id < 0) {
 87  	rtapi_print_msg(RTAPI_MSG_ERR, "MODMATH: ERROR: hal_init() failed\n");
 88  	return -1;
 89      }
 90      /* allocate and export modulo direction finders */
 91      if (mod_dir > 0) {
 92  	for (n = 0; n < mod_dir; n++) {
 93  	    if (export_mod_dir(n) != 0) {
 94  		rtapi_print_msg(RTAPI_MSG_ERR,
 95  		    "MODMATH: ERROR: export_mod_dir(%d) failed\n", n);
 96  		hal_exit(comp_id);
 97  		return -1;
 98  	    }
 99  	}
100  	rtapi_print_msg(RTAPI_MSG_INFO,
101  	    "MODMATH: installed %d mod-dirs\n", mod_dir);
102      }
103      hal_ready(comp_id);
104      return 0;
105  }
106  
107  void rtapi_app_exit(void)
108  {
109      hal_exit(comp_id);
110  }
111  
112  /***********************************************************************
113  *                     REALTIME BLOCK FUNCTIONS                         *
114  ************************************************************************/
115  
116  static void mod_dir_funct(void *arg, long period)
117  {
118      mod_dir_t *mod;
119      int range, act, des, to_go;
120  
121      /* point to block data */
122      mod = (mod_dir_t *) arg;
123      range = *(mod->max_num) - *(mod->min_num) + 1;
124      act = *(mod->actual);
125      if (act > *(mod->max_num) || act < *(mod->min_num)) {
126  	act = *(mod->min_num) + ((act-*(mod->min_num)) % (range));
127      }
128      des = *(mod->desired);
129      if (des > *(mod->max_num) || des < *(mod->min_num)) {
130  	des = *(mod->min_num) + ((des-*(mod->min_num)) % (range));
131      }
132  
133      to_go = des-act;
134  
135      if ((*(mod->wrap)) && (to_go > range/2)) {
136  	to_go -= range;
137      }
138      if ((*(mod->wrap)) && (to_go < -range/2)) {
139  	to_go += range;
140      }
141  
142      /* if (desired-actual) >= (actual+(max-min+1)-desired), output "up" */
143      if (to_go == 0) {
144  	*(mod->up) = 0;
145  	*(mod->down) = 0;
146  	*(mod->on_target) = 1;
147      } else if (to_go > 0 ) {
148  	*(mod->down) = 0;
149  	*(mod->on_target) = 0;
150  	*(mod->up) = 1;
151      } else {
152  	*(mod->up) = 0;
153  	*(mod->on_target) = 0;
154  	*(mod->down) = 1;
155      }
156  }
157  
158  /***********************************************************************
159  *                   LOCAL FUNCTION DEFINITIONS                         *
160  ************************************************************************/
161  
162  static int export_mod_dir(int num)
163  {
164      int retval;
165      char buf[HAL_NAME_LEN + 1];
166      mod_dir_t *moddir;
167  
168      /* allocate shared memory for modulo "closest direction finder" */
169      moddir = hal_malloc(sizeof(mod_dir_t));
170      if (moddir == 0) {
171  	rtapi_print_msg(RTAPI_MSG_ERR,
172  	    "MODMATH: ERROR: hal_malloc() failed\n");
173  	return -1;
174      }
175      /* export output pins */
176      rtapi_snprintf(buf, sizeof(buf), "mod-dir.%d.up", num);
177      retval = hal_pin_bit_new(buf, HAL_OUT, &(moddir->up), comp_id);
178      if (retval != 0) {
179  	rtapi_print_msg(RTAPI_MSG_ERR,
180  	    "MODMATH: ERROR: '%s' pin export failed\n", buf);
181  	return retval;
182      }
183      rtapi_snprintf(buf, sizeof(buf), "mod-dir.%d.down", num);
184      retval = hal_pin_bit_new(buf, HAL_OUT, &(moddir->down), comp_id);
185      if (retval != 0) {
186  	rtapi_print_msg(RTAPI_MSG_ERR,
187  	    "MODMATH: ERROR: '%s' pin export failed\n", buf);
188  	return retval;
189      }
190      rtapi_snprintf(buf, sizeof(buf), "mod-dir.%d.on-target", num);
191      retval = hal_pin_bit_new(buf, HAL_OUT, &(moddir->on_target), comp_id);
192      if (retval != 0) {
193  	rtapi_print_msg(RTAPI_MSG_ERR,
194  	    "MODMATH: ERROR: '%s' pin export failed\n", buf);
195  	return retval;
196      }
197  
198      /* export input pins */
199      rtapi_snprintf(buf, sizeof(buf), "mod-dir.%d.actual", num);
200      retval = hal_pin_s32_new(buf, HAL_IN, &(moddir->actual), comp_id);
201      if (retval != 0) {
202  	rtapi_print_msg(RTAPI_MSG_ERR,
203  	    "MODMATH: ERROR: '%s' pin export failed\n", buf);
204  	return retval;
205      }
206      rtapi_snprintf(buf, sizeof(buf), "mod-dir.%d.desired", num);
207      retval = hal_pin_s32_new(buf, HAL_IN, &(moddir->desired), comp_id);
208      if (retval != 0) {
209  	rtapi_print_msg(RTAPI_MSG_ERR,
210  	    "MODMATH: ERROR: '%s' pin export failed\n", buf);
211  	return retval;
212      }
213  
214      /* export pins for max and min values */
215      rtapi_snprintf(buf, sizeof(buf), "mod-dir.%d.min-num", num);
216      retval = hal_pin_s32_new(buf, HAL_IO, &(moddir->min_num), comp_id);
217      if (retval != 0) {
218  	rtapi_print_msg(RTAPI_MSG_ERR,
219  	    "MODMATH: ERROR: '%s' pin export failed\n", buf);
220  	return retval;
221      }
222      rtapi_snprintf(buf, sizeof(buf), "mod-dir.%d.max-num", num);
223      retval = hal_pin_s32_new(buf, HAL_IO, &(moddir->max_num), comp_id);
224      if (retval != 0) {
225  	rtapi_print_msg(RTAPI_MSG_ERR,
226  	    "MODMATH: ERROR: '%s' pin export failed\n", buf);
227  	return retval;
228      }
229      rtapi_snprintf(buf, sizeof(buf), "mod-dir.%d.wrap", num);
230      retval = hal_pin_bit_new(buf, HAL_IO, &(moddir->wrap), comp_id);
231      if (retval != 0) {
232  	rtapi_print_msg(RTAPI_MSG_ERR,
233  	    "MODMATH: ERROR: '%s' param export failed\n", buf);
234  	return retval;
235      }
236      /* export function */
237      rtapi_snprintf(buf, sizeof(buf), "mod-dir.%d", num);
238      retval = hal_export_funct(buf, mod_dir_funct, moddir, 1, 0, comp_id);
239      if (retval != 0) {
240  	rtapi_print_msg(RTAPI_MSG_ERR,
241  	    "MODMATH: ERROR: '%s' funct export failed\n", buf);
242  	return -1;
243      }
244      /* set default parameter values */
245      *(moddir->up) = 0;
246      *(moddir->down) = 0;
247      *(moddir->on_target) = 1;
248      *(moddir->min_num) = 0;
249      *(moddir->max_num) = 15;
250      *(moddir->actual) = 0;
251      *(moddir->desired) = 0;
252      *(moddir->wrap) = 1;		/* wrap by default */
253      return 0;
254  }