From 387e924ab591ad5ba8fe4493de85fbb00dbfdce8 Mon Sep 17 00:00:00 2001
From: crinq <crinq@crinq.de>
Date: Sun, 25 Mar 2018 07:23:26 +0200
Subject: [PATCH] fanuc abs pos fix

---
 conf/fanuc_b05-6000.txt     | 14 ++++++++++++++
 conf/template/com_test.txt  |  9 +++++++++
 conf/template/fanuc_fb0.txt |  3 ++-
 src/comps/encf.c            | 21 +++++++++++++++++----
 4 files changed, 42 insertions(+), 5 deletions(-)
 create mode 100644 conf/fanuc_b05-6000.txt
 create mode 100644 conf/template/com_test.txt

diff --git a/conf/fanuc_b05-6000.txt b/conf/fanuc_b05-6000.txt
new file mode 100644
index 00000000..3e9f5f96
--- /dev/null
+++ b/conf/fanuc_b05-6000.txt
@@ -0,0 +1,14 @@
+link pid
+link pmsm
+link misc
+link fanuc_fb0
+link jog_cmd
+conf0.r = 1
+conf0.l = 0.005
+conf0.j = 0.000025
+conf0.polecount = 4
+conf0.max_ac_cur = 18
+conf0.max_vel = 250
+conf0.max_acc = 20000
+conf0.max_force = 20
+conf0.cur_i = 0.0001
diff --git a/conf/template/com_test.txt b/conf/template/com_test.txt
new file mode 100644
index 00000000..17482e88
--- /dev/null
+++ b/conf/template/com_test.txt
@@ -0,0 +1,9 @@
+fb_switch0.en = 1
+term0.wave0 = sim0.vel
+term0.wave1 = fb_switch0.com_pos
+term0.wave2 = fb_switch0.mot_abs_pos
+term0.wave3 = fb_switch0.com_fb
+term0.gain2 = 20.000000
+hv0.pos = sim0.vel
+hv0.d_cmd = 1.000000
+hv0.en = 1.000000
\ No newline at end of file
diff --git a/conf/template/fanuc_fb0.txt b/conf/template/fanuc_fb0.txt
index b68563b6..e4ac98e6 100644
--- a/conf/template/fanuc_fb0.txt
+++ b/conf/template/fanuc_fb0.txt
@@ -6,7 +6,8 @@ fb_switch0.com_state = 3
 fb_switch0.com_polecount = conf0.com_fb_polecount
 conf0.com_fb_polecount = conf0.polecount
 fb_switch0.mot_pos = encf0.pos
-fb_switch0.mot_state = 1
+fb_switch0.mot_abs_pos = encf0.abs_pos
+fb_switch0.mot_state = encf0.state
 fault0.mot_fb_error = encf0.error
 fb_switch0.mot_polecount = 1
 conf0.mot_fb_res = 1048576
diff --git a/src/comps/encf.c b/src/comps/encf.c
index 31971c5d..01b6443d 100644
--- a/src/comps/encf.c
+++ b/src/comps/encf.c
@@ -18,6 +18,7 @@ HAL_PIN(state);
 HAL_PIN(turns);
 HAL_PIN(com_pos);
 HAL_PIN(index);
+HAL_PIN(req_len);
 
 volatile uint16_t tim_data[160];
 
@@ -46,10 +47,11 @@ union {
 } data;
 
 int32_t pos_offset;
+uint32_t state_counter;
 
 static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
   // struct encf_ctx_t *ctx = (struct encf_ctx_t *)ctx_ptr;
-  // struct encf_pin_ctx_t * pins = (struct encf_pin_ctx_t *)pin_ptr;
+  struct encf_pin_ctx_t * pins = (struct encf_pin_ctx_t *)pin_ptr;
   GPIO_InitTypeDef GPIO_InitStruct;
 
   //TX enable
@@ -125,6 +127,8 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
   GPIO_SetBits(GPIOD, GPIO_Pin_15);  //tx enable
 
   pos_offset = 0;
+  PIN(req_len) = 2046;
+  state_counter = 0;
 }
 
 static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
@@ -158,14 +162,21 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
     pos = data.fanuc.pos_lo + (data.fanuc.pos_hi << 6);
     PIN(index) = data.fanuc.no_index;
 
-    PIN(abs_pos) = ((float)pos * 2.0 * M_PI / (1<<22)) - M_PI;
+    PIN(abs_pos) = mod((float)pos * 2.0 * M_PI / (1<<22));
 
     if(PIN(index) > 0.0){
       pos_offset = pos;
       PIN(pos) = PIN(abs_pos);
       PIN(state) = 1;
+      state_counter = 1;
+    }
+    else if(state_counter == 1){
+      state_counter = 2;
+      pos_offset = pos;
+      PIN(pos) = PIN(abs_pos);
     }
     else{
+      state_counter = 3;
       PIN(pos) = mod((float)(pos + pos_offset) * 2.0 * M_PI / (1<<22));
       PIN(state) = 3;
     }
@@ -173,10 +184,12 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
     
     PIN(turns) = data.fanuc.turns;
     pos = data.fanuc.com_pos;
-    PIN(com_pos) = (pos * 2.0 * M_PI / 1024) - M_PI;
+    PIN(com_pos) = mod(pos * 2.0 * M_PI / 1024);
     PIN(error) = 0;
   }else{
     PIN(error) = 1;
+    PIN(state) = 1;
+    state_counter = 0;
   }
   //reset timer
   FB0_ENC_TIM->CNT = 0;
@@ -184,7 +197,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
   FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim
 
   //send request, 1/(42e6/32)*11 = 8.4uS
-  SPI3->DR = 0xffe;
+  SPI3->DR = PIN(req_len);
   //start DMA
   DMA_Cmd(DMA1_Stream0, DISABLE);
   DMA_ClearFlag(DMA1_Stream0, DMA_FLAG_TCIF0);