1  /********************************************************************
  2  * Description: iotaskintf.cc
  3  *   NML interface functions for IO
  4  *
  5  *   Based on a work by Fred Proctor & Will Shackleford
  6  *
  7  * Author:
  8  * License: GPL Version 2
  9  * System: Linux
 10  *    
 11  * Copyright (c) 2004 All rights reserved.
 12  *
 13  * Last change:
 14  ********************************************************************/
 15  
 16  #include <math.h>		// fabs()
 17  #include <float.h>		// DBL_MAX
 18  #include <string.h>		// memcpy() strncpy()
 19  #include <stdlib.h>		// malloc()
 20  
 21  #include "rcs.hh"		// RCS_CMD_CHANNEL, etc.
 22  #include "rcs_print.hh"
 23  #include "timer.hh"             // esleep, etc.
 24  #include "emc.hh"		// EMC NML
 25  #include "emc_nml.hh"
 26  #include "emcglb.h"		// EMC_INIFILE
 27  
 28  #include "initool.hh"
 29  
 30  // IO INTERFACE
 31  
 32  // the NML channels to the EMCIO controller
 33  static RCS_CMD_CHANNEL *emcIoCommandBuffer = 0;
 34  static RCS_STAT_CHANNEL *emcIoStatusBuffer = 0;
 35  
 36  // global status structure
 37  EMC_IO_STAT *emcIoStatus = 0;
 38  
 39  // serial number for communication
 40  static int emcIoCommandSerialNumber = 0;
 41  static double EMCIO_BUFFER_GET_TIMEOUT = 5.0;
 42  
 43  static int forceCommand(RCS_CMD_MSG *msg);
 44  
 45  static int emcioNmlGet()
 46  {
 47      int retval = 0;
 48      double start_time;
 49      RCS_PRINT_DESTINATION_TYPE orig_dest;
 50      if (emcIoCommandBuffer == 0) {
 51  	orig_dest = get_rcs_print_destination();
 52  	set_rcs_print_destination(RCS_PRINT_TO_NULL);
 53  	start_time = etime();
 54  	while (start_time - etime() < EMCIO_BUFFER_GET_TIMEOUT) {
 55  	    emcIoCommandBuffer =
 56  		new RCS_CMD_CHANNEL(emcFormat, "toolCmd", "emc",
 57  				    emc_nmlfile);
 58  	    if (!emcIoCommandBuffer->valid()) {
 59  		delete emcIoCommandBuffer;
 60  		emcIoCommandBuffer = 0;
 61  	    } else {
 62  		break;
 63  	    }
 64  	    esleep(0.1);
 65  	}
 66  	set_rcs_print_destination(orig_dest);
 67      }
 68  
 69      if (emcIoCommandBuffer == 0) {
 70  	emcIoCommandBuffer =
 71  	    new RCS_CMD_CHANNEL(emcFormat, "toolCmd", "emc", emc_nmlfile);
 72  	if (!emcIoCommandBuffer->valid()) {
 73  	    delete emcIoCommandBuffer;
 74  	    emcIoCommandBuffer = 0;
 75  	    retval = -1;
 76  	}
 77      }
 78  
 79      if (emcIoStatusBuffer == 0) {
 80  	orig_dest = get_rcs_print_destination();
 81  	set_rcs_print_destination(RCS_PRINT_TO_NULL);
 82  	start_time = etime();
 83  	while (start_time - etime() < EMCIO_BUFFER_GET_TIMEOUT) {
 84  	    emcIoStatusBuffer =
 85  		new RCS_STAT_CHANNEL(emcFormat, "toolSts", "emc",
 86  				     emc_nmlfile);
 87  	    if (!emcIoStatusBuffer->valid()) {
 88  		delete emcIoStatusBuffer;
 89  		emcIoStatusBuffer = 0;
 90  	    } else {
 91  		emcIoStatus =
 92  		    (EMC_IO_STAT *) emcIoStatusBuffer->get_address();
 93  		break;
 94  	    }
 95  	    esleep(0.1);
 96  	}
 97  	set_rcs_print_destination(orig_dest);
 98      }
 99  
100      if (emcIoStatusBuffer == 0) {
101  	emcIoStatusBuffer =
102  	    new RCS_STAT_CHANNEL(emcFormat, "toolSts", "emc", emc_nmlfile);
103  	if (!emcIoStatusBuffer->valid()
104  	    || EMC_IO_STAT_TYPE != emcIoStatusBuffer->peek()) {
105  	    delete emcIoStatusBuffer;
106  	    emcIoStatusBuffer = 0;
107  	    emcIoStatus = 0;
108  	    retval = -1;
109  	} else {
110  	    emcIoStatus = (EMC_IO_STAT *) emcIoStatusBuffer->get_address();
111  	}
112      }
113  
114      return retval;
115  }
116  
117  static RCS_CMD_MSG *last_io_command = 0;
118  static long largest_io_command_size = 0;
119  
120  /*
121    sendCommand() waits until any currently executing command has finished,
122    then writes the given command.*/
123  /*! \todo
124    FIXME: Not very RCS-like to wait for status done here. (wps)
125  */
126  static int sendCommand(RCS_CMD_MSG * msg)
127  {
128      // need command buffer to be there
129      if (0 == emcIoCommandBuffer) {
130  	return -1;
131      }
132      // need status buffer also, to check for command received
133      if (0 == emcIoStatusBuffer || !emcIoStatusBuffer->valid()) {
134  	return -1;
135      }
136  
137      // always force-queue an abort
138      if (msg->type == EMC_TOOL_ABORT_TYPE) {
139  	// just queue the abort and call it a day
140  	int rc = forceCommand(msg);
141  	if (rc) {
142  	    rcs_print_error("forceCommand(EMC_TOOL_ABORT) returned %d\n", rc);
143  	}
144  	return 0;
145      }
146  
147      double send_command_timeout = etime() + 5.0;
148  
149      // check if we're executing, and wait until we're done
150      int serial_diff = 0;
151      while (etime() < send_command_timeout) {
152  	emcIoStatusBuffer->peek();
153  	serial_diff = emcIoStatus->echo_serial_number - emcIoCommandSerialNumber;
154  	if (serial_diff < 0 || emcIoStatus->status == RCS_EXEC) {
155  	    esleep(0.001);
156  	    continue;
157  	} else {
158  	    break;
159  	}
160      }
161  
162      if (serial_diff < 0 || emcIoStatus->status == RCS_EXEC) {
163  	// Still not done, must have timed out.
164  	rcs_print_error
165  	    ("Command to IO level (%s:%s) timed out waiting for last command done. \n",
166  	     emcSymbolLookup(msg->type), emcIoCommandBuffer->msg2str(msg));
167  	rcs_print_error
168  	    ("emcIoStatus->echo_serial_number=%d, emcIoCommandSerialNumber=%d, emcIoStatus->status=%d\n",
169  	     emcIoStatus->echo_serial_number, emcIoCommandSerialNumber,
170  	     emcIoStatus->status);
171  	if (0 != last_io_command) {
172  	    rcs_print_error("Last command sent to IO level was (%s:%s)\n",
173  			    emcSymbolLookup(last_io_command->type),
174  			    emcIoCommandBuffer->msg2str(last_io_command));
175  	}
176  	return -1;
177      }
178      // now we can send
179      if (0 != emcIoCommandBuffer->write(msg)) {
180  	rcs_print_error("Failed to send command to  IO level (%s:%s)\n",
181  			emcSymbolLookup(msg->type),
182  			emcIoCommandBuffer->msg2str(msg));
183  	return -1;
184      }
185      emcIoCommandSerialNumber = msg->serial_number;
186  
187      if (largest_io_command_size < msg->size) {
188  	largest_io_command_size = std::max<long>(msg->size, 4096);
189  	last_io_command = (RCS_CMD_MSG *) realloc(last_io_command, largest_io_command_size);
190      }
191  
192      if (0 != last_io_command) {
193  	memcpy(last_io_command, msg, msg->size);
194      }
195  
196      return 0;
197  }
198  
199  /*
200    forceCommand() writes the given command regardless of the executing
201    status of any previous command.
202  */
203  static int forceCommand(RCS_CMD_MSG * msg)
204  {
205      // need command buffer to be there
206      if (0 == emcIoCommandBuffer) {
207  	return -1;
208      }
209      // need status buffer also, to check for command received
210      if (0 == emcIoStatusBuffer || !emcIoStatusBuffer->valid()) {
211  	return -1;
212      }
213      // send it immediately
214      if (0 != emcIoCommandBuffer->write(msg)) {
215  	rcs_print_error("Failed to send command to  IO level (%s:%s)\n",
216  			emcSymbolLookup(msg->type),
217  			emcIoCommandBuffer->msg2str(msg));
218  	return -1;
219      }
220      emcIoCommandSerialNumber = msg->serial_number;
221  
222      if (largest_io_command_size < msg->size) {
223  	largest_io_command_size = std::max<long>(msg->size, 4096);
224  	last_io_command = (RCS_CMD_MSG *) realloc(last_io_command, largest_io_command_size);
225      }
226  
227      if (0 != last_io_command) {
228  	memcpy(last_io_command, msg, msg->size);
229      }
230  
231      return 0;
232  }
233  
234  // NML commands
235  
236  int emcIoInit()
237  {
238      EMC_TOOL_INIT ioInitMsg;
239  
240      // get NML buffer to emcio
241      if (0 != emcioNmlGet()) {
242  	rcs_print_error("emcioNmlGet() failed.\n");
243  	return -1;
244      }
245  
246      if (0 != iniTool(emc_inifile)) {
247  	return -1;
248      }
249      // send init command to emcio
250      if (forceCommand(&ioInitMsg)) {
251  	rcs_print_error("Can't forceCommand(ioInitMsg)\n");
252  	return -1;
253      }
254  
255      return 0;
256  }
257  
258  int emcIoHalt()
259  {
260      EMC_TOOL_HALT ioHaltMsg;
261  
262      // send halt command to emcio
263      if (emcIoCommandBuffer != 0) {
264  	forceCommand(&ioHaltMsg);
265      }
266      // clear out the buffers
267  
268      if (emcIoStatusBuffer != 0) {
269  	delete emcIoStatusBuffer;
270  	emcIoStatusBuffer = 0;
271  	emcIoStatus = 0;
272      }
273  
274      if (emcIoCommandBuffer != 0) {
275  	delete emcIoCommandBuffer;
276  	emcIoCommandBuffer = 0;
277      }
278  
279      if (last_io_command) {
280          free(last_io_command);
281          last_io_command = 0;
282      }
283  
284      return 0;
285  }
286  
287  int emcIoAbort(int reason)
288  {
289      EMC_TOOL_ABORT ioAbortMsg;
290  
291      ioAbortMsg.reason = reason;
292      // send abort command to emcio
293      sendCommand(&ioAbortMsg);
294  
295      return 0;
296  }
297  
298  int emcIoSetDebug(int debug)
299  {
300      EMC_SET_DEBUG ioDebugMsg;
301  
302      ioDebugMsg.debug = debug;
303  
304      return sendCommand(&ioDebugMsg);
305  }
306  
307  int emcAuxEstopOn()
308  {
309      EMC_AUX_ESTOP_ON estopOnMsg;
310  
311      return forceCommand(&estopOnMsg);
312  }
313  
314  int emcAuxEstopOff()
315  {
316      EMC_AUX_ESTOP_OFF estopOffMsg;
317  
318      return forceCommand(&estopOffMsg); //force the EstopOff message
319  }
320  
321  int emcCoolantMistOn()
322  {
323      EMC_COOLANT_MIST_ON mistOnMsg;
324  
325      sendCommand(&mistOnMsg);
326  
327      return 0;
328  }
329  
330  int emcCoolantMistOff()
331  {
332      EMC_COOLANT_MIST_OFF mistOffMsg;
333  
334      sendCommand(&mistOffMsg);
335  
336      return 0;
337  }
338  
339  int emcCoolantFloodOn()
340  {
341      EMC_COOLANT_FLOOD_ON floodOnMsg;
342  
343      sendCommand(&floodOnMsg);
344  
345      return 0;
346  }
347  
348  int emcCoolantFloodOff()
349  {
350      EMC_COOLANT_FLOOD_OFF floodOffMsg;
351  
352      sendCommand(&floodOffMsg);
353  
354      return 0;
355  }
356  
357  int emcLubeOn()
358  {
359      EMC_LUBE_ON lubeOnMsg;
360  
361      sendCommand(&lubeOnMsg);
362  
363      return 0;
364  }
365  
366  int emcLubeOff()
367  {
368      EMC_LUBE_OFF lubeOffMsg;
369  
370      sendCommand(&lubeOffMsg);
371  
372      return 0;
373  }
374  
375  int emcToolPrepare(int p, int tool)
376  {
377      EMC_TOOL_PREPARE toolPrepareMsg;
378  
379      toolPrepareMsg.pocket = p;
380      toolPrepareMsg.tool = tool;
381      sendCommand(&toolPrepareMsg);
382  
383      return 0;
384  }
385  
386  
387  int emcToolStartChange()
388  {
389      EMC_TOOL_START_CHANGE toolStartChangeMsg;
390  
391      sendCommand(&toolStartChangeMsg);
392  
393      return 0;
394  }
395  
396  
397  int emcToolLoad()
398  {
399      EMC_TOOL_LOAD toolLoadMsg;
400  
401      sendCommand(&toolLoadMsg);
402  
403      return 0;
404  }
405  
406  int emcToolUnload()
407  {
408      EMC_TOOL_UNLOAD toolUnloadMsg;
409  
410      sendCommand(&toolUnloadMsg);
411  
412      return 0;
413  }
414  
415  int emcToolLoadToolTable(const char *file)
416  {
417      EMC_TOOL_LOAD_TOOL_TABLE toolLoadToolTableMsg;
418  
419      strcpy(toolLoadToolTableMsg.file, file);
420  
421      sendCommand(&toolLoadToolTableMsg);
422  
423      return 0;
424  }
425  
426  int emcToolSetOffset(int pocket, int toolno, EmcPose offset, double diameter,
427                       double frontangle, double backangle, int orientation)
428  {
429      EMC_TOOL_SET_OFFSET toolSetOffsetMsg;
430  
431      toolSetOffsetMsg.pocket = pocket;
432      toolSetOffsetMsg.toolno = toolno;
433      toolSetOffsetMsg.offset = offset;
434      toolSetOffsetMsg.diameter = diameter;
435      toolSetOffsetMsg.frontangle = frontangle;
436      toolSetOffsetMsg.backangle = backangle;
437      toolSetOffsetMsg.orientation = orientation;
438  
439      sendCommand(&toolSetOffsetMsg);
440  
441      return 0;
442  }
443  
444  int emcToolSetNumber(int number)
445  {
446      EMC_TOOL_SET_NUMBER toolSetNumberMsg;
447  
448      toolSetNumberMsg.tool = number;
449  
450      sendCommand(&toolSetNumberMsg);
451  
452      return 0;
453  }
454  
455  // Status functions
456  
457  int emcIoUpdate(EMC_IO_STAT * stat)
458  {
459  
460      if (0 == emcIoStatusBuffer || !emcIoStatusBuffer->valid()) {
461  	return -1;
462      }
463  
464      switch (emcIoStatusBuffer->peek()) {
465      case -1:
466  	// error on CMS channel
467  	return -1;
468  	break;
469  
470      case 0:			// nothing new
471      case EMC_IO_STAT_TYPE:	// something new
472  	// drop out to copy
473  	break;
474  
475      default:
476  	// something else is in there
477  	return -1;
478  	break;
479      }
480  
481      // copy status
482      *stat = *emcIoStatus;
483  
484      /*
485         We need to check that the RCS_DONE isn't left over from the previous
486         command, by comparing the command number we sent with the command
487         number that emcio echoes. If they're different, then the command
488         hasn't been acknowledged yet and the state should be forced to be
489         RCS_EXEC. */
490      int serial_diff = emcIoStatus->echo_serial_number - emcIoCommandSerialNumber;
491      if (serial_diff < 0) {
492  	stat->status = RCS_EXEC;
493      }
494      //commented out because it keeps resetting the spindle speed to some odd value
495      //the speed gets set by the IO controller, no need to override it here (io takes care of increase/decrease speed too)
496      // stat->spindle.speed = spindleSpeed;
497  
498      return 0;
499  }