/* ----------------------------------------------------------------------- */ /* */ /* 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