1 /*******************************************************************
2 Components for embedded applications builded for
3 laboratory and medical instruments firmware
5 cmd_pxmc.c - interconnection of PXMC library
6 subsystem with RS-232 command processor
7 used mainly for controller tuning
9 Copyright (C) 2001 by Pavel Pisa pisa@cmp.felk.cvut.cz
10 (C) 2002 by PiKRON Ltd. http://www.pikron.com
12 *******************************************************************/
25 * cmd_opchar_getreg - selects the right axis
27 * pxmc is designed for multi axis motion control, so each axis must be identificated.
28 * This done by a capital letter. The first axis must be A, the 2nd B, etc.
30 pxmc_state_t *cmd_opchar_getreg(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
35 if(chan>=pxmc_main_list.pxml_cnt) return NULL;
36 mcs=pxmc_main_list.pxml_arr[chan];
42 * cmd_do_reg_go - checks the command format validity and calls pxmc_go.
44 * if pxmc_go returns -1, cmd_do_reg_go returns -1.
46 int cmd_do_reg_go(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
50 if(*param[2]!=':') return -CMDERR_OPCHAR;
51 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
52 val=atol(param[3])<<PXMC_SUBDIV(mcs);
53 val=pxmc_go(mcs,val,0,0);
61 * cmd_do_pwm - checks the command format validity and calls pxmc_set_const_out.
64 int cmd_do_pwm(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
68 if(*param[2]!=':') return -CMDERR_OPCHAR;
69 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
71 pxmc_set_const_out(mcs,val);
76 * cmd_do_reg_hh - checks the command format validity and calls pxmc_hh (home hardware).
78 * if pxmc_hh returns -1, cmd_do_reg_hh returns -1.
80 int cmd_do_reg_hh(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
84 if(*param[2]!=':') return -CMDERR_OPCHAR;
85 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
94 * cmd_do_reg_spd - checks the command format validity and calls pxmc_spd.
96 * if pxmc_spd returns -1, cmd_do_reg_spd returns -1.
98 int cmd_do_reg_spd(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
102 if(*param[2]!=':') return -CMDERR_OPCHAR;
103 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
105 val=pxmc_spd(mcs,val,0);
112 * cmd_do_reg_spdfg - checks the command format validity and calls pxmc_spdfg.
114 * if pxmc_spdfg returns -1, cmd_do_reg_spdfg returns -1.
116 int cmd_do_reg_spdfg(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
120 if(*param[2]!=':') return -CMDERR_OPCHAR;
121 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
123 val=pxmc_spdfg(mcs,val,0);
130 * cmd_do_stop - checks the command format validity and calls pxmc_stop.
133 int cmd_do_stop(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
136 if(*param[2]!=':') return -CMDERR_OPCHAR;
137 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
143 * cmd_do_release - checks the command format validity and calls pxmc_set_const_out(mcs,0).
146 int cmd_do_release(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
149 if(*param[2]!=':') return -CMDERR_OPCHAR;
150 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
151 pxmc_set_const_out(mcs,0);
156 * cmd_do_clrerr - checks the command format validity, clears the error flag.
158 * it also stop the rotation calling pxmc_set_const_out(mcs,0)
160 int cmd_do_clrerr(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
163 if(*param[2]!=':') return -CMDERR_OPCHAR;
164 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
165 pxmc_set_const_out(mcs,0);
166 pxmc_clear_flag(mcs,PXMS_ERR_b);
171 * cmd_do_zero - checks the command format validity, sets axis position to 0.
173 * it also stop the rotation calling pxmc_set_const_out(mcs,0)
175 int cmd_do_zero(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
178 if(*param[2]!=':') return -CMDERR_OPCHAR;
179 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
180 pxmc_set_const_out(mcs,0);
181 pxmc_axis_set_pos(mcs,0);
186 * cmd_do_align - checks the command format validity, sets offset between table and IRC counter to 0 .
188 * it also stop the rotation calling pxmc_set_const_out(mcs,0) and sets axis position to 0.
190 int cmd_do_align(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
193 if(*param[2]!=':') return -CMDERR_OPCHAR;
194 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
195 pxmc_set_const_out(mcs,0);
196 pxmc_axis_set_pos(mcs,0);
202 * cmd_do_reg_rw_pos - read or write function, param is converted in 'long' and shifted
204 * if the command typed is a write function, records the value,
205 * if it is a read function returns the value asked.
208 int cmd_do_reg_rw_pos(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
215 if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
216 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
217 ptr=(long*)((long)des->info[0]+(char*)mcs);
220 *ptr=val<<PXMC_SUBDIV(mcs);
222 return cmd_opchar_replong(cmd_io, param, (*ptr)>>PXMC_SUBDIV(mcs), 0, 0);
228 * cmd_do_reg_short_val - read or write function, param is converted in 'integer'
230 * if the command typed is a write function, records the value,
231 * if it is a read function returns the value asked.
234 int cmd_do_reg_short_val(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
241 if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
242 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
243 ptr=(short*)((long)des->info[0]+(char*)mcs);
248 return cmd_opchar_replong(cmd_io, param, (long)*ptr, 0, 0);
254 * cmd_do_reg_long_val - read or write function, param is converted in 'long'
256 * if the command typed is a write function, records the value,
257 * if it is a read function returns the value asked.
259 int cmd_do_reg_long_val(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
266 if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
267 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
268 ptr=(long*)((long)des->info[0]+(char*)mcs);
273 return cmd_opchar_replong(cmd_io, param, (long)*ptr, 0, 0);
280 * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
282 * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
284 int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
288 if(*param[2]!=':') return -CMDERR_OPCHAR;
289 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
290 if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
292 val=pxmc_axis_mode(mcs,val);
299 int cmd_do_regptmod_short_val(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
306 if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
307 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
308 ptr=(short*)((long)des->info[0]+(char*)mcs);
310 if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
311 pxmc_set_const_out(mcs,0);
313 if((val<((long)des->info[1])) || (val>((long)des->info[2])))
314 return -CMDERR_BADPAR;
316 val=pxmc_axis_mode(mcs,0);
320 return cmd_opchar_replong(cmd_io, param, (long)*ptr, 0, 0);
327 * cmd_do_reg_type - no code written, will set controller structure
329 int cmd_do_reg_type(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
331 /* should set controller structure */
337 * cmd_do_regsfrq - sets or returns smapling frequency
340 int cmd_do_regsfrq(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
345 if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
348 return pxmc_sfi_sel(val);
350 return cmd_opchar_replong(cmd_io, param, pxmc_get_sfi_hz(NULL), 0, 0);
354 #endif /* WITH_SFI_SEL */
357 /* debugging functions */
358 int cmd_do_reg_dbgset(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
364 if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
365 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
368 pxmc_dbgset(mcs,pxmc_dbg_ene_as,val);
370 cmd_io_write(cmd_io,param[0],param[2]-param[0]);
371 cmd_io_putc(cmd_io,'=');
372 cmd_io_putc(cmd_io,mcs->pxms_flg&PXMS_DBG_m?'1':'0');
378 int cmd_do_reg_dbgpre(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
380 pxmc_dbg_hist_t *hist;
386 if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
388 if(si_long(&ps,&count,0)<0) return -CMDERR_BADPAR;
389 if(!count||(count>0x10000)) return -CMDERR_BADPAR;
390 pxmc_dbg_histfree(NULL);
391 if((hist=pxmc_dbg_histalloc(count+2))==NULL) return -CMDERR_NOMEM;
392 for(i=0;i<count;i++){
393 /* ps=cmd_rs232_rdline(cmd_io,1); */ /* !!!!!!!!! */
394 if(si_long(&ps,&val,0)<0) return -CMDERR_BADPAR;
402 int cmd_do_reg_dbghis(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
404 pxmc_dbg_hist_t *hist;
409 if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
411 if(si_long(&ps,&count,0)<0) return -CMDERR_BADPAR;
412 if(!count||(count>0x10000)) return -CMDERR_BADPAR;
414 for(i=0;i<count;i++){
415 if(hist&&(&hist->buff[i]<hist->end))
419 /* printf("%ld\r\n",val); */ /* !!!!!!!! */
425 int cmd_do_reg_dbggnr(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
431 if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
432 for(i=0;i<pxmc_main_list.pxml_cnt;i++){
433 mcs=pxmc_main_list.pxml_arr[i];
434 if(mcs&&(mcs->pxms_flg&PXMS_DBG_m)){
435 if(pxmc_dbg_gnr(mcs)<0){
436 for(i=0;i<pxmc_main_list.pxml_cnt;i++){
437 mcs=pxmc_main_list.pxml_arr[i];
438 if(mcs&&(mcs->pxms_flg&PXMS_DBG_m))
439 pxmc_stop(pxmc_main_list.pxml_arr[i],0);
441 return -CMDERR_BADREG;
445 pxmc_dbg_hist->ptr=pxmc_dbg_hist->buff;
450 /* motor executable commands */
451 cmd_des_t const cmd_des_go={0, CDESM_OPCHR|CDESM_RW,
452 "G?","go to target position",cmd_do_reg_go,{}};
453 cmd_des_t const cmd_des_pwm={0, CDESM_OPCHR|CDESM_RW,
454 "PWM?","direct axis PWM output",cmd_do_pwm,{}};
455 cmd_des_t const cmd_des_hh={0, CDESM_OPCHR,"HH?","hard home request for axis",cmd_do_reg_hh,{}};
456 cmd_des_t const cmd_des_spd={0, CDESM_OPCHR,"SPD?","speed request for axis",cmd_do_reg_spd,{}};
457 cmd_des_t const cmd_des_spdfg={0, CDESM_OPCHR,"SPDFG?",
458 "fine grained speed request for axis",cmd_do_reg_spdfg,{}};
459 cmd_des_t const cmd_des_stop={0, CDESM_OPCHR,
460 "STOP?","stop motion of requested axis",cmd_do_stop,{}};
461 cmd_des_t const cmd_des_release={0, CDESM_OPCHR,
462 "RELEASE?","releases axis closed loop control",cmd_do_release,{}};
463 cmd_des_t const cmd_des_zero={0, CDESM_OPCHR,
464 "ZERO?","zero actual position",cmd_do_zero,{}};
465 cmd_des_t const cmd_des_align={0, CDESM_OPCHR,
466 "ALIGN?","align commutator",cmd_do_align,{}};
467 cmd_des_t const cmd_des_clrerr={0, CDESM_OPCHR,
468 "PURGE?"," clear 'axis in error state' flag",cmd_do_clrerr,{}};
469 /* motors and controllers variables */
470 cmd_des_t const cmd_des_ap={0, CDESM_OPCHR|CDESM_RD,
471 "AP?","actual position",cmd_do_reg_rw_pos,
472 {(char*)pxmc_state_offs(pxms_ap),
474 cmd_des_t const cmd_des_st={0, CDESM_OPCHR|CDESM_RD,
475 "ST?","axis status bits encoded in number",cmd_do_reg_short_val,
476 {(char*)pxmc_state_offs(pxms_flg),
478 cmd_des_t const cmd_des_axerr={0, CDESM_OPCHR|CDESM_RD,
479 "AXERR?","last axis error code",cmd_do_reg_short_val,
480 {(char*)pxmc_state_offs(pxms_errno),
483 cmd_des_t const cmd_des_r_one={0, CDESM_OPCHR,
484 "R?","send R?! or FAIL?! at axis finish",cmd_do_r_one,{}};
486 cmd_des_t const cmd_des_regp={0, CDESM_OPCHR|CDESM_RW,
487 "REGP?","controller proportional gain",cmd_do_reg_short_val,
488 {(char*)pxmc_state_offs(pxms_p),
490 cmd_des_t const cmd_des_regi={0, CDESM_OPCHR|CDESM_RW,
491 "REGI?","controller integral gain",cmd_do_reg_short_val,
492 {(char*)pxmc_state_offs(pxms_i),
494 cmd_des_t const cmd_des_regd={0, CDESM_OPCHR|CDESM_RW,
495 "REGD?","controller derivative gain",cmd_do_reg_short_val,
496 {(char*)pxmc_state_offs(pxms_d),
498 cmd_des_t const cmd_des_regs1={0, CDESM_OPCHR|CDESM_RW,
499 "REGS1?","controller S1",cmd_do_reg_short_val,
500 {(char*)pxmc_state_offs(pxms_s1),
502 cmd_des_t const cmd_des_regs2={0, CDESM_OPCHR|CDESM_RW,
503 "REGS2?","controller S2",cmd_do_reg_short_val,
504 {(char*)pxmc_state_offs(pxms_s2),
506 cmd_des_t const cmd_des_regmd={0, CDESM_OPCHR|CDESM_RW,
507 "REGMD?","maximal allowed position error",cmd_do_reg_rw_pos,
508 {(char*)pxmc_state_offs(pxms_md),
510 cmd_des_t const cmd_des_regms={0, CDESM_OPCHR|CDESM_RW,
511 "REGMS?","maximal speed",cmd_do_reg_long_val,
512 {(char*)pxmc_state_offs(pxms_ms),
514 cmd_des_t const cmd_des_regacc={0, CDESM_OPCHR|CDESM_RW,
515 "REGACC?","maximal acceleration",cmd_do_reg_long_val,
516 {(char*)pxmc_state_offs(pxms_ma),
518 cmd_des_t const cmd_des_regme={0, CDESM_OPCHR|CDESM_RW,
519 "REGME?","maximal PWM energy or voltage for axis",cmd_do_reg_short_val,
520 {(char*)pxmc_state_offs(pxms_me),
522 cmd_des_t const cmd_des_regcfg={0, CDESM_OPCHR|CDESM_RW,
523 "REGCFG?","hard home and profile configuration",cmd_do_reg_short_val,
524 {(char*)pxmc_state_offs(pxms_cfg),
526 cmd_des_t const cmd_des_ptirc={0, CDESM_OPCHR|CDESM_RW,
527 "REGPTIRC?","number of irc pulses per phase table",cmd_do_regptmod_short_val,
528 {(char*)pxmc_state_offs(pxms_ptirc),
529 (char*)4,(char*)10000}};
530 cmd_des_t const cmd_des_ptper={0, CDESM_OPCHR|CDESM_RW,
531 "REGPTPER?","number of elmag. revolutions per phase table",cmd_do_regptmod_short_val,
532 {(char*)pxmc_state_offs(pxms_ptper),
533 (char*)1,(char*)100}};
534 cmd_des_t const cmd_des_ptshift={0, CDESM_OPCHR|CDESM_RW,
535 "REGPTSHIFT?","shift (in irc) of generated phase curves",cmd_do_reg_short_val,
536 {(char*)pxmc_state_offs(pxms_ptshift),
538 cmd_des_t const cmd_des_ptvang={0, CDESM_OPCHR|CDESM_RW,
539 "REGPTVANG?","angle (in irc) between rotor and stator mag. fld.",cmd_do_reg_short_val,
540 {(char*)pxmc_state_offs(pxms_ptvang),
542 cmd_des_t const cmd_des_pwm1cor={0, CDESM_OPCHR|CDESM_RW,
543 "REGPWM1COR?","PWM1 correction",cmd_do_reg_short_val,
544 {(char*)pxmc_state_offs(pxms_pwm1cor),
546 cmd_des_t const cmd_des_pwm2cor={0, CDESM_OPCHR|CDESM_RW,
547 "REGPWM2COR?","PWM2 correction",cmd_do_reg_short_val,
548 {(char*)pxmc_state_offs(pxms_pwm2cor),
550 cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
551 "REGMODE?","axis working mode",cmd_do_axis_mode,
554 cmd_des_t const cmd_des_regsfrq={0, CDESM_OPCHR|CDESM_RW,
555 "REGSFRQ","set new sampling frequency",cmd_do_regsfrq,{}};
556 #endif /* WITH_SFI_SEL */
557 /* axes debugging and tuning */
558 cmd_des_t const cmd_des_reg_type={0, CDESM_OPCHR|CDESM_RW,
559 "REGTYPE?","unused",cmd_do_reg_type,{}};
560 cmd_des_t const cmd_des_reg_dbgset={0, CDESM_OPCHR|CDESM_RW,
561 "REGDBG?","sets debug flag",cmd_do_reg_dbgset,{}};
562 cmd_des_t const cmd_des_reg_dbgpre={0, CDESM_OPCHR|CDESM_WR,
563 "REGDBGPRE","store reference course",cmd_do_reg_dbgpre,{}};
564 cmd_des_t const cmd_des_reg_dbghis={0, CDESM_OPCHR|CDESM_WR,
565 "REGDBGHIS","read history course",cmd_do_reg_dbghis,{}};
566 cmd_des_t const cmd_des_reg_dbggnr={0, CDESM_OPCHR|CDESM_WR,
567 "REGDBGGNR","controller response to HIST course",cmd_do_reg_dbggnr,{}};
569 cmd_des_t const *cmd_pxmc_default[]={
604 #endif /* WITH_SFI_SEL */