source: trunk/MagicSoft/Cosy/aposs/Manual.m@ 1807

Last change on this file since 1807 was 1807, checked in by tbretz, 22 years ago
*** empty log message ***
File size: 13.5 KB
Line 
1/* ----------------------------------------------------------------------- */
2/* */
3/* Version: */
4/* */
5kVERSION = 0 /* */
6kSUBVERSION = 6 /* */
7/* */
8/* HISTORY: */
9/* */
10/* * V0.6 */
11/* - do not reset the velocity if set already */
12/* - set acceleration to 20% */
13/* */
14/* * V0.5: */
15/* - changes 'DKC Ready' signal to IN1 for all MACS */
16/* - control brake only when cannr==3 */
17/* */
18/* * V0.4: */
19/* - restart MACS with PRGPAR 1 when not in manual mode */
20/* - replaced label reset by a subprg */
21/* */
22/* * V0.3: */
23/* - added support for the elevation axis brake */
24/* - enabled check for operation mode (remote control/pc) */
25/* - added 'reset' label */
26/* - moved syncv/cstart to setting rf */
27/* */
28/* * V0.2: */
29/* - fixed a bug, shaking the telescope switching on again after an */
30/* emergency stop */
31/* */
32/* * V0.1: */
33/* - first implementation */
34/* */
35/* ----------------------------------------------------------------------- */
36
37PRINT "Magic Manc (Manual Control) V", kVERSION, ".", kSUBVERSION /* */
38
39/*
40if (get cannr!=1) and (get cannr!=3) then
41 PRINT "Sorry, wrong MACS (CAN Id=", get cannr, ") only #1 and #3 allowed!"
42 exit
43endif
44*/
45
46/*-------------------------------------------------------------------------*/
47/* section for global constants */
48/*-------------------------------------------------------------------------*/
49SET PRGPAR -1 /* Don't restart any Program on Exit */
50
51SET ENCODERTYPE 0 /* Incremental Encoder */
52SET MENCODERTYPE 0 /* Incremental Encoder (Master) */
53
54SET ENDSWMOD 0 /* No End Switch */
55SET ERRCOND 2 /* Motor Stop, position control, no break */
56SET POSDRCT -1 /* rotation direction */
57SET POSFACT_Z 1 /* 1 user unit (be) = POSFACT_Z/POSFACT_N qc */
58SET POSFACT_N 1 /* */
59
60SET HOME_FORCE 0 /* Don't force Home positioning on mainloopup */
61SET HOME_OFFSET 0 /* Offset between index and home position */
62SET HOMETYPE 0 /* drive to home, reverse, go to next index */
63
64/*----------------*/
65/* syncronisation */
66/*----------------*/
67SET SYNCFACTM 1 /* Master Sync Velocity factor */
68SET SYNCFACTS 1 /* Slave Sync Velocity factor */
69SET SYNCPOSOFFS 0 /* Sync Position offset between M/S */
70SET SYNCACCURACY 50 /* When to set Accuracy Flag */
71SET REVERS 0 /* How to handle reversation of vel */
72
73/*----------------*/
74/* Inputs */
75/*----------------*/
76SET I_REFSWITCH 0 /* Reference Switch */
77SET I_POSLIMITSW 0 /* Pos Limit Switch */
78SET I_NEGLIMITSW 0 /* Neg Limit Switch */
79SET I_BREAK 0 /* Input which brakes a running program */
80SET I_CONTINUE 0 /* Input to continue a broken program */
81SET I_ERRCLR 0 /* Input to clear error */
82
83/*----------------*/
84/* Outputs */
85/*----------------*/
86SET O_AXMOVE 0 /* Motor control is working */
87SET O_BRAKE 0 /* Brake */
88SET O_ERROR 0 /* error occured */
89
90/*----------------*/
91/* Unit param. */
92/*----------------*/
93SET RAMPTYPE 1 /* Ramp Type: 0=Trapez, 1=Sinus */
94SET ENCODER 1500 /* Encoder has 1500 Ticks */
95SET MENCODER 1500 /* Encoder has 500 Ticks (Master) */
96SET VELMAX 3000 /* Motor: Maximum revolutions per minute */
97SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */
98SET RAMPMIN 10000 /* Shortest Ramp 10s */
99
100/*----------------*/
101/* Dflt vel & acc */
102/*----------------*/
103
104/* Prop=100, Div=300, Int=800 */
105if (get cannr==1) then
106SET KPROP 100
107SET KDER 300
108SET KINT 1000
109elseif (get cannr==2) then
110SET KPROP 100
111SET KDER 200
112SET KINT 150
113else
114SET KPROP 350
115SET KDER 50
116SET KINT 350
117endif
118
119vres = (GET ENCODER)*(GET VELMAX) /* ticks/R * R/M = ticks/min */
120SET VELRES vres /* Set velocity units */
121
122/*----------------*/
123/* Manual control */
124/*----------------*/
125SET RAMPTYPE 1 /* Ramp: 0=linear, 1=sinus */
126defacc = 10*vres%100
127
128SET DFLTACC defacc /* Default acceleratio: [%] */
129ACC defacc
130DEC defacc*2
131 /* Velocity which is reached in
132 a time given by RAMPMIN */
133SET DFLTVEL (1*vres%100) /* Default velocity [%] */
134
135manvel = (4*vres%100) /* 150 U/min */ /* Max speed in man mode: [%] */
136
137print "Vel Res (vel max): ", GET VELRES, " Encoder Ticks/min"
138print "V_man: ", manvel, " Encoder Ticks/min, Acc=", defacc
139
140/*----------------*/
141/* Software range */
142/*----------------*/
143SET SWPOSLIMACT 0 /* positive software limit switch inactive */
144SET SWNEGLIMACT 0 /* negative software limit switch inactive */
145SET POSLIMIT 0 /* positive software limit (qc) */
146SET NEGLIMIT 0 /* negative software limit (qc) */
147
148/*-------------------------------------------------------------------------*/
149/* const section for constant velues */
150/*-------------------------------------------------------------------------*/
151kTRUE = 1
152kFALSE = 0
153
154/*-------------------------------------------------------------------------*/
155/* Error routine */
156/*-------------------------------------------------------------------------*/
157on error gosub suberror
158
159kIoModule = 4*256
160
161/*-------------------------------------------------------------------------*/
162/* mainloop rotation mode but stand still */
163/*-------------------------------------------------------------------------*/
164
165brake = 0
166RF = 0
167velo = 0
168
169gosub reset
170
171mainloop:
172 fuse = in (kIoModule+1)
173 emcy = in (kIoModule+2)
174 vltg = in (kIoModule+3)
175 mode = in (kIoModule+4)
176 /*
177 if (get cannr==1) or (get cannr==2) then
178 */
179 ready = in 1
180 /*
181 elseif (get cannr==3) then
182 ready = in (kIoModule+5)
183 endif
184 */
185
186 if (ready==0) and (RF==1) then
187 print "DKC not ready, but RF set... setting RF=AH=0!"
188 gosub reset
189 goto mainloop
190 elseif mode==0 then
191 print "Control not in manual mode!"
192 gosub reset
193 SET PRGPAR 1
194 exit
195 elseif fuse==0 then
196 print "Motor-Power Fuse not OK!"
197 gosub reset
198 goto mainloop
199 elseif vltg==0 then
200 print "Overvoltage control broken!"
201 gosub reset
202 goto mainloop
203 elseif emcy==0 then
204 print "Please release Emergency Stop!"
205 gosub reset
206 goto mainloop
207 elseif (ready==1) and (RF==0) then
208 print "DKC powered, RF=0... setting RF=AH=1!"
209 /*
210 * After switching on power wait at least 300ms until
211 * control changed state 'bb' to 'ab'
212 */
213 cvel 0
214 waitt 300
215 out 1 0
216 out 2 0
217 motor off
218 waitt 100
219 out 1 1
220 out 2 1
221 RF = 1
222 waitt 100
223
224 if (brake==0 and get cannr==3) then
225 out (kIoModule+1) 1
226 brake = 1
227 waitt 1000
228 endif
229
230 motor on
231
232 if (get cannr==2) then
233 syncv
234 print "Synchronizing speed..."
235 else
236 cstart
237 print "Starting revolution mode..."
238 waitt 500
239 endif
240 elseif (ready==0) or (RF==0) then
241 goto mainloop
242 endif
243/*
244 if (get cannr==2) then
245 print apos, " ", mapos, " ", avel," ", mavel
246 waitt 500
247 goto mainloop
248 endif
249*/
250 forward = in 2
251 backward = in 3
252
253 if (forward==1) and (backward==0) and (velo!=manvel) then
254 cvel manvel
255 velo = manvel
256 elseif (forward==0) and (backward==1) and (velo!=-manvel) then
257 cvel -manvel
258 velo = -manvel
259 elseif (forward==backward) and (velo!=0) then
260 cvel 0
261 velo = 0
262 endif
263goto mainloop
264
265SUBMAINPROG
266 subprog reset
267 out 1 0
268 out 2 0
269 RF = 0
270 motor off
271 velo = 0
272 waitt 1000
273
274 if (brake==1 and get cannr==3) then
275 waitt 3000 /* wait 3s for DKC to stop the motor */
276 out (kIoModule+1) 0 /* brake the brake */
277 waitt 1000
278 endif
279 return
280
281 subprog suberror
282 out 1 0
283 out 2 0
284 RF = 0
285 waitt 100
286 velo = 0
287
288 if (brake==1 and get cannr==3) then
289 waitt 5000
290 out (kIoModule+1) 0
291 waitt 500
292 endif
293
294 print "Error #", errno
295
296 if errno==3 then /* axis not existing: shoud never happen */
297 exit
298 elseif errno==5 then /* error remaining: tried moving while error not cleared */
299 /* !!! */
300 exit
301 elseif errno==6 then /* home not first command: shoud never happen */
302 exit
303 elseif errno==8 then /* control deviation too large */
304 /*
305 *
306 */
307 elseif errno==9 then /* index not found: shoud never happen */
308 exit
309 elseif errno==10 then /* unknown command: shoud never happen */
310 exit
311 elseif errno==11 then /* software endswitch reached */
312 /*
313 *
314 */
315 elseif errno==12 then /* wrong paremeter number: shoud never happen */
316 exit
317 elseif errno==14 then /* too many LOOP commands: shoud never happen */
318 exit
319 elseif errno==16 then /* parameter in EEPROM broken */
320 exit
321 elseif errno==17 then /* programs in EEPROM broken */
322 exit
323 elseif errno==18 then /* RESET by CPU: Reason could be power-problems */
324 exit
325 elseif errno==19 then /* User break */
326 exit
327 elseif errno==25 then /* hardware Endswitch reached */
328 /*
329 *
330 */
331 elseif errno==51 then /* too many gosub: shoud never happen */
332 exit
333 elseif errno==52 then /* too many return: shoud never happen */
334 exit
335 elseif errno==62 then /* error verifying EEPROM */
336 exit
337 elseif errno==70 then /* error in DIM statement: should never happen */
338 exit
339 elseif errno==72 then /* DIM limit reached: should never happen */
340 exit
341 elseif errno==79 then /* Timeout waiting for an index */
342 exit
343 elseif errno==84 then /* Too many ON TIME calls */
344 exit
345 elseif errno==87 then /* storage for variables exhausted */
346 exit
347 else
348 print "Unknown (internal) error #", errno
349 exit
350 endif
351
352 exit
353 /*errclr*/ /* errclr includes 'motor on' which enables the motor controlling */
354
355 return
356ENDPROG
357
358
Note: See TracBrowser for help on using the repository browser.