RoCoN and TUMBL firmware: reimplemented control of stepper motor without feedback.

[fpga/lx-cpu1/lx-rocon.git] / hw / lx-rocon_firmware / firmware.c

diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c
index 2e1822dc99afe3a84636190e1e7ef2a4023255fc..a117a62e0fba18f8b3d9f0777a1ae8b05a5bfa14 100644 (file)
--- a/hw/lx-rocon_firmware/firmware.c
+++ b/hw/lx-rocon_firmware/firmware.c
@@ -2,7 +2,7 @@
   Components for embedded applications builded for
   laboratory and medical instruments firmware
 
-  firmware.c - multi axis motion controller comprocesor
+  firmware.c - multi axis motion controller coprocessor
                firmware for FPGA tumble CPU of lx-rocon system
 
   (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz
@@ -84,14 +84,16 @@ 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;
+  pxmcc_data.curadc[3].siroladc_offs = 0x0c66;
 
   pxmcc = pxmcc_data.axis;
   do {
     pxmcc->ccflg  = 0;
-    pxmcc->mode   = 1;
+    pxmcc->mode   = PXMCC_MODE_IDLE;
     pxmcc->ptirc  = 1000;
     pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
     pxmcc->pwm_dq = 0;
@@ -117,10 +119,16 @@ 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;
-      if ((uint32_t)pti >= per) {
+      /*
+       * the '>=' is appropriate to keep pti in <0;per-1> range,
+       * but sine and cosine computations can work over multiple
+       * periods and value of per=0xffffffff allows to control/stop
+       * updates if only '>' is used.
+       */
+      if ((uint32_t)pti > per) {
         if (pti < 0) {
           ofs -= per;
         } else {
@@ -162,7 +170,7 @@ void main(void)
         pwm_alp = pwm_d * pxmcc->ptcos - pwm_q * pxmcc->ptsin;
         pwm_bet = pwm_d * pxmcc->ptsin + pwm_q * pxmcc->ptcos;
 
-        if (!pxmcc->mode) {
+        if (pxmcc->mode == PXMCC_MODE_BLDC) {
           pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
 
          #ifndef SUPPRESS_CONDITIONALS
@@ -279,14 +287,14 @@ void main(void)
           *uptr = pwm1 | 0x4000;
          #endif
         } else {
-          uint32_t bet_sgn = pwm_bet >> 31;
           uint32_t alp_sgn = pwm_alp >> 31;
-          pwm1 = pwm2 = pwm_bet | 0x4000;
-          pwm3 = pwm4 = pwm_alp | 0x4000;
-          pwm1 &= ~bet_sgn;
-          pwm2 &= bet_sgn;
-          pwm3 &= ~alp_sgn;
-          pwm4 &= alp_sgn;
+          uint32_t bet_sgn = pwm_bet >> 31;
+          pwm_alp >>= 16;
+          pwm_bet >>= 16;
+          pwm1 = -pwm_alp & alp_sgn;
+          pwm2 = pwm_alp & ~alp_sgn;
+          pwm3 = -pwm_bet & bet_sgn;
+          pwm4 = pwm_bet & ~bet_sgn;
 
           pwmtx_info = pxmcc->pwmtx_info;
           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  0) & 0xff);
@@ -301,9 +309,18 @@ 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_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;
+            }
+          }
         }
       } else {
-        if (!pxmcc->mode) {
+        if (pxmcc->mode == PXMCC_MODE_BLDC) {
           pwmtx_info = pxmcc->pwmtx_info;
           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  0) & 0xff);
           pxmcc->pwm_prew[1] = *uptr & 0x3fff;
@@ -392,7 +409,7 @@ void main(void)
      #endif /*COMPUTE_PHASE_SECTOR*/
 
       out_info = pxmcc->out_info;
-      if (!pxmcc->mode) {
+      if (pxmcc->mode == PXMCC_MODE_BLDC) {
 
         pwm1 = pxmcc->pwm_prew[0];
         pwm2 = pxmcc->pwm_prew[1];
@@ -534,14 +551,14 @@ void main(void)
         cur_bet *= CONST16_K3;
         cur_bet >>= 16;
       } else {
-        int32_t bet_pwm = pxmcc->pwm_prew[1];
+        int32_t bet_pwm = pxmcc->pwm_prew[2];
         uint32_t bet_sgn = (bet_pwm - 1) >> 31;
-        int32_t alp_pwm = pxmcc->pwm_prew[3];
+        int32_t alp_pwm = pxmcc->pwm_prew[0];
         uint32_t alp_sgn = (alp_pwm - 1) >> 31;
-        cur_bet = (pxmcc_data.curadc[out_info + 0].cur_val & ~bet_sgn) -
-                  (pxmcc_data.curadc[out_info + 1].cur_val & bet_sgn);
-        cur_alp = (pxmcc_data.curadc[out_info + 2].cur_val & ~alp_sgn) -
-                  (pxmcc_data.curadc[out_info + 3].cur_val & alp_sgn);
+        cur_bet = (pxmcc_data.curadc[out_info + 3].cur_val & ~bet_sgn) -
+                  (pxmcc_data.curadc[out_info + 2].cur_val & bet_sgn);
+        cur_alp = (pxmcc_data.curadc[out_info + 1].cur_val & ~alp_sgn) -
+                  (pxmcc_data.curadc[out_info + 0].cur_val & alp_sgn);
       }
 
       cur_d =  cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;