{\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 56 #include 57 #include 58 #include 59 #include 60 #include 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}}