/* ----------------------------------------------------------------------- */ /* */ /* Version: */ /* */ kVERSION = 0 /* */ kSUBVERSION = 6 /* */ /* */ /* HISTORY: */ /* */ /* * V0.6 */ /* - do not reset the velocity if set already */ /* - set acceleration to 20% */ /* */ /* * V0.5: */ /* - changes 'DKC Ready' signal to IN1 for all MACS */ /* - control brake only when cannr==3 */ /* */ /* * V0.4: */ /* - restart MACS with PRGPAR 1 when not in manual mode */ /* - replaced label reset by a subprg */ /* */ /* * V0.3: */ /* - added support for the elevation axis brake */ /* - enabled check for operation mode (remote control/pc) */ /* - added 'reset' label */ /* - moved syncv/cstart to setting rf */ /* */ /* * V0.2: */ /* - fixed a bug, shaking the telescope switching on again after an */ /* emergency stop */ /* */ /* * 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 = 10*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, Acc=", defacc /*----------------*/ /* 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 /*-------------------------------------------------------------------------*/ /* Error routine */ /*-------------------------------------------------------------------------*/ on error gosub suberror kIoModule = 4*256 /*-------------------------------------------------------------------------*/ /* mainloop rotation mode but stand still */ /*-------------------------------------------------------------------------*/ brake = 0 RF = 0 velo = 0 gosub reset 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 (ready==0) and (RF==1) then print "DKC not ready, but RF set... setting RF=AH=0!" gosub reset goto mainloop elseif mode==0 then print "Control not in manual mode!" gosub reset SET PRGPAR 1 exit elseif fuse==0 then print "Motor-Power Fuse not OK!" gosub reset goto mainloop elseif vltg==0 then print "Overvoltage control broken!" gosub reset goto mainloop elseif emcy==0 then print "Please release Emergency Stop!" gosub reset 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 motor off waitt 100 out 1 1 out 2 1 RF = 1 waitt 100 if (brake==0 and get cannr==3) then out (kIoModule+1) 1 brake = 1 waitt 1000 endif motor on if (get cannr==2) then syncv print "Synchronizing speed..." else cstart print "Starting revolution mode..." waitt 500 endif elseif (ready==0) or (RF==0) then goto mainloop endif /* if (get cannr==2) then print apos, " ", mapos, " ", avel," ", mavel waitt 500 goto mainloop endif */ forward = in 2 backward = in 3 if (forward==1) and (backward==0) and (velo!=manvel) then cvel manvel velo = manvel elseif (forward==0) and (backward==1) and (velo!=-manvel) then cvel -manvel velo = -manvel elseif (forward==backward) and (velo!=0) then cvel 0 velo = 0 endif goto mainloop SUBMAINPROG subprog reset out 1 0 out 2 0 RF = 0 motor off velo = 0 waitt 1000 if (brake==1 and get cannr==3) then waitt 3000 /* wait 3s for DKC to stop the motor */ out (kIoModule+1) 0 /* brake the brake */ waitt 1000 endif return subprog suberror out 1 0 out 2 0 RF = 0 waitt 100 velo = 0 if (brake==1 and get cannr==3) then waitt 5000 out (kIoModule+1) 0 waitt 500 endif 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