From 460a6d61f83d448249b49c108857a1d8cbd1cd45 Mon Sep 17 00:00:00 2001 From: Pavel Pisa Date: Sat, 10 Jan 2015 15:28:32 +0100 Subject: [PATCH] RoCoN and TUMBL firmware: initial experimental support of stepper motor without feedback. Signed-off-by: Pavel Pisa --- hw/lx-rocon_firmware/firmware.c | 9 ++- hw/lx-rocon_firmware/pxmcc_types.h | 6 ++ sw/app/rocon/appl_pxmc.c | 91 ++++++++++++++++++++++++++++-- sw/app/rocon/appl_tests.c | 6 +- 4 files changed, 104 insertions(+), 8 deletions(-) diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c index 8a21bbc..a139a3c 100644 --- a/hw/lx-rocon_firmware/firmware.c +++ b/hw/lx-rocon_firmware/firmware.c @@ -84,6 +84,7 @@ void main(void) pxmcc_data.common.pwm_cycle = 2500; pxmcc_data.common.min_idle = 0x7fff; + pxmcc_data.common.irc_base = (uint32_t)FPGA_IRC0; pxmcc_data.curadc[0].siroladc_offs = 0x0c17; pxmcc_data.curadc[1].siroladc_offs = 0x0c66; pxmcc_data.curadc[2].siroladc_offs = 0x0c66; @@ -118,7 +119,7 @@ void main(void) uint32_t pwmtx_info; volatile uint32_t *uptr; - irc = *(FPGA_IRC0 + (FPGA_IRC1 - FPGA_IRC0) * pxmcc->inp_info); + irc = *(uint32_t*)pxmcc->inp_info; pti = irc - ofs; /* @@ -308,6 +309,12 @@ void main(void) uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 24) & 0xff); pxmcc->pwm_prew[3] = *uptr & 0x3fff; *uptr = pwm4 | 0x4000; + if (pxmcc->mode == PXMCC_MODE_STEPPER) { + if (pxmcc->steps_cnt != pxmcc->steps_lim) { + pxmcc->steps_cnt++; + pxmcc->steps_pos += pxmcc->steps_inc; + } + } } } else { if (pxmcc->mode == PXMCC_MODE_BLDC) { diff --git a/hw/lx-rocon_firmware/pxmcc_types.h b/hw/lx-rocon_firmware/pxmcc_types.h index c09a18d..7228705 100644 --- a/hw/lx-rocon_firmware/pxmcc_types.h +++ b/hw/lx-rocon_firmware/pxmcc_types.h @@ -27,6 +27,7 @@ #define PXMCC_MODE_IDLE 2 #define PXMCC_MODE_BLDC 0 #define PXMCC_MODE_STEPPER_WITH_IRC 1 +#define PXMCC_MODE_STEPPER 3 typedef struct pxmcc_common_data_t { uint32_t fwversion; @@ -34,6 +35,7 @@ typedef struct pxmcc_common_data_t { uint32_t act_idle; uint32_t min_idle; uint32_t rx_done_sqn; + uint32_t irc_base; } pxmcc_common_data_t; typedef struct pxmcc_axis_data_t { @@ -54,6 +56,10 @@ typedef struct pxmcc_axis_data_t { uint32_t out_info; /* output index */ uint32_t pwmtx_info; /* offsets of pwm1 .. pwm4 from FPGA_LX_MASTER_TX */ uint32_t pwm_prew[4]; + uint32_t steps_lim; /* master selected final value to limit automatic steps */ + uint32_t steps_cnt; /* PXMCC counter - when reaches steps_lim then stops */ + uint32_t steps_inc; /* increments for selfgenerated stepper motor */ + uint32_t steps_pos; /* self generated position for stepper motor */ } pxmcc_axis_data_t; typedef struct pxmcc_curadc_data_t { diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c index 6954b0b..ff05a75 100644 --- a/sw/app/rocon/appl_pxmc.c +++ b/sw/app/rocon/appl_pxmc.c @@ -37,6 +37,7 @@ #define PXMC_AXIS_MODE_BLDC_PXMCC (PXMC_AXIS_MODE_BLDC + 1) #define PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC (PXMC_AXIS_MODE_BLDC + 2) +#define PXMC_AXIS_MODE_STEPPER_PXMCC (PXMC_AXIS_MODE_BLDC + 3) int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc, unsigned long index_irc, int diff2err); @@ -868,12 +869,66 @@ pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs) return 0; } +/** + * pxmc_pxmcc_nofb_inp - Dummy input for direct stepper motor control + * @mcs: Motion controller state information + */ +int +pxmc_pxmcc_nofb_inp(pxmc_state_t *mcs) +{ + return 0; +} + +/** + * pxmc_pxmcc_nofb_con - Empty controller for direct stepper motor control + * @mcs: Motion controller state information + */ +int +pxmc_pxmcc_nofb_con(pxmc_state_t *mcs) +{ + mcs->pxms_ene=mcs->pxms_me; + return 0; +} + +int +pxmc_pxmcc_nofb2ph_out(pxmc_state_t *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; + + 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; + + stpinc = mcs->pxms_rs; + mcc_axis->steps_inc = stpinc; + + /* stpinc /= (mcs->pxms_ptirc << PXMC_SUBDIV(mcs)); */ + /* pxms_ptscale_mult; pxms_ptscale_shift; */ + } + } + + return 0; +} + int pxmcc_axis_setup(pxmc_state_t *mcs, int mode) { volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data(); volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs); uint32_t ptirc; uint32_t ptreci; + uint32_t inp_info; uint32_t pwmtx_info; uint32_t pwmtx_info_dummy = 27; uint64_t ull; @@ -900,6 +955,9 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode) pxmcc_pxmc_ptofs2mcc(mcs, 0); + inp_info = (char*)&fpga_irc[mcs->pxms_inp_info]->count - (char*)fpga_irc[0]; + inp_info += mcc_data->common.irc_base; + switch (mode) { case PXMCC_MODE_IDLE: phcnt = 0; @@ -910,9 +968,14 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode) case PXMCC_MODE_STEPPER_WITH_IRC: phcnt = 4; break; + case PXMCC_MODE_STEPPER: + phcnt = 4; + mcc_axis->ptreci = 1; + inp_info = (char*)&mcc_axis->steps_pos - (char*)mcc_data; + break; } - mcc_axis->inp_info = mcs->pxms_inp_info; + mcc_axis->inp_info = inp_info; mcc_axis->out_info = mcs->pxms_out_info; pwm_chan = mcs->pxms_out_info; @@ -940,9 +1003,13 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode) mcc_axis->ccflg = 0; mcc_axis->pwm_dq = 0; + mcc_axis->steps_lim = mcc_axis->steps_cnt; + mcc_axis->steps_inc = 0; + mcc_axis->steps_pos = 0; - pxmcc_pxmc_ptofs2mcc(mcs, 1); - + if (mode != PXMCC_MODE_STEPPER) { + pxmcc_pxmc_ptofs2mcc(mcs, 1); + } return 0; } @@ -1840,6 +1907,8 @@ int pxmc_axis_rdmode(pxmc_state_t *mcs) return PXMC_AXIS_MODE_BLDC_PXMCC; if (mcs->pxms_do_out == pxmc_pxmcc_pwm2ph_out) return PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC; + if (mcs->pxms_do_out == pxmc_pxmcc_nofb2ph_out) + return PXMC_AXIS_MODE_STEPPER_PXMCC; return -1; } @@ -1856,7 +1925,8 @@ pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode) return -1; switch (mode) { - /*case PXMC_AXIS_MODE_STEPPER:*/ + /* case PXMC_AXIS_MODE_STEPPER: */ + case PXMC_AXIS_MODE_STEPPER_PXMCC: case PXMC_AXIS_MODE_STEPPER_WITH_IRC: case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC: res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0); @@ -1930,6 +2000,11 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode) if (res < 0) return -1; + if (mcs->pxms_do_inp == pxmc_pxmcc_nofb_inp) + mcs->pxms_do_inp = pxmc_inp_rocon_inp; + if (mcs->pxms_do_con == pxmc_pxmcc_nofb_con) + mcs->pxms_do_con = pxmc_pid_con; + switch (mode) { /*case PXMC_AXIS_MODE_STEPPER:*/ case PXMC_AXIS_MODE_STEPPER_WITH_IRC: @@ -1954,6 +2029,14 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode) pxmcc_axis_enable(mcs, 1); mcs->pxms_do_out = pxmc_pxmcc_pwm2ph_out; break; + case PXMC_AXIS_MODE_STEPPER_PXMCC: + if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER) < 0) + return -1; + pxmcc_axis_enable(mcs, 1); + mcs->pxms_do_inp = pxmc_pxmcc_nofb_inp; + mcs->pxms_do_con = pxmc_pxmcc_nofb_con; + mcs->pxms_do_out = pxmc_pxmcc_nofb2ph_out; + break; default: return -1; } diff --git a/sw/app/rocon/appl_tests.c b/sw/app/rocon/appl_tests.c index 69ac9eb..1f00f36 100644 --- a/sw/app/rocon/appl_tests.c +++ b/sw/app/rocon/appl_tests.c @@ -427,17 +427,17 @@ int cmd_do_testtumblefw(cmd_io_t *cmd_io, const struct cmd_des *des, char *param pxmcc_axis_enable(mcs, 1); if (0) { - mcc_data->axis[1].inp_info = 1; + mcc_data->axis[1].inp_info = 0; mcc_data->axis[1].out_info = 3; mcc_data->axis[1].pwmtx_info = (12 << 0) | (13 << 8) | (14 << 16); mcc_data->axis[1].mode = PXMCC_MODE_BLDC; mcc_data->axis[1].ccflg = 1; - mcc_data->axis[2].inp_info = 2; + mcc_data->axis[2].inp_info = 0; mcc_data->axis[2].out_info = 6; mcc_data->axis[2].pwmtx_info = (15 << 0) | (16 << 8) | (18 << 16); mcc_data->axis[2].mode = PXMCC_MODE_BLDC; mcc_data->axis[2].ccflg = 1; - mcc_data->axis[3].inp_info = 3; + mcc_data->axis[3].inp_info = 0; mcc_data->axis[3].out_info = 9; mcc_data->axis[3].pwmtx_info = (19 << 0) | (20 << 8) | (21 << 16); mcc_data->axis[3].mode = PXMCC_MODE_BLDC; -- 2.39.2