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

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