bldc.comp
1 component bldc "BLDC and AC-servo control component "; 2 // 1.3.2 (26/1/11 20:10) 3 4 pin in bit hall1 if personality & 0x01 "Hall sensor signal 1"; 5 pin in bit hall2 if personality & 0x01 "Hall sensor signal 2"; 6 pin in bit hall3 if personality & 0x01 "Hall sensor signal 3"; 7 pin out bit hall_error if personality & 0x01 """Indicates that the selected hall 8 pattern gives inconsistent rotor position data. This can be due to the pattern 9 being wrong for the motor, or one or more sensors being unconnected or broken. 10 A consistent pattern is not neceesarily valid, but an inconsistent one can never 11 be valid."""; 12 13 pin in bit C1 if (personality & 0x10) "Fanuc Gray-code bit 0 input"; 14 pin in bit C2 if (personality & 0x10) "Fanuc Gray-code bit 1 input"; 15 pin in bit C4 if (personality & 0x10) "Fanuc Gray-code bit 2 input"; 16 pin in bit C8 if (personality & 0x10) "Fanuc Gray-code bit 3 input"; 17 18 pin in float value "PWM master amplitude input"; 19 20 pin in float lead-angle = 90 if personality & 0x06 21 "The phase lead between the electrical vector and the rotor position in degrees"; 22 23 pin in bit rev 24 """Set this pin true to reverse the motor. Negative PWM amplitudes will also 25 reverse the motor and there will generally be a Hall pattern that runs the motor 26 in each direction too."""; 27 28 pin in float frequency if (personality & 0x0F)==0 """Frequency input for motors 29 with no feedback at all, or those with only an index (which is ignored)"""; 30 31 pin in float initvalue = 0.2 if personality & 0x04 """The current to be used for 32 the homing sequence in applications where an incremental encoder is used with no 33 hall-sensor feedback"""; 34 35 pin in signed rawcounts = 0 if personality & 0x06 36 """Encoder counts input. This must be linked to the encoder rawcounts pin or 37 encoder index resets will cause the motor commutation to fail"""; 38 39 pin io bit index-enable if personality & 0x08 """This pin should be connected to 40 the associated encoder index-enable pin to zero the encoder when it passes index 41 This is only used indicate to the bldc control component that an index has been 42 seen"""; 43 44 pin in bit init if (personality & 0x05) == 4 45 """A rising edge on this pin starts the motor alignment sequence. This pin 46 should be connected in such a way that the motors re-align any time that 47 encoder monitoring has been interrupted. Typically this will only be at machine 48 power-off. 49 The alignment process involves powering the motor phases in such a way as to 50 put the motor in a known position. The encoder counts are then stored in the 51 \\fBoffset\\fP parameter. The alignment process will tend to cause a following 52 error if it is triggered while the axis is enabled, so should be set before the 53 matching axis.N.enable pin. The complementary \\fBinit-done\\fP pin can be used 54 to handle the required sequencing. 55 56 Both pins can be ignored if the encoder offset is known explicitly, such as is 57 the case with an absolute encoder. In that case the \\fBoffset\\fP parameter 58 can be set directly in the HAL file"""; 59 60 pin out bit init-done = 0 if (personality & 0x05) == 4 61 "Indicates homing sequence complete"; 62 63 pin out float A-value if (personality & 0xF00)==0 "Output amplitude for phase A"; 64 pin out float B-value if (personality & 0xF00)==0 "Output amplitude for phase B"; 65 pin out float C-value if (personality & 0xF00)==0 "Output amplitude for phase C"; 66 pin out bit A-on if (personality & 0xF00)==0x100 "Output bit for phase A"; 67 pin out bit B-on if (personality & 0xF00)==0x100 "Output bit for phase B"; 68 pin out bit C-on if (personality & 0xF00)==0x100 "Output bit for phase C"; 69 pin out float A-high if (personality & 0xF00)==0x200 "High-side driver for phase A"; 70 pin out float B-high if (personality & 0xF00)==0x200 "High-side driver for phase B"; 71 pin out float C-high if (personality & 0xF00)==0x200 "High-side driver for phase C"; 72 pin out float A-low if (personality & 0xF00)==0x200 "Low-side driver for phase A"; 73 pin out float B-low if (personality & 0xF00)==0x200 "Low-side driver for phase B"; 74 pin out float C-low if (personality & 0xF00)==0x200 "Low-side driver for phase C"; 75 pin out bit A-high-on if (personality & 0xF00)==0x300 "High-side driver for phase A"; 76 pin out bit B-high-on if (personality & 0xF00)==0x300 "High-side driver for phase B"; 77 pin out bit C-high-on if (personality & 0xF00)==0x300 "High-side driver for phase C"; 78 pin out bit A-low-on if (personality & 0xF00)==0x300 "Low-side driver for phase A"; 79 pin out bit B-low-on if (personality & 0xF00)==0x300 "Low-side driver for phase B"; 80 pin out bit C-low-on if (personality & 0xF00)==0x300"Low-side driver for phase C"; 81 82 pin out bit hall1-out if (personality & 0x400) "Hall 1 output"; 83 pin out bit hall2-out if (personality & 0x400) "Hall 2 output"; 84 pin out bit hall3-out if (personality & 0x400) "Hall 3 output"; 85 86 pin out bit C1-out if (personality & 0x800) "Fanuc Gray-code bit 0 output"; 87 pin out bit C2-out if (personality & 0x800) "Fanuc Gray-code bit 1 output"; 88 pin out bit C4-out if (personality & 0x800) "Fanuc Gray-code bit 2 output"; 89 pin out bit C8-out if (personality & 0x800) "Fanuc Gray-code bit 3 output"; 90 91 pin out float phase-angle = 0 92 """Phase angle including lead/lag angle after encoder zeroing etc. Useful for 93 angle/current drives. This value has a range of 0 to 1 and measures electrical 94 revolutions. It will have two zeros for a 4 pole motor, three for a 6-pole etc"""; 95 96 pin out float rotor-angle = 0 97 """Rotor angle after encoder zeroing etc. Useful for angle/current drives which 98 add their own phase offset such as the 8i20. This value has a range of 0 to 1 99 and measures electrical revolutions. It will have two zeros for a 4 pole motor, 100 three for a 6-pole etc"""; 101 102 pin out float out 103 "Current output, including the effect of the dir pin and the alignment sequence"; 104 105 pin out bit out-dir 106 "Direction output, high if /fBvalue/fR is negative XOR /fBrev/fR is true."; 107 108 pin out float out-abs 109 "Absolute value of the input value"; 110 111 param r signed in_type = -1 "state machine output, will probably hide after debug"; 112 param r signed out_type = -1 "state machine output, will probably hide after debug"; 113 114 param rw signed scale = 512 if personality & 0x06 115 "The number of encoder counts per rotor revolution."; 116 param rw signed poles = 4 if personality & 0x06 117 """The number of motor poles. The encoder scale will be divided by this value 118 to determine the number of encoder counts per electrical revolution"""; 119 param rw signed encoder-offset = 0 if personality & 0x0A 120 """The offset, in encoder counts, between the motor electrical zero and the 121 encoder zero modulo the number of counts per electrical revolution"""; 122 param r signed offset_measured = 0 if personality & 0x04 123 """The encoder offset measured by the homing sequence (in certain modes)"""; 124 param rw float drive-offset = 0 """The angle, in degrees, 125 applied to the commanded angle by the drive in degrees. This value is only used 126 during the homing sequence of drives with incremental encoder feedback. It is 127 used to back-calculate from commanded angle to actual phase angle. It is only 128 relevant to drives which expect rotor-angle input rather than phase-angle 129 demand. Should be 0 for most drives. """; 130 131 param rw unsigned output-pattern=25 if personality & 0x400 132 """Commutation pattern to be output in Hall Signal translation mode. See the 133 description of /fBpattern/fR for details"""; 134 135 param rw unsigned pattern=25 if personality & 0x01 136 """Commutation pattern to use, from 0 to 47. Default is type 25. 137 Every plausible combination is included. The table shows the excitation pattern 138 along the top, and the pattern code on the left hand side. The table entries 139 are the hall patterns in H1, H2, H3 order. 140 Common patterns are: 141 0 (30 degree commutation) and 26, its reverse. 142 17 (120 degree). 143 18 (alternate 60 degree). 144 21 (300 degree, Bodine). 145 22 (240 degree). 146 25 (60 degree commutation). 147 148 Note that a number of incorrect commutations will have non-zero net torque 149 which might look as if they work, but don't really. 150 151 If your motor lacks documentation it might be worth trying every pattern. 152 153 .ie '\*[.T]'html' \\{\\ 154 .HTML \\ 155 <STYLE> \\ 156 #pattern TD { text-align: center; padding-left: .5ex; padding-right: .5ex } \\ 157 #pattern TH { text-align: center; padding-left: .5ex; padding-right: .5ex } \\ 158 #pattern TD.W { text-align: right; } \\ 159 </STYLE> \\ 160 <TABLE ID="pattern" STYLE="border: 1px solid black; border-collapse: collapse"> \\ 161 <COL SPAN=7 STYLE="margin: .2ex"><COL SPAN=1 STYLE="border-left: 1px solid black"> \\ 162 <TR><TD> <TH COLSPAN=6 CLASS=W>Phases, Source - Sink \\ 163 <TR><TH CLASS=W>pat<TH CLASS=W>B-A<TH CLASS=W>C-A<TH CLASS=W>C-B<TH CLASS=W>A-B<TH CLASS=W>A-C<TH CLASS=W>B-C \\ 164 <TR><TH>0<TD>000<TD>001<TD>011<TD>111<TD>110<TD>100 \\ 165 <TR><TH>1<TD>001<TD>000<TD>010<TD>110<TD>111<TD>101 \\ 166 <TR><TH>2<TD>000<TD>010<TD>011<TD>111<TD>101<TD>100 \\ 167 <TR><TH>3<TD>001<TD>011<TD>010<TD>110<TD>100<TD>101 \\ 168 <TR><TH>4<TD>010<TD>011<TD>001<TD>101<TD>100<TD>110 \\ 169 <TR><TH>5<TD>011<TD>010<TD>000<TD>100<TD>101<TD>111 \\ 170 <TR><TH>6<TD>010<TD>000<TD>001<TD>101<TD>111<TD>110 \\ 171 <TR><TH>7<TD>011<TD>001<TD>000<TD>100<TD>110<TD>111 \\ 172 <TR><TH>8<TD>000<TD>001<TD>101<TD>111<TD>110<TD>010 \\ 173 <TR><TH>9<TD>001<TD>000<TD>100<TD>110<TD>111<TD>011 \\ 174 <TR><TH>10<TD>000<TD>010<TD>110<TD>111<TD>101<TD>001 \\ 175 <TR><TH>11<TD>001<TD>011<TD>111<TD>110<TD>100<TD>000 \\ 176 <TR><TH>12<TD>010<TD>011<TD>111<TD>101<TD>100<TD>000 \\ 177 <TR><TH>13<TD>011<TD>010<TD>110<TD>100<TD>101<TD>001 \\ 178 <TR><TH>14<TD>010<TD>000<TD>100<TD>101<TD>111<TD>011 \\ 179 <TR><TH>15<TD>011<TD>001<TD>101<TD>100<TD>110<TD>010 \\ 180 <TR><TH>16<TD>000<TD>100<TD>101<TD>111<TD>011<TD>010 \\ 181 <TR><TH>17<TD>001<TD>101<TD>100<TD>110<TD>010<TD>011 \\ 182 <TR><TH>18<TD>000<TD>100<TD>110<TD>111<TD>011<TD>001 \\ 183 <TR><TH>19<TD>001<TD>101<TD>111<TD>110<TD>010<TD>000 \\ 184 <TR><TH>20<TD>010<TD>110<TD>111<TD>101<TD>001<TD>000 \\ 185 <TR><TH>21<TD>011<TD>111<TD>110<TD>100<TD>000<TD>001 \\ 186 <TR><TH>22<TD>010<TD>110<TD>100<TD>101<TD>001<TD>011 \\ 187 <TR><TH>23<TD>011<TD>111<TD>101<TD>100<TD>000<TD>010 \\ 188 <TR><TH>24<TD>100<TD>101<TD>111<TD>011<TD>010<TD>000 \\ 189 <TR><TH>25<TD>101<TD>100<TD>110<TD>010<TD>011<TD>001 \\ 190 <TR><TH>26<TD>100<TD>110<TD>111<TD>011<TD>001<TD>000 \\ 191 <TR><TH>27<TD>101<TD>111<TD>110<TD>010<TD>000<TD>001 \\ 192 <TR><TH>28<TD>110<TD>111<TD>101<TD>001<TD>000<TD>010 \\ 193 <TR><TH>29<TD>111<TD>110<TD>100<TD>000<TD>001<TD>011 \\ 194 <TR><TH>30<TD>110<TD>100<TD>101<TD>001<TD>011<TD>010 \\ 195 <TR><TH>31<TD>111<TD>101<TD>100<TD>000<TD>010<TD>011 \\ 196 <TR><TH>32<TD>100<TD>101<TD>001<TD>011<TD>010<TD>110 \\ 197 <TR><TH>33<TD>101<TD>100<TD>000<TD>010<TD>011<TD>111 \\ 198 <TR><TH>34<TD>100<TD>110<TD>010<TD>011<TD>001<TD>101 \\ 199 <TR><TH>35<TD>101<TD>111<TD>011<TD>010<TD>000<TD>100 \\ 200 <TR><TH>36<TD>110<TD>111<TD>011<TD>001<TD>000<TD>100 \\ 201 <TR><TH>37<TD>111<TD>110<TD>010<TD>000<TD>001<TD>101 \\ 202 <TR><TH>38<TD>110<TD>100<TD>000<TD>001<TD>011<TD>111 \\ 203 <TR><TH>39<TD>111<TD>101<TD>001<TD>000<TD>010<TD>110 \\ 204 <TR><TH>40<TD>100<TD>000<TD>001<TD>011<TD>111<TD>110 \\ 205 <TR><TH>41<TD>101<TD>001<TD>000<TD>010<TD>110<TD>111 \\ 206 <TR><TH>42<TD>100<TD>000<TD>010<TD>011<TD>111<TD>101 \\ 207 <TR><TH>43<TD>101<TD>001<TD>011<TD>010<TD>110<TD>100 \\ 208 <TR><TH>44<TD>110<TD>010<TD>011<TD>001<TD>101<TD>100 \\ 209 <TR><TH>45<TD>111<TD>011<TD>010<TD>000<TD>100<TD>101 \\ 210 <TR><TH>46<TD>110<TD>010<TD>000<TD>001<TD>101<TD>111 \\ 211 <TR><TH>47<TD>111<TD>011<TD>001<TD>000<TD>100<TD>110 \\ 212 </TABLE> 213 \\} 214 .el \\{\\ 215 216 .TS 217 box tab(;); 218 cb s s s s s s 219 cb|cb cb cb cb cb cb 220 c | c c c c c r. 221 Phases, Source - Sink 222 _ 223 pat;B-A;C-A;C-B;A-B;A-C;B-C 224 _ 225 0;000;001;011;111;110;100 226 1;001;000;010;110;111;101 227 2;000;010;011;111;101;100 228 3;001;011;010;110;100;101 229 4;010;011;001;101;100;110 230 5;011;010;000;100;101;111 231 6;010;000;001;101;111;110 232 7;011;001;000;100;110;111 233 8;000;001;101;111;110;010 234 9;001;000;100;110;111;011 235 10;000;010;110;111;101;001 236 11;001;011;111;110;100;000 237 12;010;011;111;101;100;000 238 13;011;010;110;100;101;001 239 14;010;000;100;101;111;011 240 15;011;001;101;100;110;010 241 16;000;100;101;111;011;010 242 17;001;101;100;110;010;011 243 18;000;100;110;111;011;001 244 19;001;101;111;110;010;000 245 20;010;110;111;101;001;000 246 21;011;111;110;100;000;001 247 22;010;110;100;101;001;011 248 23;011;111;101;100;000;010 249 24;100;101;111;011;010;000 250 25;101;100;110;010;011;001 251 26;100;110;111;011;001;000 252 27;101;111;110;010;000;001 253 28;110;111;101;001;000;010 254 29;111;110;100;000;001;011 255 30;110;100;101;001;011;010 256 31;111;101;100;000;010;011 257 32;100;101;001;011;010;110 258 33;101;100;000;010;011;111 259 34;100;110;010;011;001;101 260 35;101;111;011;010;000;100 261 36;110;111;011;001;000;100 262 37;111;110;010;000;001;101 263 38;110;100;000;001;011;111 264 39;111;101;001;000;010;110 265 40;100;000;001;011;111;110 266 41;101;001;000;010;110;111 267 42;100;000;010;011;111;101 268 43;101;001;011;010;110;100 269 44;110;010;011;001;101;100 270 45;111;011;010;000;100;101 271 46;110;010;000;001;101;111 272 47;111;011;001;000;100;110 273 .TE 274 \\} 275 """; 276 277 description """ 278 This component is designed as an interface between the most common forms of 279 three-phase motor feedback devices and the corresponding types of drive. However 280 there is no requirement that the motor and drive should necessarily be of 281 inherently compatible types. 282 .SH SYNOPSIS 283 (ignore the auto-generated SYNOPSIS above) 284 .SH 285 .HP 286 .B loadrt bldc cfg=qi6,aH\\fB 287 Each instance of the component is defined by a group of letters describing the 288 input and output types. A comma separates individual instances of the component. 289 .SH Tags 290 Input type definitions are all lower-case. 291 292 \\fBn\\fR No motor feedback. This mode could be used to drive AC 293 induction motors, but is also potentially useful for creating free-running motor 294 simulators for drive testing. 295 296 \\fBh\\fR Hall sensor input. Brushless DC motors (electronically commutated 297 permanent magnet 3-phase motors) typically use a set of three Hall sensors to 298 measure the angular position of the rotor. A lower-case \\fBh\\fR in the cfg 299 string indicates that these should be used. 300 301 \\fBa\\fR Absolute encoder input. (Also possibly used by some forms of Resolver 302 conversion hardware). The presence of this tag over-rides all other inputs. Note 303 that the component still requires to be be connected to the \\fBrawcounts\\fR 304 encoder pin to prevent loss of commutation on index-reset. 305 306 \\fBq\\fR Incremental (quadrature) encoder input. If this input is used then 307 the rotor will need to be homed before the motor can be run. 308 309 \\fBi\\fR Use the index of an incremental encoder as a home reference. 310 311 \\fBf\\fR Use a 4-bit Gray-scale patttern to determine rotor alignment. This 312 scheme is only used on the Fanuc "Red Cap" motors. This mode could be used to 313 control one of these motors using a non-Fanuc drive. 314 315 Output type descriptions are all upper-case. 316 317 \\fBDefaults\\fR The component will always calculate rotor angle, phase angle 318 and the absolute value of the input \\fBvalue\\fR for interfacing with drives 319 such as the Mesa 8i20. It will also default to three individual, bipolar phase 320 output values if no other output type modifiers are used. 321 322 \\fBB\\fR Bit level outputs. Either 3 or 6 logic-level outputs indicating which 323 high or low gate drivers on an external drive should be used. 324 325 \\fB6\\fR Create 6 rather than the default 3 outputs. In the case of numeric 326 value outputs these are separate positive and negative drive amplitudes. Both 327 have positive magnitude. 328 329 \\fBH\\fR Emulated Hall sensor output. This mode can be used to control a drive 330 which expects 3x Hall signals, or to convert between a motor with one hall 331 pattern and a drive which expects a different one. 332 333 \\fBF\\fR Emulated Fanuc Red Cap Gray-code encoder output. This mode might be 334 used to drive a non-Fanuc motor using a Fanuc drive intended for the "Red-Cap" 335 motors. 336 337 \\fBT\\fR Force Trapezoidal mode. 338 339 .SH OPERATING MODES 340 The component can control a drive in either Trapezoidal or Sinusoidal mode, but 341 will always default to sinusoidal if the input and output modes allow it. This 342 can be over-ridden by the \\fBT\\fR tag. Sinusoidal commutation is significantly 343 smoother (trapezoidal commutation induces 13% torque ripple). 344 345 .SH ROTOR HOMING. 346 To use an encoder for commutation a reference 0-degrees point must be found. 347 The component uses the convention that motor zero is the point that an unloaded 348 motor aligns to with a positive voltage on the A (or U) terminal and the B & C 349 (or V and W) terminals connected together and to \-ve voltage. There will be 350 two such positions on a 4-pole motor, 3 on a 6-pole and so on. They are all 351 functionally equivalent as far as driving the motor is concerned. 352 If the motor has Hall sensors then the motor can be started in trapezoidal 353 commutation mode, and will switch to sinusoidal commutation when an alignment is 354 found. If the mode is \\fBqh\\fR then the first Hall state-transition will be 355 used. If the mode is \\fBqhi\\fR then the encoder index will be used. This 356 gives a more accurate homing position if the distance in encoder counts between 357 motor zero and encoder index is known. To force homing to the Hall edges instead 358 simply omit the \\fBi\\fR. 359 360 Motors without Hall sensors may be homed in synchronous/direct mode. 361 The better of these options is to home to the encoder zero using the \\fBiq\\fR 362 config parameter. When the \\fBinit\\fR pin goes high the motor will rotate (in 363 a direction determined by the \\fBrev\\fR pin) until the encoder indicates an 364 index-latch (the servo thread runs too slowly to rely on detecting an encoder 365 index directly). 366 If there is no encoder index or its location relative to motor zero can not be 367 found, then an alternative is to use \\fImagnetic\\fR homing using the \\fBq\\fR 368 config. In this mode the motor will go through an alignment sequence ending at 369 motor zero when the init pin goes high It will then set the final position as 370 motor zero. Unfortunately the motor is rather \\fIspringy\\fR in this mode and 371 so alignment is likely to be fairly sensitive to load. 372 """; 373 374 license "GPL"; 375 376 author "Andy Pugh"; 377 378 function _; 379 380 option extra_setup yes; 381 option count_function yes; 382 383 variable int old_init = 0; 384 variable char old_ph = 000; 385 variable int old_pattern = -1; 386 variable double counter = 0; 387 variable rtapi_s64 long_rawcounts = 0; 388 variable rtapi_s64 old_long_rawcounts = 0; 389 variable rtapi_s32 old_rawcounts = 0; 390 variable int force_trap = 0; 391 392 ;; 393 394 #include <rtapi_math.h> 395 #define MAX_CHAN 8 396 #define NUM_TAG 8 397 398 static char *cfg[NUM_TAG]; 399 RTAPI_MP_ARRAY_STRING(cfg, MAX_CHAN, "Description of each motor"); 400 401 /*dir H1 H2 H3 pattern 402 000 001 010 011 100 101 110 111 */ 403 static unsigned int P[]={ 404 024, 014, 000, 012, 021, 000, 041, 042, 405 014, 024, 012, 000, 000, 021, 042, 041, 406 024, 000, 014, 012, 021, 041, 000, 042, 407 000, 024, 012, 014, 041, 021, 042, 000, 408 000, 012, 024, 014, 041, 042, 021, 000, 409 012, 000, 014, 024, 042, 041, 000, 021, 410 014, 012, 024, 000, 000, 042, 021, 041, 411 012, 014, 000, 024, 042, 000, 041, 021, 412 024, 014, 021, 000, 000, 012, 041, 042, 413 014, 024, 000, 021, 012, 000, 042, 041, 414 024, 021, 014, 000, 000, 041, 012, 042, 415 021, 024, 000, 014, 041, 000, 042, 012, 416 021, 000, 024, 014, 041, 042, 000, 012, 417 000, 021, 014, 024, 042, 041, 012, 000, 418 014, 000, 024, 021, 012, 042, 000, 041, 419 000, 014, 021, 024, 042, 012, 041, 000, 420 024, 000, 021, 041, 014, 012, 000, 042, 421 000, 024, 041, 021, 012, 014, 042, 000, 422 024, 021, 000, 041, 014, 000, 012, 042, 423 021, 024, 041, 000, 000, 014, 042, 012, 424 021, 041, 024, 000, 000, 042, 014, 012, 425 041, 021, 000, 024, 042, 000, 012, 014, 426 000, 041, 024, 021, 012, 042, 014, 000, 427 041, 000, 021, 024, 042, 012, 000, 014, 428 021, 000, 041, 042, 024, 014, 000, 012, 429 000, 021, 042, 041, 014, 024, 012, 000, 430 021, 041, 000, 042, 024, 000, 014, 012, 431 041, 021, 042, 000, 000, 024, 012, 014, 432 041, 042, 021, 000, 000, 012, 024, 014, 433 042, 041, 000, 021, 012, 000, 014, 024, 434 000, 042, 021, 041, 014, 012, 024, 000, 435 042, 000, 041, 021, 012, 014, 000, 024, 436 000, 012, 041, 042, 024, 014, 021, 000, 437 012, 000, 042, 041, 014, 024, 000, 021, 438 000, 041, 012, 042, 024, 021, 014, 000, 439 041, 000, 042, 012, 021, 024, 000, 014, 440 041, 042, 000, 012, 021, 000, 024, 014, 441 042, 041, 012, 000, 000, 021, 014, 024, 442 012, 042, 000, 041, 014, 000, 024, 021, 443 042, 012, 041, 000, 000, 014, 021, 024, 444 014, 012, 000, 042, 024, 000, 021, 041, 445 012, 014, 042, 000, 000, 024, 041, 021, 446 014, 000, 012, 042, 024, 021, 000, 041, 447 000, 014, 042, 012, 021, 024, 041, 000, 448 000, 042, 014, 012, 021, 041, 024, 000, 449 042, 000, 012, 014, 041, 021, 000, 024, 450 012, 042, 014, 000, 000, 041, 024, 021, 451 042, 012, 000, 014, 041, 000, 021, 024}; 452 453 int phases[] = {021, 024, 014, 012, 042, 041, 021, 024 }; 454 double angles[] = {0.0/36,6.0/36,12.0/36,18.0/36,24.0/36,30.0/36,36.0/36,42.0/36}; 455 int gray_b[] = {000, 001, 003, 002, 006, 007, 005, 004, 014, 015, 017, 016, 012, 456 013, 011, 010, 000}; 457 double gray_a[] = {0.0, 0.0625, 0.125, 0.1875, 0.25, 0.3125, 0.375, 0.4375, 0.5, 458 0.5625, 0.625, 0.6875, 0.75, 0.8125, 0.875, 0.9375, 1.0}; 459 460 const double pi2 = 6.283185307179586; 461 const double cos120 = -0.5; 462 const double sin120 = 0.8660254037844386; 463 const double absc[] = {2.4, 2.0, 1.2, 0.8, 0.4, 0.0}; // Alignment sequence def 464 const double V_a[] = {0, 1, 1, 1, 1, 1}; 465 const double th_a[] = {0, 0, -.25, .25, 0, 0}; 466 467 FUNCTION(_) { 468 int i; 469 double V; 470 double lead; 471 double sintheta, costheta; 472 long long lagcomped_counts; 473 char ph; 474 int trap = -1; // Flag for sinusoidal/trapezoidal output 475 476 if (rev) V = -value; 477 else V = value; 478 out_dir = (V < 0); 479 out_abs = fabs(V); 480 // Would expect to set "out" here too, but some modes need to over-ride it. 481 482 if ((personality & 0x05) == 4){ 483 if (init && !old_init && init_done) { 484 init_done = 0; 485 in_type = -1; 486 } 487 else { 488 old_init = init; 489 } 490 } 491 492 if (personality & 0x06) { 493 // Handle s32 rollover of rawcounts 494 old_long_rawcounts = long_rawcounts; 495 long_rawcounts += (rawcounts-old_rawcounts); 496 old_rawcounts = rawcounts; 497 } 498 499 trap = 0; 500 501 switch(in_type){ 502 case -2: // Error type, simply to suppress error messages 503 return; 504 case -1: // Initialisation 505 in_type = personality & 0xFF; 506 if (in_type & 0x08) index_enable = 1; 507 counter = absc[0]; 508 out_type = personality & 0x7F00; 509 force_trap = (personality & 0x8000); 510 return; 511 512 case 0x00: // Dumb VFD mode, just set the angle with no feedback 513 phase_angle += (frequency * period / 1000000000.0); 514 phase_angle -= floor(phase_angle); 515 rotor_angle = phase_angle; 516 break; 517 518 case 0x01: // Trapezoidal Hall Commutation. 519 case 0x05: // incremental encoder homing to hall transition 520 case 0x0D:// incremental encoder homing to index 521 522 trap = 1; 523 524 if (pattern > 47){ 525 rtapi_print_msg(RTAPI_MSG_ERR, 526 "Only `Hall patterns 0-47 are allowed, you have" 527 " requested pattern %i\n", pattern); 528 pattern = 0; 529 return; 530 } 531 532 i = (pattern << 3) + ((hall1 != 0) << 2) 533 + ((hall2 != 0) << 1) + (hall3 != 0); 534 ph = P[i]; 535 536 // calculate angle. This looks at transitions between motor field 537 // excitation patterns to remove the effect of different hall patterns. 538 // As the phase pattern at this point corresponds to that which is 539 // required for forwards rotation, it provides a way to infer rotor 540 // position. The fixed 90 degree Hall offset is incorporated in the 541 // angle lookup table. 542 543 for (i = 0 ; phases[i] != ph && i<8 ; i++) {} 544 rotor_angle = angles[i]; 545 rotor_angle -= floor(rotor_angle); 546 if (out_dir) phase_angle = rotor_angle + 0.25; 547 else phase_angle = rotor_angle - 0.25; 548 phase_angle -= floor(phase_angle); 549 550 if (! (ph & old_ph)){ hall_error = 1;} 551 if (pattern != old_pattern){hall_error = 0;} 552 553 if (force_trap) break; 554 555 if (in_type == 0x05 && old_ph && ph != old_ph) { // Homing to hall edges 556 for (i = 1 ; phases[i] != ph && i<8 ; i++) {} 557 if (phases[i - 1] == old_ph) { 558 rotor_angle = (angles[i] + angles[i - 1])/2;} 559 else{ 560 rotor_angle = (angles[i] + angles[i + 1])/2; 561 } 562 rotor_angle -= floor(rotor_angle); 563 if (out_dir) phase_angle = rotor_angle - 0.25; 564 else phase_angle = rotor_angle + 0.25; 565 phase_angle -= floor(phase_angle); 566 offset_measured = long_rawcounts - rotor_angle * ( 2 * scale/poles); 567 // And now we are homed, switch to sinusoidal drive 568 in_type = 0x02; 569 } 570 else if (in_type == 0x0D) { // homing to encoder index 571 if (!index_enable){ // index has reset 572 offset_measured = long_rawcounts; 573 in_type = 0x02; 574 } 575 } 576 old_ph = ph; 577 old_pattern = pattern; 578 break; 579 580 case 0x10: // Fanuc-style Gray-code input 581 case 0x14: // incremental encoder homing to gray-code transition 582 case 0x1C: // incremental encoder homing to index. 583 ph = (C1 != 0) | ((C2 != 0) << 1) 584 | ((C4 != 0) << 2) | ((C8 != 0) << 3); 585 586 for (i = 0 ; gray_b[i] != ph && i<16 ; i++) {} 587 rotor_angle = gray_a[i] + 0.03125; 588 rotor_angle -= floor(rotor_angle); 589 if (out_dir) phase_angle = rotor_angle - 0.25; 590 else phase_angle = rotor_angle + 0.25; 591 phase_angle -= floor(phase_angle); 592 593 if (force_trap) break; 594 595 if (in_type == 0x14 && old_ph && ph != old_ph) { // Homing to Gray edges 596 for (i = 1 ; gray_b[i] != ph && i<15 ; i++) {} 597 if (phases[i - 1] == old_ph) { 598 rotor_angle = gray_a[i]; 599 } 600 else{ 601 rotor_angle = gray_a[i + 1]; 602 } 603 rotor_angle -= floor(rotor_angle); 604 if (out_dir) phase_angle = rotor_angle - 0.25; 605 else phase_angle = rotor_angle + 0.25; 606 phase_angle -= floor(phase_angle); 607 offset_measured = long_rawcounts - rotor_angle * ( 2 * scale/poles); 608 // And now we are homed, switch to sinusoidal drive 609 in_type = 0x02; 610 } 611 else if (in_type == 0x1C) { // homing to encoder index 612 if (!index_enable){ // index has reset 613 offset_measured = long_rawcounts; 614 in_type = 0x02; 615 } 616 } 617 old_ph = ph; 618 619 break; 620 621 case 0x04: // Incremental encoder homing "magnetically" 622 623 if (! init) { 624 V = 0; 625 return; 626 } 627 if (! init_done) { 628 if (counter <= 0) { 629 offset_measured = long_rawcounts - phase_angle * ( 2 * scale/poles); 630 counter = absc[0]; 631 old_init = 1; 632 init_done = 1; 633 in_type = 0x02; // switch to sinusoidal commutation 634 return; 635 } 636 init_done = 0; 637 for (i = 0 ; absc[i] >= counter && i < 6 ; i++ ) {} 638 V = initvalue * (V_a[i-1] + (V_a[i] - V_a[i-1]) 639 * (counter - absc[i-1]) / (absc[i] - absc[i-1])); 640 phase_angle = (th_a[i-1] + (th_a[i] - th_a[i-1]) 641 * (counter - absc[i-1]) / (absc[i] - absc[i-1])); 642 phase_angle -= floor(phase_angle); 643 counter -= fperiod; 644 rotor_angle = phase_angle - (drive_offset/360.0); 645 rotor_angle -= floor(rotor_angle); 646 } 647 else { 648 rtapi_print_msg(RTAPI_MSG_ERR, "An error has occurred in the " 649 "bldc homing sequence. Init done without state"); 650 return;} 651 break; 652 653 case 0x0C: // Incremental encoder homing to index. 654 if (! init) return; 655 if (!index_enable){ // index has reset 656 offset_measured = long_rawcounts; 657 counter = absc[0]; 658 old_init = 1; 659 init_done = 1; 660 in_type = 0x02; 661 break; 662 } 663 if (! init_done){ 664 if (initvalue < 0) rotor_angle -= period / 1000000000.0; 665 else rotor_angle += period / 1000000000.0; 666 rotor_angle -= floor(rotor_angle); 667 phase_angle = rotor_angle; 668 V = fabs(initvalue); 669 } 670 break; 671 672 case 0x02: // Sinusoidal Commutation, homed or absolute 673 if (out_dir && !force_trap) { 674 lead = lead_angle / -360.0; 675 } else { 676 lead = lead_angle / 360.0; 677 } 678 lagcomped_counts = long_rawcounts + ((long_rawcounts - old_long_rawcounts)/2); 679 rotor_angle = (double)((lagcomped_counts - offset_measured 680 - encoder_offset)* poles/2)/scale; 681 rotor_angle -= floor(rotor_angle); 682 phase_angle = rotor_angle + lead; 683 phase_angle -= floor(phase_angle); 684 break; 685 686 case 0x03: 687 case 0x0B: 688 rtapi_print_msg(RTAPI_MSG_ERR, "Both Hall Sensors and Absolute " 689 "encoder specified on the same motor. Only the" 690 "Encoder will be used\n"); 691 in_type = 0x02; 692 return; 693 case 0x06: 694 case 0x07: 695 case 0x0E: 696 case 0x0F: 697 rtapi_print_msg(RTAPI_MSG_ERR, "Specifying the use of both absolute " 698 "and incremental encoders on the same motor is an " 699 "error. Motor disabled\n"); 700 in_type = -2; 701 return; 702 case 0x08: 703 rtapi_print_msg(RTAPI_MSG_ERR, "Driving an electronically commutated" 704 "motor with only an index for feedback is not" 705 "possible. Motor Disabled\n" ); 706 in_type = -2; 707 return; 708 case 0x09: 709 rtapi_print_msg(RTAPI_MSG_ERR, "The use of an encoder Index with " 710 "Hall sensors is not supported. Defaulting to " 711 "trapezoidal commutation\n"); 712 in_type = 0x01; 713 return; 714 case 0x0A: 715 rtapi_print_msg(RTAPI_MSG_ERR, "Index is not needed with an Absolute" 716 "encoder and will be ignored\n"); 717 in_type = 0x02; 718 return; 719 default: 720 rtapi_print_msg(RTAPI_MSG_ERR, "Unknown input type pattern (%X) in " 721 "bldc\n", in_type); 722 in_type = -2; 723 return; 724 } 725 726 /************************************************************** 727 Now calculate the output values 728 ***************************************************************/ 729 730 // equivalent trapezoidal pattern for non-hall types 731 if (!trap){ 732 for (i = 0 ; rotor_angle > angles[i] - 3.0/36 && i < 8 ; i++) {} 733 ph = phases[i]; 734 } 735 736 if (force_trap) {trap = 1;} // forced trapezoidal mode 737 738 out = V; 739 out_abs = fabs(V); 740 741 switch (out_type){ 742 case 0: // Default; 3-wire sinusoidal or trapezoidal 743 if (trap){ 744 if (ph & 040) A_value =V; 745 else if (ph & 004) A_value = -V; 746 else A_value = 0; 747 748 if (ph & 020) B_value = V; 749 else if (ph & 002) B_value = -V; 750 else B_value = 0; 751 752 if (ph & 010) C_value = V; 753 else if (ph & 001) C_value = -V; 754 else C_value = 0; 755 } 756 else 757 { 758 sintheta = sin(phase_angle * pi2); 759 costheta = cos(phase_angle * pi2); 760 A_value = out_abs * costheta; 761 B_value = out_abs * (costheta * cos120 + sintheta * sin120); 762 C_value = out_abs * (costheta * cos120 - sintheta * sin120); 763 } 764 return; 765 766 case 0x100: // bit outputs, 3-wire. Dubious utility 767 if (out_dir){ ph = (ph & 070) >> 3 | (ph & 007) << 3;} 768 if (ph & 040) {A_on = 1;} 769 else if (ph & 020) {B_on = 1;} 770 else if (ph & 010) {C_on = 1;} 771 else {A_on = 0 ; B_on = 0 ; C_on = 0;} 772 if (ph & 004) {A_on = 0;} 773 else if (ph & 002) {B_on = 0;} 774 else if (ph & 001) {C_on = 0;} 775 else {A_on = 0 ; B_on = 0 ; C_on = 0;} 776 return; 777 778 case 0x200: // 6-wire modes 779 if (trap){ 780 if (out_dir){ ph = (ph & 070) >> 3 | (ph & 007) << 3;} 781 if (ph & 040) { A_high = out_abs; A_low = 0;} 782 else if (ph & 004) {A_high = 0; A_low = out_abs;} 783 else {A_high = 0; A_low = 0;} 784 785 if (ph & 020) {B_high = out_abs; B_low = 0;} 786 else if (ph & 002) {B_high = 0; B_low = out_abs;} 787 else {B_high = 0; B_low = 0;} 788 789 if (ph & 010) {C_high = out_abs; C_low = 0;} 790 else if (ph & 001) {C_high = 0; C_low = out_abs;} 791 else {C_high = 0; C_low=0;} 792 } 793 else 794 { 795 sintheta = sin(phase_angle * pi2); 796 costheta = cos(phase_angle * pi2); 797 if (costheta >=0){ 798 A_high = out_abs * costheta; A_low = 0;} 799 else { 800 A_high = 0; A_low = -out_abs * costheta;} 801 802 if ((costheta * cos120 + sintheta * sin120) >= 0){ 803 B_high = out_abs * (costheta * cos120 + sintheta * sin120); 804 B_low = 0;} 805 else { 806 B_high = 0; 807 B_low = -out_abs * (costheta * cos120 + sintheta * sin120);} 808 809 if ((costheta * cos120 - sintheta * sin120) >= 0) { 810 C_high = out_abs * (costheta * cos120 - sintheta * sin120); 811 C_low = 0;} 812 else { 813 C_high = 0; 814 C_low = -out_abs * (costheta * cos120 - sintheta * sin120);} 815 } 816 return; 817 case 0x300: // 6-wire bit mode 818 if (out_dir) { 819 A_high_on = (ph & 004) ; A_low_on = (ph & 040); 820 B_high_on = (ph & 002) ; B_low_on = (ph & 020); 821 C_high_on = (ph & 001) ; C_low_on = (ph & 010); 822 823 } 824 else 825 { 826 A_high_on = (ph & 040) ; A_low_on = (ph & 004); 827 B_high_on = (ph & 020) ; B_low_on = (ph & 002); 828 C_high_on = (ph & 010) ; C_low_on = (ph & 001); 829 } 830 return; 831 832 case 0x400: // Hall Output 833 for (i = 0; P[(output_pattern << 3) + i] != ph && i < 8 ; i++) {} 834 hall1_out = (i & 0x04); 835 hall2_out = (i & 0x02); 836 hall3_out = (i & 0x01); 837 return; 838 839 case 0x800: // Fanuc Red Cap style Gray-Code emulation 840 for (i = 0; (gray_a[i] + 0.0625) < rotor_angle && i < 16 ; i++) {} 841 C1_out = (gray_b[i] & 001); 842 C2_out = (gray_b[i] & 002); 843 C4_out = (gray_b[i] & 004); 844 C8_out = (gray_b[i] & 010); 845 return; 846 847 case 0x500: 848 rtapi_print_msg(RTAPI_MSG_ERR, "Combinations of Hall Pattern and Bit" 849 " outputs are not supported. Defaulting to Hall"); 850 out_type = 0x400; 851 return; 852 case 0x600: 853 rtapi_print_msg(RTAPI_MSG_ERR, "6-Wire Hall patterns outputs are " 854 "not supported. Defaulting to 3-wire"); 855 out_type = 0x400; 856 return; 857 case 0x700: 858 rtapi_print_msg(RTAPI_MSG_ERR, "6-wire combinations of Hall and Bit" 859 " outputs can't be supported. Defaulting to 3-wire" 860 " Hall pattern"); 861 out_type = 0x400; 862 return; 863 case 0x900: 864 case 0xB00: 865 case 0xA00: 866 rtapi_print_msg(RTAPI_MSG_ERR, "Combinations of bit level and " 867 " Gray Code outputs are not supported. Defaulting" 868 " to Gray Code"); 869 out_type = 0x800; 870 return; 871 case 0xC00: 872 rtapi_print_msg(RTAPI_MSG_ERR, "Hall Sensor and Gray-code outputs" 873 " can not be combined. defaulting to Hall"); 874 out_type = 0x400; 875 return; 876 default: 877 rtapi_print_msg(RTAPI_MSG_ERR, "Unsupported output type (%X) in bldc" 878 , out_type); 879 in_type = -2; 880 return; 881 } 882 } 883 884 EXTRA_SETUP(){ 885 int i; 886 char c; 887 for (i = 0; cfg[extra_arg][i] != 0 && i < NUM_TAG ; i++){ 888 c = cfg[extra_arg][i]; 889 if (c == 'h') personality |= 0x01; 890 if (c == 'a') personality |= 0x02; 891 if (c == 'q') personality |= 0x04; 892 if (c == 'i') personality |= 0x08; 893 if (c == 'f') personality |= 0x10; 894 if (c == 'B') personality |= 0x0100; 895 if (c == '6') personality |= 0x0200; 896 if (c == 'H') personality |= 0x0400; 897 if (c == 'F') personality |= 0x0800; 898 if (c == 'T') personality |= 0x8000; 899 } 900 return 0; 901 } 902 903 int get_count(void){ 904 int i; 905 906 for (i=0; cfg[i] != NULL && i < MAX_CHAN; i++){} 907 if (i == 0){ 908 rtapi_print_msg(RTAPI_MSG_ERR, "The bldc component needs at least one " 909 "feedback type tag.\nValid tags are h, a, q, i, b, 6, n\n" ); 910 return 0; 911 } 912 return i; 913 }