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

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