pluto_servo.comp
1 // Copyright (C) 2007 Jeff Epler 2 // 3 // This program is free software; you can redistribute it and/or modify 4 // it under the terms of the GNU General Public License as published by 5 // the Free Software Foundation; either version 2 of the License, or 6 // (at your option) any later version. 7 // 8 // This program is distributed in the hope that it will be useful, 9 // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 // GNU General Public License for more details. 12 // 13 // You should have received a copy of the GNU General Public License 14 // along with this program; if not, write to the Free Software 15 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 16 17 component pluto_servo """Hardware driver and firmware for the Pluto-P parallel-port FPGA, for use with servo machines."""; 18 19 description """ 20 Pluto_servo is a LinuxCNC software driver and associated firmware that allow the Pluto-P board to be used to control a servo-based CNC machine. 21 22 The driver has 4 PWM channels, 4 quadrature channels with index pulse, 18 23 digital outputs (8 shared with PWM), and 20 digital inputs (12 shared with 24 quadrature). 25 26 .SS Encoders 27 The encoder pins and parameters conform to the `canonical encoder' interface 28 described in the HAL manual. It operates in `x4 mode'. 29 30 The sample rate of the encoder is 40MHz. The maximum number quadrature rate is 31 8191 counts per LinuxCNC servo cycle. For correct handling of the index pulse, the 32 number of encoder counts per revolution must be less than 8191. 33 34 .SS PWM 35 The PWM pins and parameters conform to the `canonical analog output' interface 36 described in the HAL manual. The output pins are `up/down' or `pwm/dir' 37 pins as described in the documentation of the `pwmgen' component. 38 39 Internally the PWM generator is based on a 12-bit, 40MHz counter, giving 4095 40 duty cycles from -100% to +100% and a frequency of approximately 19.5kHz. In 41 PDM mode, the duty periods are approximately 100ns long. 42 43 .SS Digital I/O 44 The digital output pins conform to the `canonical digital output' interface 45 described in the HAL manual. 46 47 The digital input pins conform to the `canonical digital input' interface 48 described in the HAL manual. 49 """; 50 pin out s32 encoder.#.count[4]; 51 pin out float encoder.#.position[4]; 52 pin out float encoder.#.velocity[4]; 53 pin in bit encoder.#.reset[4]; 54 pin io bit encoder.#.index-enable[4] """encoder.\\fIM\\fR corresponds to the 55 pins labeled QA\\fIM\\fR, QB\\fIM\\fR, and QZ\\fIM\\fR on the pinout diagram"""; 56 param rw float encoder.#.scale[4] =1; 57 58 param rw bit encoder.z-polarity "Set to TRUE if the index pulse is active low, FALSE if it is active high. Affects all encoders."; 59 60 pin in float pwm.#.value[4]; 61 pin in bit pwm.#.enable[4] "pwm.\\fIM\\fR corresponds to the pins labeled UP\\fIM\\fR and DN\\fIM\\fR on the pinout diagram"; 62 param rw float pwm.#.offset[4]; 63 param rw float pwm.#.scale[4]=1; 64 param rw float pwm.#.max-dc[4]=1; 65 param rw float pwm.#.min-dc[4]=0; 66 param rw bit pwm.#.pwmdir[4]=0 67 "Set to TRUE use PWM+direction mode. Set to FALSE to use Up/Down mode."; 68 69 param rw bit pwm.is-pdm "Set to TRUE to use PDM (also called interleaved PWM) mode. Set to FALSE to use traditional PWM mode. Affects all PWM outputs."; 70 71 72 pin in bit dout.##[20] 73 """dout.\\fI0M\\fR corresponds to the pin labeled 74 OUT\\fIM\\fR on the pinout diagram. Other pins are shared with the PWM 75 function, as follows: 76 .RS 6 77 .ta T 10 6 78 .PP 79 \\fBPin\tShared\\fR 80 .PP 81 \\fBLabel\twith\\fR 82 .PP 83 dout.10\tUP0 84 .PP 85 dout.10\tUP0 86 .PP 87 dout.12\tUP1 88 .PP 89 dout.14\tUP2 90 .PP 91 dout.18\tUP3 92 .PP 93 dout.11\tDOWN0 94 .PP 95 dout.13\tDOWN1 96 .PP 97 dout.15\tDOWN2 98 .PP 99 dout.19\tDOWN3 100 .PP 101 .RE 102 .DT"""; 103 param rw bit dout.##-invert[20] 104 "If TRUE, the output on the corresponding \\fBdout.\\fIMM\\fR is inverted."; 105 106 pin out bit din.##[20]; 107 pin out bit din.##_not[20] 108 """For M=0 through 7, din.\\fI0M\\fR corresponds to the pin labeled 109 IN\\fIM\\fR on the pinout diagram. Other pins are shared with the encoder 110 function, as follows: 111 .RS 6 112 .ta T 10 6 113 .PP 114 \\fBPin\tShared\\fR 115 .PP 116 \\fBLabel\twith\\fR 117 .PP 118 din.8\tQZ0 119 .PP 120 din.9\tQZ1 121 .PP 122 din.10\tQZ2 123 .PP 124 din.11\tQZ3 125 .PP 126 din.12\tQB0 127 .PP 128 din.13\tQB1 129 .PP 130 din.14\tQB2 131 .PP 132 din.15\tQB3 133 .PP 134 din.16\tQA0 135 .PP 136 din.17\tQA1 137 .PP 138 din.18\tQA2 139 .PP 140 din.19\tQA3 141 .PP 142 .RE 143 .DT"""; 144 145 param rw u32 communication_error """Incremented each time 146 pluto-servo.read detects an error code in the EPP status register. While 147 this register is nonzero, new values are not being written to the Pluto-P 148 board, and the status of digital outputs and the PWM duty cycle of the PWM 149 outputs will remain unchanged. If the watchdog is enabled, it will activate 150 soon after the communication error is detected. To continue after a 151 communication error, set this parameter back to zero."""; 152 153 param rw s32 debug_0; 154 param rw s32 debug_1 """These parameters can display values which are useful to developers or for debugging the driver and firmware. They are not useful for integrators or users."""; 155 156 option singleton; 157 option extra_setup; 158 option extra_cleanup; 159 160 option data internal; 161 162 function read "Read all the inputs from the pluto-servo board"; 163 function write "Write all the outputs on the pluto-servo board"; 164 165 see_also """The \\fIpluto_servo\\fR section in the HAL User Manual, which shows the location of each physical pin on the pluto board."""; 166 167 modparam dummy ioaddr = 0x378 "The base address of the parallel port."; 168 modparam dummy ioaddr_hi = 0 169 """The secondary address of the parallel port, used to set EPP 170 mode. 0 means to use ioaddr + 0x400. -1 means there is no 171 secondary address. The secondary address is used to set the port to EPP 172 mode."""; 173 174 modparam dummy epp_wide = 1 175 """Set to zero to disable the "wide EPP mode". "Wide" mode allows a 16- 176 and 32-bit EPP transfers, which can reduce the time spent in the read and write 177 functions. However, this may not work on all EPP parallel ports."""; 178 179 modparam dummy watchdog = 1 180 """Set to zero to disable the "hardware watchdog". "Watchdog" will 181 tristate all outputs approximately 6ms after the last execution of 182 \\fBpluto-servo.write\\fR, which adds some protection in the case of LinuxCNC 183 crashes."""; 184 185 modparam int test_encoder = 0 186 "Internally connect dout0..2 to QA0, QB0, QZ0 to test quadrature counting"; 187 188 license "GPL"; 189 190 include "hal/drivers/pluto_common.h"; 191 ;; 192 193 194 typedef struct { 195 int64_t last_index[4]; 196 int64_t last_count[4]; 197 int64_t reset_count[4]; 198 } internal; 199 200 #define W 14 201 #define MASK ((1<<W)-1) 202 203 int PWM(int enable, double value, double offset, double scale, double min_dc, double max_dc, int dio0, int dio1, int is_pdm, int is_pwmdir) { 204 int result; 205 if(enable == 0) return 0; 206 207 value = value / scale + offset; 208 if(value < -max_dc) value = -max_dc; 209 else if(value > -min_dc && value < 0) value = -min_dc; 210 else if(value > 0 && value < min_dc) value = min_dc; 211 else if(value > max_dc) value = max_dc; 212 213 value = 2047 * value; 214 if(value < -2047) value = -2047; 215 if(value > 2047) value = 2047; 216 217 if(value < 0) { 218 if(is_pwmdir) { 219 result = (1<<13) | (int)(-value); 220 } else { 221 result = (1<<15) | (int)(-value); 222 } 223 } else result = value; 224 if(is_pdm) result |= 1<<14; 225 if(dio0) result ^= 1<<12; 226 if(dio1) result ^= 1<<13; 227 228 return result; 229 } 230 231 FUNCTION(write) { 232 int r = 0; 233 int i; 234 if(communication_error) return; 235 236 EPP_ADDR(0); 237 238 for(i=0; i<4; i++) { 239 if(pwm_max_dc(i) > 1) pwm_max_dc(i) = 1; 240 else if(pwm_max_dc(i) < 0) pwm_max_dc(i) = 0; 241 if(pwm_min_dc(i) < 0) pwm_min_dc(i) = 0; 242 else if(pwm_min_dc(i) > pwm_max_dc(i)) pwm_min_dc(i) = pwm_max_dc(i); 243 } 244 245 #define D(x) (!dout(x) ^ !dout_invert(x)) 246 #define P(x,y) PWM(pwm_enable(x), pwm_value(x), pwm_offset(x), pwm_scale(x), \ 247 pwm_min_dc(x), pwm_max_dc(x), D(y), D(y+1), pwm_is_pdm, pwm_pwmdir(x)) 248 write32( P(0,10) | (P(1, 12) << 16)); 249 write32( P(2,14) | (P(3, 16) << 16)); 250 251 for(i=0; i<10; i++) { 252 if(!dout(i) ^ !dout_invert(i)) r |= (1<<i); 253 } 254 if (encoder_z_polarity) r |= 0x8000; 255 if (watchdog) r |= 0x4000; 256 if (test_encoder) r |= 0x2000; 257 write16(r); 258 } 259 260 FUNCTION(read) { 261 int i; 262 __u32 ppdata; 263 264 EPP_ADDR(0); 265 EPP_DIR_READ(); 266 267 for(i=0; i<4; i++) { 268 int64_t count, index; 269 int newlow, indlow, indexed; 270 int reset; 271 ppdata = read32(); 272 reset = encoder_reset(i); 273 if(i == 0) { 274 int status = inb(ioaddr+1) & 1; 275 if(status) { 276 communication_error ++; 277 pluto_clear_error_register(); 278 } 279 if(communication_error) { EPP_DIR_WRITE(); return; } 280 } 281 282 newlow = ppdata & MASK; 283 indlow = (ppdata >> W) & MASK; 284 indexed = ppdata & (1<<(2*W)); 285 286 count = extend(data.last_count[i], newlow, W); 287 if(indexed) 288 index = extend(count, indlow, W); 289 else 290 index = data.last_index[i]; 291 292 if(encoder_index_enable(i) && indexed) { 293 encoder_index_enable(i) = 0; 294 data.reset_count[i] = index; 295 } 296 if(reset) encoder_velocity(i) = 0; 297 else encoder_velocity(i) = (count - data.last_count[i]) / 298 encoder_scale(i) / fperiod; 299 data.last_index[i] = index; 300 data.last_count[i] = count; 301 if(reset) data.reset_count[i] = count; 302 encoder_count(i) = count - data.reset_count[i]; 303 encoder_position(i) = encoder_count(i) / encoder_scale(i); 304 305 if(i == 0) { 306 debug_0 = ppdata; debug_1 = count; 307 } 308 } 309 310 ppdata = read32(); 311 312 for(i=0; i< 20; i++) { 313 int b = ppdata & (1<<i); 314 din(i) = !!b; din_not(i) = !b; 315 } 316 317 EPP_DIR_WRITE(); 318 } 319 320 #include "hal/drivers/pluto_servo_rbf.h" 321 322 EXTRA_SETUP() { 323 return pluto_setup(firmware); 324 } 325 326 EXTRA_CLEANUP() { 327 pluto_cleanup(); 328 } 329 330 // vim:sts=4:et:sw=4