source: trunk/Cosy/aposs/Manual.m@ 16077

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