/*******************************************************************
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
#include "appl_defs.h"
#include "appl_fpga.h"
+#include "appl_pxmc.h"
#include "pxmcc_types.h"
#include "pxmcc_interface.h"
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,
}
}
+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)
{
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;
return pxmc_rocon_hh_gi;
}
-pxmc_state_t mcs0 =
+pxmc_rocon_state_t mcs0 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*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:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs2 =
+pxmc_rocon_state_t mcs2 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs3 =
+pxmc_rocon_state_t mcs3 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs4 =
+pxmc_rocon_state_t mcs4 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs5 =
+pxmc_rocon_state_t mcs5 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs6 =
+pxmc_rocon_state_t mcs6 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs7 =
+pxmc_rocon_state_t mcs7 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*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 =
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;
--- /dev/null
+/*******************************************************************
+ 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 <system_def.h>
+#include <pxmc.h>
+#include <stdlib.h>
+#include <string.h>
+#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
+};
projects / fpga / lx-cpu1 / lx-rocon.git / commitdiff