From: Pavel Pisa Date: Thu, 15 Jan 2015 18:58:02 +0000 (+0100) Subject: RoCoN and TUMBL firmware: reimplemented control of stepper motor without feedback. X-Git-Url: http://rtime.felk.cvut.cz/gitweb/fpga/lx-cpu1/lx-rocon.git/commitdiff_plain/af9a5829910669517ef953bf8988c35851ed76b0?hp=847043ee2041db202831013c1a4aa6dfbdeb1fdf RoCoN and TUMBL firmware: reimplemented control of stepper motor without feedback. This implementation already keeps position and is suitable for real applications. Signed-off-by: Pavel Pisa --- diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c index a139a3c..a117a62 100644 --- a/hw/lx-rocon_firmware/firmware.c +++ b/hw/lx-rocon_firmware/firmware.c @@ -310,9 +310,12 @@ void main(void) 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++; + if ((pxmcc->steps_sqn_next ^ last_rx_done_sqn) & 0x1f) { pxmcc->steps_pos += pxmcc->steps_inc; + } else { + pxmcc->steps_pos = pxmcc->steps_pos_next; + pxmcc->steps_inc = pxmcc->steps_inc_next; + pxmcc->steps_inc_next = 0; } } } diff --git a/hw/lx-rocon_firmware/pxmcc_types.h b/hw/lx-rocon_firmware/pxmcc_types.h index 7228705..afad06c 100644 --- a/hw/lx-rocon_firmware/pxmcc_types.h +++ b/hw/lx-rocon_firmware/pxmcc_types.h @@ -56,10 +56,11 @@ 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 */ + uint32_t steps_sqn_next; /* when to apply steps_inc_next */ + uint32_t steps_inc_next; /* increment to apply at steps_sqn_next */ + uint32_t steps_pos_next; /* base position to apply at steps_sqn_next */ } 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 17f6e2b..fdd9ef9 100644 --- a/sw/app/rocon/appl_pxmc.c +++ b/sw/app/rocon/appl_pxmc.c @@ -106,6 +106,35 @@ void pxmc_rocon_vin_compute(void) pxmc_rocon_vin_act = vin_act; } +unsigned int pxmc_rocon_rx_done_sqn; +unsigned int pxmc_rocon_rx_done_sqn_inc; +unsigned int pxmc_rocon_rx_done_sqn_misscnt; +unsigned int pxmc_rocon_rx_done_sqn_missoffs; + +static inline +void pxmc_rocon_rx_done_sqn_compute(void) +{ + uint32_t sqn_act; + uint32_t sqn_offs; + unsigned int sqn_expect = pxmc_rocon_rx_done_sqn + pxmc_rocon_rx_done_sqn_inc; + + sqn_act = *fpga_lx_master_receiver_done_div; + sqn_offs = (sqn_act - sqn_expect) & 0x1f; + if (sqn_offs) { + if (pxmc_rocon_rx_done_sqn_missoffs != sqn_offs) { + pxmc_rocon_rx_done_sqn_misscnt = 1; + } else { + pxmc_rocon_rx_done_sqn_misscnt++; + if (pxmc_rocon_rx_done_sqn_misscnt >= 10) + sqn_expect += 1 - ((sqn_offs >> 3) & 2); + } + } else { + pxmc_rocon_rx_done_sqn_misscnt = 0; + } + pxmc_rocon_rx_done_sqn = sqn_expect; + pxmc_rocon_rx_done_sqn_missoffs = sqn_offs; +} + 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, @@ -1023,12 +1052,16 @@ pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs) } /** - * pxmc_pxmcc_nofb_inp - Dummy input for direct stepper motor control + * pxmc_pxmcc_nofb2ph_inp - Dummy input for direct stepper motor control * @mcs: Motion controller state information */ int -pxmc_pxmcc_nofb_inp(pxmc_state_t *mcs) +pxmc_pxmcc_nofb2ph_inp(pxmc_state_t *mcs) { + volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs); + uint32_t steps_pos_act = mcc_axis->steps_pos; + mcs->pxms_as = (int32_t)(steps_pos_act - mcs->pxms_ap); + mcs->pxms_ap += mcs->pxms_as; return 0; } @@ -1058,6 +1091,7 @@ pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs) pxmc_rocon_pwm2ph_wr(mcs, 0, 0); pxmcc_axis_pwm_dq_out(mcs, 0, 0); mcc_axis->steps_inc = 0; + mcc_axis->steps_inc_next = 0; mcsrc->cur_d_err_sum = 0; mcsrc->cur_q_err_sum = 0; } else { @@ -1067,6 +1101,7 @@ pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs) int cur_q_req, cur_q_err; int max_pwm = mcs->pxms_me; int32_t stpinc; + int32_t stpdiv; pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw); pxmcc_axis_cur_dq_raw2filt(&cur_q, cur_q_raw); @@ -1092,15 +1127,16 @@ pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs) pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q); - mcs->pxms_ap=mcs->pxms_rp; - mcs->pxms_as=mcs->pxms_rs; - mcc_axis->steps_lim = mcc_axis->steps_cnt + 6; + stpinc = mcs->pxms_rp - mcsrc->steps_pos_prev; + + stpdiv = pxmc_rocon_rx_done_sqn_inc; + mcc_axis->steps_inc_next = (stpinc + stpdiv / 2) / stpdiv; + mcc_axis->steps_pos_next = mcsrc->steps_pos_prev; - stpinc = mcs->pxms_rs; - mcc_axis->steps_inc = stpinc; + mcsrc->steps_pos_prev = mcs->pxms_rp; - /* stpinc /= (mcs->pxms_ptirc << PXMC_SUBDIV(mcs)); */ - /* pxms_ptscale_mult; pxms_ptscale_shift; */ + mcc_axis->steps_sqn_next = pxmc_rocon_rx_done_sqn + + pxmc_rocon_rx_done_sqn_inc - 1; } return 0; @@ -1110,6 +1146,7 @@ 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); + pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs); uint32_t ptirc; uint32_t ptreci; uint32_t inp_info; @@ -1132,6 +1169,9 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode) mcc_data->common.pwm_cycle = PXMC_LXPWR_PWM_CYCLE; ptirc = mcs->pxms_ptirc; + if (mode == PXMCC_MODE_STEPPER) + ptirc <<= PXMC_SUBDIV(mcs); + ull = (1ULL << 32) * mcs->pxms_ptper; ptreci = (ull + ptirc / 2) / ptirc; @@ -1154,7 +1194,6 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode) break; case PXMCC_MODE_STEPPER: phcnt = 4; - mcc_axis->ptreci = 1; inp_info = (char*)&mcc_axis->steps_pos - (char*)mcc_data; break; } @@ -1187,11 +1226,16 @@ 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; - if (mode != PXMCC_MODE_STEPPER) { + if (mode == PXMCC_MODE_STEPPER) { + mcsrc->steps_pos_prev = mcs->pxms_rp = mcs->pxms_ap; + mcs->pxms_rs = mcs->pxms_as = 0; + mcc_axis->steps_inc_next = 0; + mcc_axis->steps_pos_next = mcsrc->steps_pos_prev; + mcc_axis->steps_inc = 0; + mcc_axis->steps_pos = mcsrc->steps_pos_prev; + mcc_axis->ptirc = mcs->pxms_ptirc << PXMC_SUBDIV(mcs); + } else { pxmcc_pxmc_ptofs2mcc(mcs, 1); } return 0; @@ -1226,6 +1270,7 @@ IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr) hal_gpio_set_value(T2MAT1_PIN, 0); hal_gpio_set_value(T2MAT0_PIN, 0); + pxmc_rocon_rx_done_sqn_compute(); pxmc_rocon_vin_compute(); if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end) @@ -1337,6 +1382,7 @@ pxmc_rocon_pwm_master_init(void) *fpga_lx_master_transmitter_reg = 0; *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */ *fpga_lx_master_receiver_done_div = receiver_done_div << 8; + pxmc_rocon_rx_done_sqn_inc = receiver_done_div; for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++) fpga_lx_master_receiver_base[i] = 0; @@ -2200,7 +2246,7 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode) if (res < 0) return -1; - if (mcs->pxms_do_inp == pxmc_pxmcc_nofb_inp) + if (mcs->pxms_do_inp == pxmc_pxmcc_nofb2ph_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; @@ -2233,7 +2279,7 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode) 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_inp = pxmc_pxmcc_nofb2ph_inp; mcs->pxms_do_con = pxmc_pxmcc_nofb_con; mcs->pxms_do_out = pxmc_pxmcc_nofb2ph_out; break; diff --git a/sw/app/rocon/appl_pxmc.h b/sw/app/rocon/appl_pxmc.h index 9c35175..17ce6ce 100644 --- a/sw/app/rocon/appl_pxmc.h +++ b/sw/app/rocon/appl_pxmc.h @@ -23,6 +23,7 @@ typedef struct pxmc_rocon_state_t { pxmc_state_t base; + uint32_t steps_pos_prev; uint32_t cur_d_cum_prev; uint32_t cur_q_cum_prev; int32_t cur_d_err_sum;