{\scriptsize \begin{lstlisting}
     1	/**
     2	   @file
     3	
     4	   Implementation of an example interface to a simple robot with two
     5	   independent axies. The robot has four inputs whish are connected to
     6	   the lower nible on IO port "interface_ioport". The bits are
     7	   connected in this way (the signals a high-active):
     8	
     9	   Bit 0: Drive Up
    10	     wires: switch to GND: yellow-green;    +24V: gray-black
    11	
    12	   Bit 1: Dirve down
    13	     wires: switch to GND: red-green;       +24V: orange-black
    14	
    15	   Bit 2: Dirve right
    16	     wires: switch to GND: green-red;       +24V: yellow-black
    17	
    18	   Bit 3: Dirve left
    19	     wires: switch to GND: white-red;       +24V: red-blue
    20	
    21	     power wires:     GND: blue;            +24V: red
    22	
    23	   This interface assumes a linear dependency betwenn the coverence of
    24	   distance and moving time. The 0,0 coordinates is left,bottom. 
    25	
    26	   User functions are:
    27	
    28	   interface_init() - Initialze the interface and drive the robot to
    29	   the start position (X=undefined; Y=0).
    30	
    31	   interface_driveto(int x, int y) - Drive the robot to the absolute
    32	   coordinates x,y.
    33	*/
    34	
    35	/*
    36	  Copyright (c) Andreas Hofmeier
    37	  (www.an-h.de, www.an-h.de.vu, www.lgut.uni-bremen.de/an-h/)
    38	  
    39	  This program is free software; you can redistribute it and/or modify
    40	  it under the terms of the GNU General Public License as published by
    41	  the Free Software Foundation; either version 2 of the License, or
    42	  (at your option) any later version.
    43	
    44	  This program is distributed in the hope that it will be useful, but
    45	  WITHOUT ANY WARRANTY; without even the implied warranty of
    46	  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
    47	  General Public License for more details.
    48	
    49	  You should have received a copy of the GNU General Public License
    50	  along with this program; if not, write to the Free Software
    51	  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
    52	*/
    53	
    54	
    55	#include <stdio.h>
    56	#include <unistd.h>
    57	#include <stdlib.h>
    58	#include <asm/io.h>
    59	#include <time.h>
    60	#include <sys/wait.h>
    61	
    62	
    63	/** Time it rakes to drive to robot form y_min to y_max */
    64	#define interface_ymax_time 20000 // at 6 bar
    65	/** Time it rakes to drive to robot form x_min to x_max */
    66	#define interface_xmax_time 20000 // 34670
    67	/** Maximal Y value (cosidered as 100 percent) */
    68	#define interface_ymax 100
    69	/** Maximal X value (cosidered as 100 percent) */
    70	#define interface_xmax 100
    71	/** IO Port to which lowest nible to robot is connected */
    72	#define interface_ioport 0x378
    73	
    74	/** current X possition of robot (global) */
    75	int interface_x;
    76	/** current X possition of robot (global) */
    77	int interface_y;
    78	
    79	/** mode of interface:
    80	    0: normal,
    81	    1: simulation (do all except to drive the robot),
    82	    2: simulation with position-output
    83	    3: Blocked: Do nothing */
    84	int interface_mode;
    85	
    86	
    87	/**
    88	   Read a byte from an IO port
    89	   @param addr (int): address of port to read
    90	   @return (unsigned char) read byte
    91	*/
    92	unsigned char input(int addr) {
    93	  if (interface_mode == 0) {
    94	    return inb(addr);
    95	  }
    96	  return 255;
    97	}
    98	
    99	
   100	/**
   101	   Write a byte to an IO port
   102	   @param addr (int): address of port to write onto
   103	   @param out (unsigned char): byte to write
   104	   @return (unsigned char) written byte
   105	*/
   106	unsigned char output(int addr, unsigned char out) {
   107	  if (interface_mode == 0) {
   108	    outb(out, addr);
   109	    return out;
   110	  }
   111	  return out;
   112	}
   113	
   114	
   115	/**
   116	   Wait a specified number of milliseconds
   117	   @param msec (int): milliseconds to wait
   118	*/
   119	void msleep(int msec) {
   120	  int sec = msec / 1000;
   121	  msec = msec - sec * 1000;
   122	  sleep(sec);
   123	  usleep(msec * 1000);
   124	}
   125	
   126	
   127	/**
   128	   Get to highest number out of two input numbers
   129	   @param a (int) first input number
   130	   @param b (int) second input number
   131	   @return (int) the highest of the input numbers
   132	 */
   133	int getmax(int a, int b) {
   134	  if (a > b) {
   135	    return a;
   136	  } else {
   137	    return b;
   138	  }
   139	}
   140	
   141	
   142	/**
   143	   Get to lowest number out of two input numbers
   144	   @param a (int) first input number
   145	   @param b (int) second input number
   146	   @return (int) the lowest of the input numbers
   147	 */
   148	int getmin(int a, int b) {
   149	  if (a < b) {
   150	    return a;
   151	  } else {
   152	    return b;
   153	  }
   154	}
   155	
   156	
   157	/**
   158	   Drive the robot in a specified direction. Any axies can be zero,
   159	   greater than zero or less than zero, in this cases the robot will
   160	   not driven, driven to increase to position (up[v,y] or right[h,x])
   161	   and decrease the position (down[-v,-y] or left[-h,-x]).
   162	
   163	   @param h (int) horizontal or X axias.
   164	   @param v (int) vertical or Y axias.
   165	*/
   166	void interface_drive(int h, int v) {
   167	  unsigned char buf;
   168	
   169	  buf = 0;
   170	  /* Y Axies */
   171	  if (v > 0) {
   172	    buf = buf | 1;
   173	  }
   174	  if (v < 0) {
   175	    buf = buf | 2;
   176	  }
   177	  /* X Axies */
   178	  if (h > 0) {
   179	    buf = buf | 4;
   180	  }
   181	  if (h < 0) {
   182	    buf = buf | 8;
   183	  }
   184	
   185	  /* Apply */
   186	  buf = buf | (input(interface_ioport) & 240);
   187	  output(interface_ioport, buf);
   188	}
   189	
   190	
   191	/**
   192	   Initialze the interface and drive the robot to the start position.
   193	 */
   194	void interface_init(int mode) {
   195	  if ((mode < 0) || (mode > 3)) {
   196	    mode = 2;
   197	  }
   198	  interface_mode = mode;
   199	
   200	  // enable access to IO ports (need root previleges)
   201	  if (interface_mode == 0) {
   202	    if (iopl(3) < 0) {
   203	      perror("iopl()");
   204	      //      exit(1);
   205	      interface_mode = 2;
   206	    }
   207	  }
   208	
   209	  switch (interface_mode) {
   210	  case 0:
   211	    fprintf(stderr, "Interface full active.\n");
   212	    break;
   213	  case 1:
   214	    fprintf(stderr, "Interface disabled.\n");
   215	    break;
   216	  case 2:
   217	    fprintf(stderr, "Interface disabled: position output\n");
   218	    break;
   219	  }
   220	
   221	  fprintf(stderr, "Initialze interface (moving to x=50,y=0)...\n");
   222	
   223	  // drive to the coordinates ?, 0
   224	  // x will not be driven, because it is too noisy and slowly
   225	  interface_drive(0, -1);
   226	  // code for "normal" drive-time for 0,0
   227	  /*  if (interface_xmax_time > interface_ymax_time) {
   228	    msleep(interface_xmax_time + interface_xmax_time/5);
   229	  } else {
   230	    msleep(interface_ymax_time + interface_ymax_time/5);
   231	    } */
   232	  // code for Y-drive-time only
   233	  msleep(interface_ymax_time + interface_ymax_time/5);
   234	  interface_drive(0, 0);
   235	
   236	  // set coordinates to ?, 0
   237	  interface_x = 50;
   238	  interface_y = 0;
   239	}
   240	
   241	
   242	/**
   243	   Drive to robot to absolute coordinates
   244	   @param x (int): absolute x coordinate
   245	   @param y (int): absolute y coordinate
   246	*/
   247	void interface_driveto(int x, int y) {
   248	  int i;
   249	
   250	  // absulute coordinates are in range?
   251	  if (x > interface_xmax) {
   252	    x = interface_xmax;
   253	  }
   254	  if (x < 0) {
   255	    x = 0;
   256	  }
   257	  if (y > interface_ymax) {
   258	    y = interface_ymax;
   259	  }
   260	  if (y < 0) {
   261	    y = 0;
   262	  }
   263	
   264	  // calculate relative coordinates
   265	  x = (x - interface_x);
   266	  y = (y - interface_y);
   267	
   268	  // store new coordinates
   269	  interface_x += x;
   270	  interface_y += y;
   271	
   272	  //  if (interface_mode == 2) {
   273	    fprintf(stderr, "x = %03d ; y = %03d\n",
   274		    interface_x, interface_y);
   275	    //  }
   276	
   277	  // if any change has to be done...
   278	  if ((x != 0) || (y != 0)) {
   279	    // convert the relative distance into a time in which this
   280	    // distance is covered
   281	    x = (int) ((double)
   282		       (((double) x * (double) interface_xmax_time)
   283			/ (double) interface_xmax));
   284	    y = (int) ((double)
   285		       (((double) y * (double) interface_ymax_time)
   286			/ (double) interface_ymax));
   287	
   288	
   289	    // if the drive-time for both axies is not equal, drive the
   290	    // greater time as long as the times are equal
   291	    if ((x != 0) && (y != 0)) {
   292	      interface_drive(x, y);
   293	      msleep(getmin(abs(x), abs(y)));
   294	      interface_drive(0, 0);
   295	
   296	      i = getmin(abs(x), abs(y));
   297	      x = x/abs(x) * (abs(x) - i);
   298	      y = y/abs(y) * (abs(y) - i);
   299	    }
   300	
   301	    // if there is any time left, drive these
   302	    if ((x != 0) || (y != 0)) {
   303	      interface_drive(x, y);
   304	      msleep(getmax(abs(x), abs(y)));
   305	      interface_drive(0, 0);
   306	    }
   307	  }
   308	}
   309	
   310	
   311	/** Stop the interface, switch all off. */
   312	void interface_stop() {
   313	  fprintf(stderr, "interface_stop()\n");
   314	  // block the interface
   315	  interface_mode = 3;
   316	  // switch all off.
   317	  //  interface_drive(0, 0);
   318	
   319	  //  interface_drive do not work, because "interface_mode = 3"
   320	  //  switches to IO-access off. For this reason, the functions were
   321	  //  directly used.
   322	  outb((inb(interface_ioport) & 240), interface_ioport);
   323	}
\end{lstlisting}}
