Index: /trunk/MagicSoft/Cosy/aposs/Manual.m =================================================================== --- /trunk/MagicSoft/Cosy/aposs/Manual.m (revision 1565) +++ /trunk/MagicSoft/Cosy/aposs/Manual.m (revision 1565) @@ -0,0 +1,306 @@ +/* ----------------------------------------------------------------------- */ +/* */ +/* Version: */ +/* */ +kVERSION = 0 /* */ +kSUBVERSION = 1 /* */ +/* */ +/* HISTORY: */ +/* */ +/* * V0.1: */ +/* - first implementation */ +/* */ +/* ----------------------------------------------------------------------- */ + +PRINT "Magic Manc (Manual Control) V", kVERSION, ".", kSUBVERSION /* */ + +/* +if (get cannr!=1) and (get cannr!=3) then + PRINT "Sorry, wrong MACS (CAN Id=", get cannr, ") only #1 and #3 allowed!" + exit +endif +*/ + +/*-------------------------------------------------------------------------*/ +/* section for global constants */ +/*-------------------------------------------------------------------------*/ +SET PRGPAR -1 /* Don't restart any Program on Exit */ + +SET ENCODERTYPE 0 /* Incremental Encoder */ +SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ + +SET ENDSWMOD 0 /* No End Switch */ +SET ERRCOND 2 /* Motor Stop, position control, no break */ +SET POSDRCT -1 /* rotation direction */ +SET POSFACT_Z 1 /* 1 user unit (be) = POSFACT_Z/POSFACT_N qc */ +SET POSFACT_N 1 /* */ + +SET HOME_FORCE 0 /* Don't force Home positioning on mainloopup */ +SET HOME_OFFSET 0 /* Offset between index and home position */ +SET HOMETYPE 0 /* drive to home, reverse, go to next index */ + +/*----------------*/ +/* syncronisation */ +/*----------------*/ +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 */ +/*----------------*/ +SET I_REFSWITCH 0 /* Reference Switch */ +SET I_POSLIMITSW 0 /* Pos Limit Switch */ +SET I_NEGLIMITSW 0 /* Neg Limit Switch */ +SET I_BREAK 0 /* Input which brakes a running program */ +SET I_CONTINUE 0 /* Input to continue a broken program */ +SET I_ERRCLR 0 /* Input to clear error */ + +/*----------------*/ +/* Outputs */ +/*----------------*/ +SET O_AXMOVE 0 /* Motor control is working */ +SET O_BRAKE 0 /* Brake */ +SET O_ERROR 0 /* error occured */ + +/*----------------*/ +/* Unit param. */ +/*----------------*/ +SET RAMPTYPE 1 /* Ramp Type: 0=Trapez, 1=Sinus */ +SET ENCODER 1500 /* Encoder has 1500 Ticks */ +SET MENCODER 1500 /* Encoder has 500 Ticks (Master) */ +SET VELMAX 3000 /* Motor: Maximum revolutions per minute */ +SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */ +SET RAMPMIN 10000 /* Shortest Ramp 10s */ + +/*----------------*/ +/* Dflt vel & acc */ +/*----------------*/ + +/* Prop=100, Div=300, Int=800 */ +if (get cannr==1) then +SET KPROP 100 +SET KDER 300 +SET KINT 1000 +elseif (get cannr==2) then +SET KPROP 100 +SET KDER 200 +SET KINT 150 +else +SET KPROP 350 +SET KDER 50 +SET KINT 350 +endif + +vres = (GET ENCODER)*(GET VELMAX) /* ticks/R * R/M = ticks/min */ +SET VELRES vres /* Set velocity units */ + +/*----------------*/ +/* Manual control */ +/*----------------*/ +SET RAMPTYPE 1 /* Ramp: 0=linear, 1=sinus */ +defacc = 60*vres%100 + +SET DFLTACC defacc /* Default acceleratio: [%] */ +ACC defacc +DEC defacc*2 + /* Velocity which is reached in + a time given by RAMPMIN */ +SET DFLTVEL (1*vres%100) /* Default velocity [%] */ + +manvel = (4*vres%100) /* 150 U/min */ /* Max speed in man mode: [%] */ + +print "Vel Res (vel max): ", GET VELRES, " Encoder Ticks/min" +print "V_man: ", manvel, " Encoder Ticks/min" + +/*----------------*/ +/* Software range */ +/*----------------*/ +SET SWPOSLIMACT 0 /* positive software limit switch inactive */ +SET SWNEGLIMACT 0 /* negative software limit switch inactive */ +SET POSLIMIT 0 /* positive software limit (qc) */ +SET NEGLIMIT 0 /* negative software limit (qc) */ + +/*-------------------------------------------------------------------------*/ +/* const section for constant velues */ +/*-------------------------------------------------------------------------*/ +kTRUE = 1 +kFALSE = 0 + +/*-------------------------------------------------------------------------*/ +/* Make sure 'Reglerfreigabe' not set */ +/*-------------------------------------------------------------------------*/ +RF = 0 +out 1 0 +out 2 0 + +/*-------------------------------------------------------------------------*/ +/* Error routine */ +/*-------------------------------------------------------------------------*/ +on error gosub suberror + +kIoModule = 4*256 + +/*-------------------------------------------------------------------------*/ +/* mainloop rotation mode but stand still */ +/*-------------------------------------------------------------------------*/ +if (get cannr==1) or (get cannr==3) then + cvel 0 + cstart + print "Starting..." +else + print "Synchronizing..." + syncv +endif + +mainloop: + fuse = in (kIoModule+1) + emcy = in (kIoModule+2) + vltg = in (kIoModule+3) + mode = in (kIoModule+4) + if (get cannr==1) or (get cannr==2) then + ready = in 1 + elseif get cannr==3 then + ready = in (kIoModule+5) + endif + + if fuse==0 then + print "Motor-Power Fuse not OK!" + waitt 500 + goto mainloop + elseif emcy==0 then + print "Please release Emergency Stop!" + waitt 500 + goto mainloop + elseif vltg==0 then + print "Overvoltage control broken!" + waitt 500 + goto mainloop +/* + elseif mode==1 then + print "Control not in manual mode!" + out 1 0 + out 2 0 + waitt 500 + exit +*/ + elseif (ready==0) and (RF==1) then + print "DKC not ready... setting RF=AH=0!" + out 1 0 + out 2 0 + cvel 0 + RF = 0 + waitt 1000 + goto mainloop + elseif (ready==1) and (RF==0) then + print "DKC powered, RF=0... setting RF=AH=1!" + /* + * After switching on power wait at least 300ms until + * control changed state 'bb' to 'ab' + */ + cvel 0 + waitt 300 + out 1 0 + out 2 0 + waitt 100 + def origin + out 1 1 + out 2 1 + RF = 1 + waitt 100 + elseif (ready==0) or (RF==0) then + goto mainloop + endif +/* + if (get cannr==2) then + print "M:", mapos, " ", mavel, " S: ", apos, " ", avel, " C:", cpos + waitt 500 + goto mainloop + endif +*/ + forward = in 2 + backward = in 3 + + if (forward==1) and (backward==0) then + cvel manvel + elseif (forward==0) and (backward==1) then + cvel -manvel + else + cvel 0 + endif +goto mainloop + +SUBMAINPROG + subprog suberror + out 1 0 + out 2 0 + RF = 0 + waitt 100 + + print "Error #", errno + + if errno==3 then /* axis not existing: shoud never happen */ + exit + elseif errno==5 then /* error remaining: tried moving while error not cleared */ + /* !!! */ + exit + elseif errno==6 then /* home not first command: shoud never happen */ + exit + elseif errno==8 then /* control deviation too large */ + /* + * + */ + elseif errno==9 then /* index not found: shoud never happen */ + exit + elseif errno==10 then /* unknown command: shoud never happen */ + exit + elseif errno==11 then /* software endswitch reached */ + /* + * + */ + elseif errno==12 then /* wrong paremeter number: shoud never happen */ + exit + elseif errno==14 then /* too many LOOP commands: shoud never happen */ + exit + elseif errno==16 then /* parameter in EEPROM broken */ + exit + elseif errno==17 then /* programs in EEPROM broken */ + exit + elseif errno==18 then /* RESET by CPU: Reason could be power-problems */ + exit + elseif errno==19 then /* User break */ + exit + elseif errno==25 then /* hardware Endswitch reached */ + /* + * + */ + elseif errno==51 then /* too many gosub: shoud never happen */ + exit + elseif errno==52 then /* too many return: shoud never happen */ + exit + elseif errno==62 then /* error verifying EEPROM */ + exit + elseif errno==70 then /* error in DIM statement: should never happen */ + exit + elseif errno==72 then /* DIM limit reached: should never happen */ + exit + elseif errno==79 then /* Timeout waiting for an index */ + exit + elseif errno==84 then /* Too many ON TIME calls */ + exit + elseif errno==87 then /* storage for variables exhausted */ + exit + else + print "Unknown (internal) error #", errno + exit + endif + + exit + /*errclr*/ /* errclr includes 'motor on' which enables the motor controlling */ + + return +ENDPROG + +