Changeset 808 for trunk/MagicSoft/Cosy/aposs
- Timestamp:
- 05/25/01 16:51:24 (24 years ago)
- File:
-
- 1 edited
-
trunk/MagicSoft/Cosy/aposs/Magic.m (modified) (16 diffs)
Legend:
- Unmodified
- Added
- Removed
-
trunk/MagicSoft/Cosy/aposs/Magic.m
r738 r808 3 3 /*-------------------------------------------------------------------------*/ 4 4 5 DIM errlist[9] 5 DIM errlist[9] /* idx 1=number of valid entries */ 6 6 7 7 /* ----------------------------------------------------------------------- */ … … 10 10 /* */ 11 11 kVERSION = 0 /* */ 12 kSUBVERSION = 50/* */12 kSUBVERSION = 62 /* */ 13 13 /* */ 14 14 /* HISTORY: */ 15 /* */ 16 /* * V0.62: */ 17 /* - changed handling of 0x2000/1/2 added /3 */ 18 /* */ 19 /* * V0.61: */ 20 /* - corrected problems with the error handling */ 21 /* */ 22 /* * V0.60: */ 23 /* - introduced syncronisation */ 24 /* */ 25 /* * V0.52: */ 26 /* - changed the handling of the endswitch error (unknown switch) */ 27 /* */ 28 /* * V0.51: */ 29 /* - made errlist working */ 15 30 /* */ 16 31 /* * V0.50: */ … … 65 80 /* 0x1800 x rw Enable PDO1 (Axe Status, Position) */ 66 81 /* 0x2000 0 rw Maximum positioning error */ 67 /* 1 rw Negative Software Endswitch */ 68 /* 2 rw Positive Software Endswitch */ 82 /* 1 rw Negative Software Endswitch Value (Set=Enable) */ 83 /* 2 rw Positive Software Endswitch Value (Set=Enable) */ 84 /* 3 rw Enable/Disable Software Endswitch */ 69 85 /* 0x2002 x rw Velocity */ 70 86 /* 0x2003 0 wo Acceleration */ … … 76 92 /* 0x3006 0 wo Velocity Mode 'strt', 'stop' */ 77 93 /* 1 wo VelMode Velocity */ 94 /* 0x3007 x wo Syncronisation 'sync' */ 78 95 /* 0x3008 x wo Nowait 'on', 'off' */ 79 96 /* 0x6000 x rw Rotation Direction */ 80 97 /* 0x6002 x rw Velocity Resolution */ 81 /* 0x6003 0 wo Define present position as origin 98 /* 0x6003 0 wo Define present position as origin ('set') */ 82 99 /* 1 wo Define new origin (0=delete) */ 83 100 /* 2 rw Home Offset */ … … 99 116 SET ENCODERTYPE 0 /* Incremental Encoder */ 100 117 SET ENCODER 500 /* Encoder has 500 Ticks */ 118 SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ 119 SET MENCODER 500 /* Encoder has 500 Ticks (Master) */ 101 120 SET VELMAX 3600 /* Motor: Maximum rounds per minute */ 102 121 SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */ … … 112 131 113 132 /*----------------*/ 133 /* syncronisation */ 134 /*----------------*/ 135 SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ 136 SET MENCODER 500 /* Encoder has 500 Ticks (Master) */ 137 SET SYNCFACTM 1 /* Master Sync Velocity factor */ 138 SET SYNCFACTS 1 /* Slave Sync Velocity factor */ 139 SET SYNCPOSOFFS 0 /* Sync Position offset between M/S */ 140 SET SYNCACCURACY 50 /* When to set Accuracy Flag */ 141 SET REVERS 0 /* How to handle reversation of vel */ 142 143 /*----------------*/ 114 144 /* Inputs */ 115 145 /*----------------*/ … … 181 211 /*-------------------------------------------------------------------------*/ 182 212 /* ON CANMSG GOSUB PROC_CANMSG */ 183 i = 8213 i = 9 184 214 while (i) do 185 215 errlist[i] = 0 … … 197 227 198 228 PRINT "Starting Mainloop..." 229 199 230 MAINLOOP: 200 231 rc = CANIN sdorx 0 0 canhi canlo … … 225 256 */ 226 257 if (idx == 0x1003 and subidx == 0 and sdoval == 0) then 227 i = 0228 while (i <9) do258 i = 9 259 while (i) do 229 260 errlist[i] = 0 230 i = i +1261 i = i - 1 231 262 endwhile 232 263 elseif (idx == 0x1010 and sdoval == 's'<<24|'a'<<16|'v'<<8|'e') then … … 244 275 SET POSERR sdoval 245 276 elseif (subidx == 1) then 246 if ((sdoval>>30)&1) then 247 SET NEGLIMIT sdoval & 0x3fffffff 248 SET SWNEGLIMACT 1 249 else 250 SET SWNEGLIMACT 0 251 endif 277 SET NEGLIMIT sdoval 278 SET SWNEGLIMACT 1 252 279 elseif (subidx == 2) then 253 if ((sdoval>>31)&1) then 254 SET POSLIMIT sdoval & 0x3fffffff 255 SET SWPOSLIMACT 1 256 else 257 SET SWPOSLIMACT 0 258 endif 280 SET POSLIMIT sdoval 281 SET SWPOSLIMACT 1 282 elseif (subidx == 3) then 283 SET SWNEGLIMACT sdoval&1 284 SET SWPOSLIMACT (sdoval>>1)&1 259 285 endif 260 286 elseif (idx == 0x2002) then … … 306 332 CVEL sdoval 307 333 endif 334 elseif (idx == 0x3007 and sdoval == 's'<<24|'y'<<16|'n'<<8|'c') then 335 SYNCP 308 336 elseif (idx == 0x3008) then 309 337 if (sdoval == 'o'<<24|'n'<<16) then … … 325 353 SET POSDRCT 1 326 354 endif 327 /* elseif (idx == 0x6001) then328 SET ENCODER sdoval*/329 355 elseif (idx == 0x6002) then 330 356 SET VELRES sdoval 331 357 elseif (idx == 0x6003) then 332 if (subidx == 0 ) then358 if (subidx == 0 and sdoval == 's'<<24|'e'<<16|'t'<<8) then 333 359 DEF ORIGIN 334 360 elseif (subidx == 1) then … … 383 409 if (idx == 0x1003) then 384 410 if (subidx >=0 and subidx<=9) then 385 sdoval = errlist[subidx ]411 sdoval = errlist[subidx-1] 386 412 endif 387 413 elseif (idx == 0x1004) then … … 422 448 elseif (subidx == 2) then 423 449 sdoval = GET POSLIMIT | (GET SWPOSLIMACT) << 31 450 elseif (subidx == 3) then 451 sdoval = (GET SWNEGLIMACT) | ((GET SWPOSLIMACT)<<1) 424 452 endif 425 453 elseif (idx == 0x2001) then … … 514 542 SUBPROG PROC_ERROR 515 543 MOTOR STOP 516 errinf = 0517 544 518 545 /* Tell the bus that an error occured */ 519 546 CANOUT pdo2 0 0 520 547 521 i = errlist[ 0] + 1522 while (i> 1) do548 i = errlist[1] + 1 /* Fill status of array */ 549 while (i>2) do /* shift errors by one */ 523 550 errlist[i] = errlist[i-1] 524 551 i = i - 1 525 endwhile 526 errlist[1] = ERRNO 527 if (errlist[0]<8) then 528 errlist[0] = errlist[0] + 1 529 endif 530 552 endwhile /* set new errornumber */ 553 errlist[2] = ERRNO 554 if (errlist[1]<8) then /* write new size if enhanced */ 555 errlist[1] = errlist[1] + 1 556 endif 557 558 errinf = 0 559 531 560 /* check if the error is repairable and repair */ 532 if (errlist[ 1]==6) then561 if (errlist[2]==6) then 533 562 PRINT "No home forced!" 534 563 ERRCLR 535 elseif (errlist[ 1]==8) then536 PRINT " Schleppabstand überschritten"564 elseif (errlist[2]==8) then 565 PRINT "Control deviation overflow." 537 566 ERRCLR 538 567 errinf = 0xaffe 539 elseif (errlist[ 1]==9) then568 elseif (errlist[2]==9) then 540 569 PRINT "Did'n find zero index." 541 570 ERRCLR 542 elseif (errlist[1]==25) then 543 lsw = -(GET I_POSLIMITSW) 544 if (IN lsw == 0) then 571 elseif (errlist[2]==11) then 572 poslsw = GET POSLIMIT 573 neglsw = GET NEGLIMIT 574 if ((GET SWNEGLIMACT) and APOS<=neglsw) then 575 PRINT "Negative software endswitch (", neglsw, ") activated at position ", APOS 576 SET SWNEGLIMACT 0 577 ERRCLR 578 CVEL (vres%100) 579 ACC (10*vres%100) 580 DEC (10*vres%100) 581 POSA neglsw + 100 582 SET SWNEGLIMACT 1 583 errinf = -1 584 elseif ((GET SWPOSLIMACT) and APOS>=poslsw) then 585 PRINT "Positive software endswitch (", poslsw, ") activated at position ", APOS 586 SET SWPOSLIMACT 0 587 ERRCLR 588 CVEL (vres%100) /* 1% */ 589 ACC (10*vres%100) /* 10% */ 590 DEC (10*vres%100) /* 10% */ 591 POSA poslsw - 100 592 SET SWPOSLIMACT 1 593 errinf = 1 594 else 595 PRINT "Software endswitch activated - command skipped. Pos: ", APOS 596 ERRCLR 597 endif 598 elseif (errlist[2]==25) then 599 /* FIXME: To handle this correct you must make sure, 600 that the endswitch numbers are negative */ 601 poslsw = -(GET I_POSLIMITSW) 602 neglsw = -(GET I_NEGLIMITSW) 603 if (IN poslsw == 0) then 545 604 PRINT "Positive endswitch activated at position ", APOS 546 605 SET I_POSLIMITSW 0 547 606 ERRCLR 548 CVEL (vres%100) /*1% */549 ACC (10*vres%100) 550 DEC (10*vres%100) 607 CVEL (vres%100) /* 1% */ 608 ACC (10*vres%100) /* 10% */ 609 DEC (10*vres%100) /* 10% */ 551 610 CSTART 552 WHILE (IN lsw == 0) DO ENDWHILE611 WHILE (IN poslsw == 0) DO ENDWHILE 553 612 CSTOP 554 SET I_POSLIMITSW - lsw613 SET I_POSLIMITSW -poslsw 555 614 errinf = 1 556 endif 557 lsw = -(GET I_NEGLIMITSW) 558 if (IN lsw == 0) then 615 elseif (IN neglsw == 0) then 559 616 PRINT "Negative endswitch activated at position ", APOS 560 617 SET I_NEGLIMITSW 0 … … 564 621 ACC (10*vres%100) /* 10% */ 565 622 DEC (10*vres%100) /* 10% */ 566 OUT 1 1567 MOTOR ON568 623 CSTART 569 WHILE (IN lsw == 0) DO ENDWHILE624 WHILE (IN poslsw == 0) DO ENDWHILE 570 625 CSTOP 571 SET I_NEGLIMITSW - lsw626 SET I_NEGLIMITSW -poslsw 572 627 errinf = -1 573 628 endif 574 elseif (errlist[ 1]==84) then629 elseif (errlist[2]==84) then 575 630 PRINT "Too many (>12) ON TIME interrupts." 576 631 ERRCLR 577 632 ELSE 578 PRINT "Error Function Called: ERRNO=", errlist[ 1]633 PRINT "Error Function Called: ERRNO=", errlist[2] 579 634 endif 580 635 581 636 /* tell the bus what exactly happened */ 582 CANOUT pdo2 errlist[ 1] errinf637 CANOUT pdo2 errlist[2] errinf 583 638 RETURN 584 639
Note:
See TracChangeset
for help on using the changeset viewer.
