From 6a39018ddad582c901866c1a4ae9c59eb188ef90 Mon Sep 17 00:00:00 2001 From: Pavel Pisa Date: Sun, 1 Jun 2014 12:31:21 +0200 Subject: [PATCH] RoCoN: Add command to select motor driver type/axis mode at runtime. Signed-off-by: Pavel Pisa --- sw/app/rocon/appl_cmds.c | 33 ++++++++++++++++ sw/app/rocon/appl_pxmc.c | 82 +++++++++++++++++++++++++++++++++------- 2 files changed, 102 insertions(+), 13 deletions(-) diff --git a/sw/app/rocon/appl_cmds.c b/sw/app/rocon/appl_cmds.c index 907d615..a264e84 100644 --- a/sw/app/rocon/appl_cmds.c +++ b/sw/app/rocon/appl_cmds.c @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -85,6 +86,34 @@ int cmd_do_pthalign(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]) return 0; } +/** + * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode + * + * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1. + */ +int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]) +{ + int val; + pxmc_state_t *mcs; + + if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG; + + if(*param[2]=='?') { + return cmd_opchar_replong(cmd_io, param, pxmc_axis_rdmode(mcs), 0, 0); + } + + if(*param[2]!=':') return -CMDERR_OPCHAR; + + if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG; + + val=atol(param[3]); + val=pxmc_axis_mode(mcs,val); + if(val<0) + return val; + + return 0; +} + int cmd_do_sqrtll(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]) { char *ps; @@ -113,6 +142,9 @@ cmd_des_t const cmd_des_stamp={0, CDESM_OPCHR, cmd_des_t const cmd_des_pthalign={0, CDESM_OPCHR|CDESM_RW,"PTHALIGN?","run alignement of phase according to HAL", cmd_do_pthalign, {0,0}}; +cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR, + "REGMODE?","axis working mode",cmd_do_axis_mode, + {}}; cmd_des_t const cmd_des_sqrtll={0, 0,"sqrtll","test 64-bit square root computation", cmd_do_sqrtll, {0,0}}; @@ -125,6 +157,7 @@ cmd_des_t const *const cmd_appl_specific[]={ &cmd_des_status_bsybits, &cmd_des_stamp, &cmd_des_pthalign, + &cmd_des_axis_mode, &cmd_des_sqrtll, NULL }; diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c index bdc2f06..d9a8408 100644 --- a/sw/app/rocon/appl_pxmc.c +++ b/sw/app/rocon/appl_pxmc.c @@ -1351,19 +1351,57 @@ int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods) return res; } +int pxmc_axis_rdmode(pxmc_state_t *mcs) +{ + if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out) + return PXMC_AXIS_MODE_STEPPER_WITH_IRC; + if (mcs->pxms_do_out == pxmc_rocon_pwm_dc_out) + return PXMC_AXIS_MODE_DC; + if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out) + return PXMC_AXIS_MODE_BLDC; + return -1; +} + + int pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode) { - int res; + static const typeof(*mcs->pxms_ptptr1) dummy0 = 0; + int res = 0; - mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90); + if (mode == PXMC_AXIS_MODE_NOCHANGE) + mode = pxmc_axis_rdmode(mcs); + if (mode < 0) + return -1; - /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */ -#ifndef PXMC_WITH_PT_ZIC - res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0); -#else /*PXMC_WITH_PT_ZIC*/ - res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0); -#endif /*PXMC_WITH_PT_ZIC*/ + switch (mode) { + /*case PXMC_AXIS_MODE_STEPPER:*/ + case PXMC_AXIS_MODE_STEPPER_WITH_IRC: + res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0); + break; + /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/ + case PXMC_AXIS_MODE_DC: + /*profive some sane dummy values*/ + mcs->pxms_ptptr1 = (typeof(mcs->pxms_ptptr1))&dummy0; + mcs->pxms_ptptr2 = (typeof(mcs->pxms_ptptr1))&dummy0; + mcs->pxms_ptptr3 = (typeof(mcs->pxms_ptptr1))&dummy0; + + mcs->pxms_ptscale_mult=1; + mcs->pxms_ptscale_shift=15; + break; + case PXMC_AXIS_MODE_BLDC: + /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */ + #ifndef PXMC_WITH_PT_ZIC + res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0); + #else /*PXMC_WITH_PT_ZIC*/ + res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0); + #endif /*PXMC_WITH_PT_ZIC*/ + break; + default: + return -1; + } + + mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90); return res; } @@ -1387,14 +1425,32 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode) pxmc_clear_flag(mcs, PXMS_PHA_b); pxmc_clear_flag(mcs, PXMS_PTI_b); + if (mode == PXMC_AXIS_MODE_NOCHANGE) + mode = pxmc_axis_rdmode(mcs); + if (mode < 0) + return -1; if (!mode) - { - /*mode=pxmc_axis_rdmode(mcs);*/ - if (!mode) - mode = PXMC_AXIS_MODE_BLDC; - } + mode = PXMC_AXIS_MODE_DC; res = pxmc_axis_pt4mode(mcs, mode); + if (res < 0) + return -1; + + switch (mode) { + /*case PXMC_AXIS_MODE_STEPPER:*/ + case PXMC_AXIS_MODE_STEPPER_WITH_IRC: + mcs->pxms_do_out = pxmc_rocon_pwm2ph_out; + break; + /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/ + case PXMC_AXIS_MODE_DC: + mcs->pxms_do_out = pxmc_rocon_pwm_dc_out; + break; + case PXMC_AXIS_MODE_BLDC: + mcs->pxms_do_out = pxmc_rocon_pwm3ph_out; + break; + default: + return -1; + } pxmc_set_flag(mcs, PXMS_ENI_b); return res; -- 2.39.2