Index: trunk/MagicSoft/Cosy/aposs/Magic.m =================================================================== --- trunk/MagicSoft/Cosy/aposs/Magic.m (revision 738) +++ trunk/MagicSoft/Cosy/aposs/Magic.m (revision 808) @@ -3,5 +3,5 @@ /*-------------------------------------------------------------------------*/ -DIM errlist[9] +DIM errlist[9] /* idx 1=number of valid entries */ /* ----------------------------------------------------------------------- */ @@ -10,7 +10,22 @@ /* */ kVERSION = 0 /* */ -kSUBVERSION = 50 /* */ +kSUBVERSION = 62 /* */ /* */ /* HISTORY: */ +/* */ +/* * V0.62: */ +/* - changed handling of 0x2000/1/2 added /3 */ +/* */ +/* * V0.61: */ +/* - corrected problems with the error handling */ +/* */ +/* * V0.60: */ +/* - introduced syncronisation */ +/* */ +/* * V0.52: */ +/* - changed the handling of the endswitch error (unknown switch) */ +/* */ +/* * V0.51: */ +/* - made errlist working */ /* */ /* * V0.50: */ @@ -65,6 +80,7 @@ /* 0x1800 x rw Enable PDO1 (Axe Status, Position) */ /* 0x2000 0 rw Maximum positioning error */ -/* 1 rw Negative Software Endswitch */ -/* 2 rw Positive Software Endswitch */ +/* 1 rw Negative Software Endswitch Value (Set=Enable) */ +/* 2 rw Positive Software Endswitch Value (Set=Enable) */ +/* 3 rw Enable/Disable Software Endswitch */ /* 0x2002 x rw Velocity */ /* 0x2003 0 wo Acceleration */ @@ -76,8 +92,9 @@ /* 0x3006 0 wo Velocity Mode 'strt', 'stop' */ /* 1 wo VelMode Velocity */ +/* 0x3007 x wo Syncronisation 'sync' */ /* 0x3008 x wo Nowait 'on', 'off' */ /* 0x6000 x rw Rotation Direction */ /* 0x6002 x rw Velocity Resolution */ -/* 0x6003 0 wo Define present position as origin */ +/* 0x6003 0 wo Define present position as origin ('set') */ /* 1 wo Define new origin (0=delete) */ /* 2 rw Home Offset */ @@ -99,4 +116,6 @@ SET ENCODERTYPE 0 /* Incremental Encoder */ SET ENCODER 500 /* Encoder has 500 Ticks */ +SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ +SET MENCODER 500 /* Encoder has 500 Ticks (Master) */ SET VELMAX 3600 /* Motor: Maximum rounds per minute */ SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */ @@ -112,4 +131,15 @@ /*----------------*/ +/* syncronisation */ +/*----------------*/ +SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ +SET MENCODER 500 /* Encoder has 500 Ticks (Master) */ +SET SYNCFACTM 1 /* Master Sync Velocity factor */ +SET SYNCFACTS 1 /* Slave Sync Velocity factor */ +SET SYNCPOSOFFS 0 /* Sync Position offset between M/S */ +SET SYNCACCURACY 50 /* When to set Accuracy Flag */ +SET REVERS 0 /* How to handle reversation of vel */ + +/*----------------*/ /* Inputs */ /*----------------*/ @@ -181,5 +211,5 @@ /*-------------------------------------------------------------------------*/ /* ON CANMSG GOSUB PROC_CANMSG */ -i = 8 +i = 9 while (i) do errlist[i] = 0 @@ -197,4 +227,5 @@ PRINT "Starting Mainloop..." + MAINLOOP: rc = CANIN sdorx 0 0 canhi canlo @@ -225,8 +256,8 @@ */ if (idx == 0x1003 and subidx == 0 and sdoval == 0) then - i = 0 - while (i<9) do + i = 9 + while (i) do errlist[i] = 0 - i = i + 1 + i = i - 1 endwhile elseif (idx == 0x1010 and sdoval == 's'<<24|'a'<<16|'v'<<8|'e') then @@ -244,17 +275,12 @@ SET POSERR sdoval elseif (subidx == 1) then - if ((sdoval>>30)&1) then - SET NEGLIMIT sdoval & 0x3fffffff - SET SWNEGLIMACT 1 - else - SET SWNEGLIMACT 0 - endif + SET NEGLIMIT sdoval + SET SWNEGLIMACT 1 elseif (subidx == 2) then - if ((sdoval>>31)&1) then - SET POSLIMIT sdoval & 0x3fffffff - SET SWPOSLIMACT 1 - else - SET SWPOSLIMACT 0 - endif + SET POSLIMIT sdoval + SET SWPOSLIMACT 1 + elseif (subidx == 3) then + SET SWNEGLIMACT sdoval&1 + SET SWPOSLIMACT (sdoval>>1)&1 endif elseif (idx == 0x2002) then @@ -306,4 +332,6 @@ CVEL sdoval endif + elseif (idx == 0x3007 and sdoval == 's'<<24|'y'<<16|'n'<<8|'c') then + SYNCP elseif (idx == 0x3008) then if (sdoval == 'o'<<24|'n'<<16) then @@ -325,10 +353,8 @@ SET POSDRCT 1 endif -/* elseif (idx == 0x6001) then - SET ENCODER sdoval*/ elseif (idx == 0x6002) then SET VELRES sdoval elseif (idx == 0x6003) then - if (subidx == 0) then + if (subidx == 0 and sdoval == 's'<<24|'e'<<16|'t'<<8) then DEF ORIGIN elseif (subidx == 1) then @@ -383,5 +409,5 @@ if (idx == 0x1003) then if (subidx >=0 and subidx<=9) then - sdoval = errlist[subidx] + sdoval = errlist[subidx-1] endif elseif (idx == 0x1004) then @@ -422,4 +448,6 @@ elseif (subidx == 2) then sdoval = GET POSLIMIT | (GET SWPOSLIMACT) << 31 + elseif (subidx == 3) then + sdoval = (GET SWNEGLIMACT) | ((GET SWPOSLIMACT)<<1) endif elseif (idx == 0x2001) then @@ -514,47 +542,76 @@ SUBPROG PROC_ERROR MOTOR STOP - errinf = 0 /* Tell the bus that an error occured */ CANOUT pdo2 0 0 - i = errlist[0] + 1 - while (i>1) do + i = errlist[1] + 1 /* Fill status of array */ + while (i>2) do /* shift errors by one */ errlist[i] = errlist[i-1] i = i - 1 - endwhile - errlist[1] = ERRNO - if (errlist[0]<8) then - errlist[0] = errlist[0] + 1 - endif - + endwhile /* set new errornumber */ + errlist[2] = ERRNO + if (errlist[1]<8) then /* write new size if enhanced */ + errlist[1] = errlist[1] + 1 + endif + + errinf = 0 + /* check if the error is repairable and repair */ - if (errlist[1]==6) then + if (errlist[2]==6) then PRINT "No home forced!" ERRCLR - elseif (errlist[1]==8) then - PRINT "Schleppabstand überschritten" + elseif (errlist[2]==8) then + PRINT "Control deviation overflow." ERRCLR errinf = 0xaffe - elseif (errlist[1]==9) then + elseif (errlist[2]==9) then PRINT "Did'n find zero index." ERRCLR - elseif (errlist[1]==25) then - lsw = -(GET I_POSLIMITSW) - if (IN lsw == 0) then + elseif (errlist[2]==11) then + poslsw = GET POSLIMIT + neglsw = GET NEGLIMIT + if ((GET SWNEGLIMACT) and APOS<=neglsw) then + PRINT "Negative software endswitch (", neglsw, ") activated at position ", APOS + SET SWNEGLIMACT 0 + ERRCLR + CVEL (vres%100) + ACC (10*vres%100) + DEC (10*vres%100) + POSA neglsw + 100 + SET SWNEGLIMACT 1 + errinf = -1 + elseif ((GET SWPOSLIMACT) and APOS>=poslsw) then + PRINT "Positive software endswitch (", poslsw, ") activated at position ", APOS + SET SWPOSLIMACT 0 + ERRCLR + CVEL (vres%100) /* 1% */ + ACC (10*vres%100) /* 10% */ + DEC (10*vres%100) /* 10% */ + POSA poslsw - 100 + SET SWPOSLIMACT 1 + errinf = 1 + else + PRINT "Software endswitch activated - command skipped. Pos: ", APOS + ERRCLR + endif + elseif (errlist[2]==25) then + /* FIXME: To handle this correct you must make sure, + that the endswitch numbers are negative */ + poslsw = -(GET I_POSLIMITSW) + neglsw = -(GET I_NEGLIMITSW) + if (IN poslsw == 0) then PRINT "Positive endswitch activated at position ", APOS SET I_POSLIMITSW 0 ERRCLR - CVEL (vres%100) /* 1% */ - ACC (10*vres%100) - DEC (10*vres%100) + CVEL (vres%100) /* 1% */ + ACC (10*vres%100) /* 10% */ + DEC (10*vres%100) /* 10% */ CSTART - WHILE (IN lsw == 0) DO ENDWHILE + WHILE (IN poslsw == 0) DO ENDWHILE CSTOP - SET I_POSLIMITSW -lsw + SET I_POSLIMITSW -poslsw errinf = 1 - endif - lsw = -(GET I_NEGLIMITSW) - if (IN lsw == 0) then + elseif (IN neglsw == 0) then PRINT "Negative endswitch activated at position ", APOS SET I_NEGLIMITSW 0 @@ -564,21 +621,19 @@ ACC (10*vres%100) /* 10% */ DEC (10*vres%100) /* 10% */ - OUT 1 1 - MOTOR ON CSTART - WHILE (IN lsw == 0) DO ENDWHILE + WHILE (IN poslsw == 0) DO ENDWHILE CSTOP - SET I_NEGLIMITSW -lsw + SET I_NEGLIMITSW -poslsw errinf = -1 endif - elseif (errlist[1]==84) then + elseif (errlist[2]==84) then PRINT "Too many (>12) ON TIME interrupts." ERRCLR ELSE - PRINT "Error Function Called: ERRNO=", errlist[1] + PRINT "Error Function Called: ERRNO=", errlist[2] endif /* tell the bus what exactly happened */ - CANOUT pdo2 errlist[1] errinf + CANOUT pdo2 errlist[2] errinf RETURN