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

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