sw/app/rocon/appl_coordmv.c

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components
   3
   4   appl_coordmv.c - application specific coordinated
   5                    motions control support
   6
   7   Copyright (C) 2001-2014 by Pavel Pisa - originator
   8                           pisa@cmp.felk.cvut.cz
   9             (C) 2001-2014 by PiKRON Ltd. - originator
  10                     http://www.pikron.com
  11
  12   This file can be used and copied according to next
  13   license alternatives
  14    - GPL - GNU Public License
  15    - other license provided by project originators
  16
  17  *******************************************************************/
  18
  19 #include <cpu_def.h>
  20 #include <system_def.h>
  21 #include <pxmc.h>
  22 #include <pxmc_coordmv.h>
  23 #include <cmd_proc.h>
  24 #include <pxmc_cmds.h>
  25 #include <utils.h>
  26
  27 #include "appl_defs.h"
  28
  29 int pxmc_coordmv_process(void)
  30 {
  31   /* coordinator bottom handling */
  32   if (pxmc_coordmv_state.mcs_flg & MCS_BOTT_m){
  33     pxmc_coordmv_bottom(&pxmc_coordmv_state);
  34   }
  35
  36   return 0;
  37 }
  38
  39 int cmd_do_coordmv(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  40 {
  41   pxmc_coordmv_state_t *mcs_state = &pxmc_coordmv_state;
  42   int con_cnt = mcs_state->mcs_con_cnt;
  43   int i;
  44   long final[con_cnt], val;
  45   long mintim = 0;
  46   char *s;
  47   pxmc_state_t *mcs;
  48   if(*param[2]!=':') return -CMDERR_OPCHAR;
  49   s = param[3];
  50   if ((long)(des->info[1]) & 1) {       /* coordinated movement with time */
  51     if (si_long(&s, &mintim, 10) <0 ) return -CMDERR_BADPAR;
  52     if (mintim < 0) return -CMDERR_BADPAR;
  53     if (si_fndsep(&s,",") <=0 ) return -CMDERR_BADSEP;
  54   }
  55   if (!con_cnt) return -CMDERR_BADCFG;
  56   i = 0;
  57   while (1) {
  58     if(si_long(&s, &val, 10) <0 ) return -CMDERR_BADPAR;
  59     mcs = pxmc_coordmv_indx2mcs(mcs_state, i);
  60     if(!mcs) return -CMDERR_BADREG;
  61     final[i] = val << PXMC_SUBDIV(mcs);
  62     if(++i >= con_cnt) break;
  63     if(si_fndsep(&s, ",") <= 0) return -CMDERR_BADSEP;
  64   }
  65   si_skspace(&s);
  66   if(*s) return -CMDERR_GARBAG;
  67
  68   if ((long)(des->info[1]) & 2){
  69     if (pxmc_coordmv_relmv(mcs_state, con_cnt, final, mintim)<0)
  70       return -1;
  71   } else {
  72     if (pxmc_coordmv_absmv(mcs_state, con_cnt, final, mintim)<0)
  73       return -1;
  74   }
  75   return 0;
  76 }
  77
  78
  79 int cmd_do_coordspline(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  80 {
  81   pxmc_coordmv_state_t *mcs_state = &pxmc_coordmv_state;
  82   int con_cnt = mcs_state->mcs_con_cnt;
  83   long spline_param[con_cnt * PXMC_SPLINE_ORDER_MAX];
  84   int i;
  85   int o;
  86   int a;
  87   long val;
  88   long mintim=0;
  89   long order;
  90   char *s;
  91   pxmc_state_t *mcs;
  92   if (*param[2] != ':') return -CMDERR_OPCHAR;
  93   s = param[3];
  94   if ((long)(des->info[1]) & 1) {       /* coordinated movement with time */
  95     if (si_long(&s,&mintim, 10)<0) return -CMDERR_BADPAR;
  96     if (mintim<0) return -CMDERR_BADPAR;
  97     if (si_fndsep(&s, ",")<=0) return -CMDERR_BADSEP;
  98   }
  99   if (!con_cnt) return -CMDERR_BADCFG;
 100
 101   if (si_long(&s,&order, 10) <0 ) return -CMDERR_BADPAR;
 102   if (mintim <0 ) return -CMDERR_BADPAR;
 103   if (si_fndsep(&s, ",") <= 0) return -CMDERR_BADSEP;
 104
 105   if ((order <= 0) || (order > PXMC_SPLINE_ORDER_MAX))
 106     return -CMDERR_BADPAR;
 107
 108   i=0; o=0; a=0;
 109   while (1) {
 110     if (si_long(&s, &val, 10) < 0) return -CMDERR_BADPAR;
 111     mcs = pxmc_coordmv_indx2mcs(mcs_state, a);
 112     if (!mcs) return -CMDERR_BADREG;
 113     spline_param[i] = val << PXMC_SUBDIV(mcs);;
 114     i++;
 115     if (++o >= order) {
 116       o = 0;
 117       if (++a >= con_cnt) break;
 118     }
 119     if (si_fndsep(&s, ",") <= 0) return -CMDERR_BADSEP;
 120   }
 121   si_skspace(&s);
 122   if(*s) return -CMDERR_GARBAG;
 123
 124   if (pxmc_coordmv_spline(mcs_state, i, spline_param, order, mintim) < 0)
 125     return -1;
 126   return 0;
 127 }
 128
 129
 130 int cmd_do_coordgrp(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 131 {
 132   pxmc_coordmv_state_t *mcs_state = &pxmc_coordmv_state;
 133   pxmc_state_list_t *reg_list = mcs_state->mcs_con_list;
 134   int reg_max = reg_list->pxml_cnt;
 135   int ret;
 136   unsigned chan;
 137   int con_indx[reg_max];
 138   int con_cnt=0;
 139   char *s;
 140   if (*param[2] != ':') return -CMDERR_OPCHAR;
 141   s = param[3];
 142   do{
 143     si_skspace(&s);
 144     if (!*s) break;
 145     chan = *(s++) - 'A';
 146     if (chan >= reg_max) return -CMDERR_BADREG;
 147     con_indx[con_cnt] = chan;
 148     con_cnt++;
 149     if ((ret = si_fndsep(&s, ",")) < 0) return -CMDERR_BADSEP;
 150     if (ret == 0) break;
 151   } while (1);
 152
 153   return pxmc_coordmv_grp(mcs_state, con_cnt, con_indx);
 154 }
 155
 156 cmd_des_t const cmd_des_coordmv={0, CDESM_OPCHR|CDESM_WR,
 157                         "COORDMV","initiate coordinated movement to point f1,f2,...",cmd_do_coordmv,
 158                         {0,0}};
 159 cmd_des_t const cmd_des_coordmvt={0, CDESM_OPCHR|CDESM_WR,
 160                         "COORDMVT","coord. movement with time to point mintime,f1,f2,...",cmd_do_coordmv,
 161                         {0,(char*)1}};
 162 cmd_des_t const cmd_des_coordrelmvt={0, CDESM_OPCHR|CDESM_WR,
 163                         "COORDRELMVT","coord. relative movement with time to point mintime,f1,f2,...",
 164                         cmd_do_coordmv,
 165                         {0,(char*)3}};
 166 cmd_des_t const cmd_des_coordsplinet={0, CDESM_OPCHR|CDESM_WR,
 167                         "COORDSPLINET","coord. spline movement with time to point mintime,order,a11,a12,...,a21,..",
 168                         cmd_do_coordspline,
 169                         {0,(char*)1}};
 170 cmd_des_t const cmd_des_coordgrp={0, CDESM_OPCHR|CDESM_WR,
 171                         "COORDGRP","group axes for COORDMV, for ex. C,D,F",cmd_do_coordgrp,
 172                         {0,0}};
 173 cmd_des_t const cmd_des_coorddiscont={0, CDESM_OPCHR|CDESM_RW,
 174                         "COORDDISCONT","max relative discontinuity between segs",cmd_do_rw_long,
 175                         {(char*)&pxmc_coordmv_state.mcs_disca,
 176                          0}};
 177
 178 cmd_des_t const *const cmd_pxmc_coordmv[]={
 179   &cmd_des_coordmv,
 180   &cmd_des_coordmvt,
 181   &cmd_des_coordrelmvt,
 182   &cmd_des_coordsplinet,
 183   &cmd_des_coordgrp,
 184   &cmd_des_coorddiscont,
 185   NULL
 186 };