From 5f0a3f30353515eb28bcfc80a96624fb9d88c749 Mon Sep 17 00:00:00 2001 From: Pavel Pisa Date: Sun, 11 Jan 2015 10:22:24 +0100 Subject: [PATCH 1/1] RoCoN: add proper DQ current components controllers for stepper motor. Signed-off-by: Pavel Pisa --- sw/app/rocon/Makefile.omk | 1 + sw/app/rocon/appl_cmdproc.c | 2 + sw/app/rocon/appl_pxmc.c | 175 ++++++++++++++++++++++++++------- sw/app/rocon/appl_pxmc.h | 42 ++++++++ sw/app/rocon/appl_pxmccmds.c | 63 ++++++++++++ sw/app/rocon/pxmcc_interface.h | 12 +++ 6 files changed, 260 insertions(+), 35 deletions(-) create mode 100644 sw/app/rocon/appl_pxmc.h create mode 100644 sw/app/rocon/appl_pxmccmds.c diff --git a/sw/app/rocon/Makefile.omk b/sw/app/rocon/Makefile.omk index fb6ae11..c1a7af9 100644 --- a/sw/app/rocon/Makefile.omk +++ b/sw/app/rocon/Makefile.omk @@ -125,6 +125,7 @@ lib_LOADLIBES += pxmc_coordmv endif ifneq ($(CONFIG_APP_ROCON_WITH_SIM_POSIX),y) rocon_SOURCES += appl_pxmc.c +rocon_SOURCES += appl_pxmccmds.c endif lib_LOADLIBES += pxmc pxmcbsp endif diff --git a/sw/app/rocon/appl_cmdproc.c b/sw/app/rocon/appl_cmdproc.c index 0dc038d..d6034a7 100644 --- a/sw/app/rocon/appl_cmdproc.c +++ b/sw/app/rocon/appl_cmdproc.c @@ -47,6 +47,7 @@ extern cmd_des_t const *cmd_appl_tests[]; extern cmd_des_t const *cmd_pxmc_ptable[]; extern cmd_des_t const *cmd_pxmc_coordmv[]; extern cmd_des_t const *cmd_appl_specific[]; +extern cmd_des_t const *cmd_appl_pxmc[]; extern cmd_des_t const cmd_des_dprint; @@ -124,6 +125,7 @@ cmd_des_t const *cmd_list_main[] = CMD_DES_INCLUDE_SUBLIST(cmd_pxmc_coordmv), CMD_DES_INCLUDE_SUBLIST(cmd_appl_specific), #ifndef APPL_WITH_SIM_POSIX + CMD_DES_INCLUDE_SUBLIST(cmd_appl_pxmc), &cmd_des_dprint, #endif /*APPL_WITH_SIM_POSIX*/ NULL diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c index ff05a75..6208037 100644 --- a/sw/app/rocon/appl_pxmc.c +++ b/sw/app/rocon/appl_pxmc.c @@ -1,8 +1,7 @@ /******************************************************************* Motion and Robotic System (MARS) aplication components. - appl_pxmc.c - position controller subsystem core generic - and LP_MPW1 hardware specific support + appl_pxmc.c - position controller RoCoN hardware support Copyright (C) 2001-2013 by Pavel Pisa - originator pisa@cmp.felk.cvut.cz @@ -32,6 +31,7 @@ #include "appl_defs.h" #include "appl_fpga.h" +#include "appl_pxmc.h" #include "pxmcc_types.h" #include "pxmcc_interface.h" @@ -67,6 +67,18 @@ unsigned pxmc_rocon_pwm_magnitude = PXMC_LXPWR_PWM_CYCLE; long pxmc_rocon_irc_offset[PXML_MAIN_CNT]; unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT]; +static inline +pxmc_rocon_state_t *pxmc_state2rocon_state(pxmc_state_t *mcs) +{ + pxmc_rocon_state_t *mcsrc; + #ifdef UL_CONTAINEROF + mcsrc = UL_CONTAINEROF(mcs, pxmc_rocon_state_t, base); + #else /*UL_CONTAINEROF*/ + mcsrc = (pxmc_rocon_state_t*)((char*)mcs - __builtin_offsetof(pxmc_rocon_state_t, base)); + #endif /*UL_CONTAINEROF*/ + return mcsrc; +} + const uint8_t onesin10bits[1024]={ 0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5, 1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6, @@ -737,6 +749,31 @@ void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs, int enable_update) } } +void pxmcc_axis_get_cur_dq_filt(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q) +{ + volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs); + pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs); + uint32_t cur_d_cum = mcc_axis->cur_d_cum; + uint32_t cur_q_cum = mcc_axis->cur_q_cum; + int32_t cur_d; + int32_t cur_q; + int cur_d_div; + int cur_q_div; + + cur_d = cur_d_cum - mcsrc->cur_d_cum_prev; + mcsrc->cur_d_cum_prev = cur_d_cum; + cur_q = cur_q_cum - mcsrc->cur_q_cum_prev; + mcsrc->cur_q_cum_prev = cur_q_cum; + + cur_d_div = cur_d & 0x1f; + cur_d = cur_d / cur_d_div; + cur_q_div = cur_q & 0x1f; + cur_q = cur_q / cur_q_div; + + *p_cur_d = cur_d >> 12; + *p_cur_q = cur_q >> 12; +} + int pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs) { @@ -893,30 +930,84 @@ pxmc_pxmcc_nofb_con(pxmc_state_t *mcs) int pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs) { + pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs); volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs); - { - int ene, pwm_d, pwm_q; - ene = mcs->pxms_ene; - pwm_d = 0; - pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15; + if (mcs->pxms_flg & PXMS_ERR_m) { + pxmc_rocon_pwm2ph_wr(mcs, 0, 0); + pxmcc_axis_pwm_dq_out(mcs, 0, 0); + mcc_axis->steps_inc = 0; + } else { + int pwm_d, pwm_q; + int cur_d, cur_q; + int cur_d_req, cur_d_err; + int cur_q_req, cur_q_err; + int max_pwm = mcs->pxms_me; + int32_t stpinc; + + /* cur_d_err_sum; */ + /* cur_q_err_sum; */ + + pxmcc_axis_get_cur_dq_filt(mcs, &cur_d, &cur_q); + + if (!mcs->pxms_ene) + cur_d_req = 0; + else + cur_d_req = mcsrc->cur_hold; + + cur_d_err = cur_d_req - cur_d; + + pwm_d = (cur_d_err * mcsrc->cur_d_p) >> 8; + + if (mcsrc->cur_d_i) + mcsrc->cur_d_err_sum += cur_d_err * mcsrc->cur_d_i; + else + mcsrc->cur_d_err_sum = 0; + + pwm_d += mcsrc->cur_d_err_sum >> 16; + + /* pwm_d = (pxmc_rocon_pwm_magnitude * ene) >> 15; */ + + if (pwm_d > max_pwm) { + mcsrc->cur_d_err_sum -= (pwm_d - max_pwm) << 16; + pwm_d = max_pwm; + } else if (-pwm_d > max_pwm) { + mcsrc->cur_d_err_sum -= (pwm_d + max_pwm) << 16; + pwm_d = 0 - max_pwm; + } + + cur_q_req = 0; + + cur_q_err = cur_q_req - cur_q; + + pwm_q = (cur_q_req * mcsrc->cur_q_p) >> 8; + + if (mcsrc->cur_q_i) + mcsrc->cur_q_err_sum += cur_q_err * mcsrc->cur_q_i; + else + mcsrc->cur_q_err_sum = 0; + + pwm_q += mcsrc->cur_q_err_sum >> 16; + + if (pwm_q > max_pwm) { + mcsrc->cur_q_err_sum -= (pwm_q - max_pwm) << 16; + pwm_q = max_pwm; + } else if (-pwm_q > max_pwm) { + mcsrc->cur_q_err_sum -= (pwm_q + max_pwm) << 16; + pwm_q = 0 - max_pwm; + } pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q); - if (mcs->pxms_flg & PXMS_ERR_m) { - pxmc_rocon_pwm2ph_wr(mcs, 0, 0); - } else { - int32_t stpinc; - mcs->pxms_ap=mcs->pxms_rp; - mcs->pxms_as=mcs->pxms_rs; - mcc_axis->steps_lim = mcc_axis->steps_cnt + 6; + mcs->pxms_ap=mcs->pxms_rp; + mcs->pxms_as=mcs->pxms_rs; + mcc_axis->steps_lim = mcc_axis->steps_cnt + 6; - stpinc = mcs->pxms_rs; - mcc_axis->steps_inc = stpinc; + stpinc = mcs->pxms_rs; + mcc_axis->steps_inc = stpinc; /* stpinc /= (mcs->pxms_ptirc << PXMC_SUBDIV(mcs)); */ /* pxms_ptscale_mult; pxms_ptscale_shift; */ - } } return 0; @@ -1384,8 +1475,9 @@ pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs) return pxmc_rocon_hh_gi; } -pxmc_state_t mcs0 = +pxmc_rocon_state_t mcs0 = { +.base = { pxms_flg: PXMS_ENI_m, pxms_do_inp: @@ -1417,10 +1509,17 @@ pxms_cfg: /*pxms_ptamp: 0x7fff,*/ pxms_hal: 0x40, +}, + .cur_d_p = 150, + .cur_d_i = 6000, + .cur_q_p = 150, + .cur_q_i = 6000, + .cur_hold = 200, }; -pxmc_state_t mcs1 = +pxmc_rocon_state_t mcs1 = { +.base = { pxms_flg: PXMS_ENI_m, pxms_do_inp: @@ -1451,10 +1550,11 @@ pxms_cfg: /*pxms_ptamp: 0x7fff,*/ pxms_hal: 0x40, -}; +}}; -pxmc_state_t mcs2 = +pxmc_rocon_state_t mcs2 = { +.base = { pxms_flg: PXMS_ENI_m, pxms_do_inp: @@ -1485,10 +1585,11 @@ pxms_cfg: /*pxms_ptamp: 0x7fff,*/ pxms_hal: 0x40, -}; +}}; -pxmc_state_t mcs3 = +pxmc_rocon_state_t mcs3 = { +.base = { pxms_flg: PXMS_ENI_m, pxms_do_inp: @@ -1519,10 +1620,11 @@ pxms_cfg: /*pxms_ptamp: 0x7fff,*/ pxms_hal: 0x40, -}; +}}; -pxmc_state_t mcs4 = +pxmc_rocon_state_t mcs4 = { +.base = { pxms_flg: PXMS_ENI_m, pxms_do_inp: @@ -1553,10 +1655,11 @@ pxms_cfg: /*pxms_ptamp: 0x7fff,*/ pxms_hal: 0x40, -}; +}}; -pxmc_state_t mcs5 = +pxmc_rocon_state_t mcs5 = { +.base = { pxms_flg: PXMS_ENI_m, pxms_do_inp: @@ -1587,10 +1690,11 @@ pxms_cfg: /*pxms_ptamp: 0x7fff,*/ pxms_hal: 0x40, -}; +}}; -pxmc_state_t mcs6 = +pxmc_rocon_state_t mcs6 = { +.base = { pxms_flg: PXMS_ENI_m, pxms_do_inp: @@ -1620,10 +1724,11 @@ pxms_cfg: /*pxms_ptamp: 0x7fff,*/ pxms_hal: 0x40, -}; +}}; -pxmc_state_t mcs7 = +pxmc_rocon_state_t mcs7 = { +.base = { pxms_flg: PXMS_ENI_m, pxms_do_inp: @@ -1653,11 +1758,11 @@ pxms_cfg: /*pxms_ptamp: 0x7fff,*/ pxms_hal: 0x40, -}; - +}}; pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] = -{&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7}; +{&mcs0.base, &mcs1.base, &mcs2.base, &mcs3.base, + &mcs4.base, &mcs5.base, &mcs6.base, &mcs7.base}; pxmc_state_list_t pxmc_main_list = @@ -2134,7 +2239,7 @@ int pxmc_initialize(void) int res; int i; - pxmc_state_t *mcs = &mcs0; + pxmc_state_t *mcs = &mcs0.base; lpc_qei_state_t *qst = &lpc_qei_state; *fpga_irc_reset = 1; diff --git a/sw/app/rocon/appl_pxmc.h b/sw/app/rocon/appl_pxmc.h new file mode 100644 index 0000000..f425c47 --- /dev/null +++ b/sw/app/rocon/appl_pxmc.h @@ -0,0 +1,42 @@ +/******************************************************************* + Components for embedded applications builded for + laboratory and medical instruments firmware + + pxmcc_rocon.h - multi axis motion controller coprocessor + RoCoN specific extensions + + (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz + (C) 2002-2014 by PiKRON Ltd. http://www.pikron.com + + This file can be used and copied according to next + license alternatives + - GPL - GNU Public License + - other license provided by project originators + + *******************************************************************/ + +#ifndef _APPL_PXMC_H_ +#define _APPL_PXMC_H_ + +#include +#include + +typedef struct pxmc_rocon_state_t { + pxmc_state_t base; + uint32_t cur_d_cum_prev; + uint32_t cur_q_cum_prev; + int32_t cur_d_err_sum; + int32_t cur_q_err_sum; + short cur_d_p; + short cur_d_i; + short cur_q_p; + short cur_q_i; + short cur_hold; +} pxmc_rocon_state_t; + +#define pxmc_rocon_state_offs(_fld) \ + (((size_t)&((pxmc_rocon_state_t *)0L)->_fld) - \ + ((size_t)&((pxmc_rocon_state_t *)0L)->base)) + + +#endif /*_APPL_PXMC_H_*/ diff --git a/sw/app/rocon/appl_pxmccmds.c b/sw/app/rocon/appl_pxmccmds.c new file mode 100644 index 0000000..1bcf667 --- /dev/null +++ b/sw/app/rocon/appl_pxmccmds.c @@ -0,0 +1,63 @@ +/******************************************************************* + Motion and Robotic System (MARS) aplication components. + + appl_pxmccmds.c - position controller RoCoN specific commands + + Copyright (C) 2001-2013 by Pavel Pisa - originator + pisa@cmp.felk.cvut.cz + (C) 2001-2013 by PiKRON Ltd. - originator + http://www.pikron.com + + This file can be used and copied according to next + license alternatives + - GPL - GNU Public License + - other license provided by project originators + + *******************************************************************/ + +#include +#include +#include +#include +#include "pxmc_cmds.h" + +#include "appl_defs.h" +#include "appl_fpga.h" +#include "appl_pxmc.h" + + +cmd_des_t const cmd_des_regcurdp={0, CDESM_OPCHR|CDESM_RW, + "REGCURDP?","current controller d component p parameter", cmd_do_reg_short_val, + {(char*)pxmc_rocon_state_offs(cur_d_p), + 0}}; + +cmd_des_t const cmd_des_regcurdi={0, CDESM_OPCHR|CDESM_RW, + "REGCURDI?","current controller d component i parameter", cmd_do_reg_short_val, + {(char*)pxmc_rocon_state_offs(cur_d_i), + 0}}; + +cmd_des_t const cmd_des_regcurqp={0, CDESM_OPCHR|CDESM_RW, + "REGCURQP?","current controller q component p parameter", cmd_do_reg_short_val, + {(char*)pxmc_rocon_state_offs(cur_q_p), + 0}}; + +cmd_des_t const cmd_des_regcurqi={0, CDESM_OPCHR|CDESM_RW, + "REGCURQI?","current controller q component i parameter", cmd_do_reg_short_val, + {(char*)pxmc_rocon_state_offs(cur_q_i), + 0}}; + +cmd_des_t const cmd_des_regcurhold={0, CDESM_OPCHR|CDESM_RW, + "REGCURHOLD?","current steady hold value for stepper", cmd_do_reg_short_val, + {(char*)pxmc_rocon_state_offs(cur_hold), + 0}}; + + +cmd_des_t const *cmd_appl_pxmc[] = +{ + &cmd_des_regcurdp, + &cmd_des_regcurdi, + &cmd_des_regcurqp, + &cmd_des_regcurqi, + &cmd_des_regcurhold, + NULL +}; diff --git a/sw/app/rocon/pxmcc_interface.h b/sw/app/rocon/pxmcc_interface.h index 2ffeccf..46a2a08 100644 --- a/sw/app/rocon/pxmcc_interface.h +++ b/sw/app/rocon/pxmcc_interface.h @@ -18,6 +18,7 @@ #ifndef _PXMCC_INTERFACE_H_ #define _PXMCC_INTERFACE_H_ +#include #include #include "pxmcc_types.h" @@ -52,6 +53,17 @@ void pxmcc_axis_enable(pxmc_state_t *mcs, int enable) mcc_axis->ccflg = enable? 1: 0; } +static inline +void pxmcc_axis_get_cur_dq_act(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q) +{ + volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs); + uint32_t cur_dq = mcc_axis->cur_dq; + *p_cur_d = (int16_t)(cur_dq >> 16); + *p_cur_q = (int16_t)(cur_dq); +} + +void pxmcc_axis_get_cur_dq_filt(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q); + void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs, int enable_update); int pxmcc_axis_setup(pxmc_state_t *mcs, int mode); -- 2.39.2