RoCoN and TUMBL firmware: initial experimental support of stepper motor without feedback.

Signed-off-by: Pavel Pisa <ppisa@pikron.com>

diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c
index 8a21bbcb3a63b1b06a07189ab7a36d2f6cb216ce..a139a3c16ff4bda7ca453bba247cd982c51299b5 100644 (file)
--- 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 c09a18da03a32d56ad257c8246b1b6b1539cfa69..722870598b4795894f055d26baea01408e95e490 100644 (file)
--- 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 6954b0b39a1eaa7f9f300ee371061e249b79dd01..ff05a752eb9f94ef066c7bb560fa7182d1189bec 100644 (file)
--- 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 69ac9eb1d3293cee9dc034daaa0fe57f6e68ca27..1f00f3643fbbbf1d88026e4c0823f2106e75dcef 100644 (file)
--- 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;