From 0b97a3f9dc74b4ccf4e8c8d10fbec116d89cf709 Mon Sep 17 00:00:00 2001
From: Rene Hopf <renehopf@mac.com>
Date: Wed, 9 Sep 2015 19:26:27 +0200
Subject: [PATCH] 1vpp formeln

---
 src/comps/adc.comp    | 42 ++++++++++++++++++++++--------------------
 src/comps/enc_fb.comp |  7 ++++---
 src/main.c            |  8 ++++----
 3 files changed, 30 insertions(+), 27 deletions(-)

diff --git a/src/comps/adc.comp b/src/comps/adc.comp
index b8bfe6ee..badb2db7 100644
--- a/src/comps/adc.comp
+++ b/src/comps/adc.comp
@@ -1,5 +1,17 @@
 COMP(adc);
 
+#define AREF 3.3// analog reference voltage
+#define ARES 4096.0// analog resolution, 12 bit
+
+#define R15 1000.0
+#define R22 3900.0
+#define R21 180.0
+#define R19 470.0
+#define V_REF 5.0
+#define INPUT_REF (V_REF * R21 / (R19 + R21))
+#define INPUT_GAIN (R22 / R15 * R21 / (R19 + R21))
+#define V_DIFF(ADC) ((ADC/ADC_ANZ/ARES*AREF-INPUT_REF)/INPUT_GAIN)
+
 HAL_PIN(sin) = 0.0;
 HAL_PIN(cos) = 0.0;
 HAL_PIN(sin3) = 0.0;
@@ -14,8 +26,8 @@ HAL_PIN(sin_offset) = 0.0;
 HAL_PIN(cos_offset) = 0.0;
 
 RT(
-  uint32_t si[PID_WAVES];
-  uint32_t co[PID_WAVES];
+  float si[PID_WAVES];
+  float co[PID_WAVES];
   uint32_t sc[PID_WAVES];
 
   float s_o = PIN(sin_offset);
@@ -23,9 +35,6 @@ RT(
   float s_g = PIN(sin_gain);
   float c_g = PIN(cos_gain);
 
-  float s = 0.0;
-  float c = 0.0;
-
   for(int i = 0; i < PID_WAVES; i++){
     sc[i] = 0.0;
     for(int j = 0; j < ADC_ANZ; j++){
@@ -40,29 +49,22 @@ RT(
     si[i] = 0.0;
     co[i] = 0.0;
 
-    si[i] = sc[i] & 0x0000ffff;
-    co[i] = sc[i] >> 16;
+    si[i] = s_g * V_DIFF((sc[i] & 0x0000ffff)) + s_o;
+    co[i] = c_g * V_DIFF((sc[i] >> 16)) + c_o;
   }
 
-  s = s_g * (si[3] + s_o);
-  c = c_g * (co[3] + c_o);
-
-  PIN(sin3) = s;
-  PIN(cos3) = c;
+  PIN(sin3) = si[3];
+  PIN(cos3) = co[3];
 
   if(PIN(res_en) > 0.0){
-    s = s_g * (0.5 * si[3] - 0.25 * si[2] + 0.125 * si[1] - 0.125 * si[0] + 0.25 * s_o);
-    c = c_g * (0.5 * co[3] - 0.25 * co[2] + 0.125 * co[1] - 0.125 * co[0] + 0.25 * c_o);
+    PIN(sin) = 0.5 * si[3] - 0.25 * si[2] + 0.125 * si[1] - 0.125 * si[0] + 0.25;
+    PIN(cos) = 0.5 * co[3] - 0.25 * co[2] + 0.125 * co[1] - 0.125 * co[0] + 0.25;
   }
   else{
-    s = s_g * (0.5 * si[3] + 0.25 * si[2] + 0.125 * si[1] + 0.125 * si[0] + s_o);
-    c = c_g * (0.5 * co[3] + 0.25 * co[2] + 0.125 * co[1] + 0.125 * co[0] + c_o);
+    PIN(sin) = 0.5 * si[3] + 0.25 * si[2] + 0.125 * si[1] + 0.125 * si[0];
+    PIN(cos) = 0.5 * co[3] + 0.25 * co[2] + 0.125 * co[1] + 0.125 * co[0];
   }
 
-  PIN(sin) = s;
-  PIN(cos) = c;
-
-
 );
 
 ENDCOMP;
diff --git a/src/comps/enc_fb.comp b/src/comps/enc_fb.comp
index be5d7646..7a27b63d 100644
--- a/src/comps/enc_fb.comp
+++ b/src/comps/enc_fb.comp
@@ -74,13 +74,14 @@ RT(
 
   float s = PIN(sin);
   float c = PIN(cos);
-  float a = s * s + c * c;
-
+  float a = sqrtf(s * s + c * c);
 
+  PIN(amp) = a;
+  
   p = mod(TIM_GetCounter(ENC1_TIM) * 2.0f * M_PI / (float)e_res);
   PIN(pos) = p;
 
-  if(a < 0.3 * 0.3){
+  if(a < 0.15){
     PIN(error) = 1.0;
   }
   else{
diff --git a/src/main.c b/src/main.c
index 3a35cc88..65dffaef 100644
--- a/src/main.c
+++ b/src/main.c
@@ -206,10 +206,10 @@ int main(void)
    HAL_PIN(out_rev) = 0.0;
    HAL_PIN(fb_res) = 1.0;
    HAL_PIN(cmd_res) = 2000.0;
-   HAL_PIN(sin_offset) = -17600.0;
-   HAL_PIN(cos_offset) = -17661.0;
-   HAL_PIN(sin_gain) = 0.0001515;
-   HAL_PIN(cos_gain) = 0.00015;
+   HAL_PIN(sin_offset) = 0.0;
+   HAL_PIN(cos_offset) = 0.0;
+   HAL_PIN(sin_gain) = 1.0;
+   HAL_PIN(cos_gain) = 1.0;
 
    HAL_PIN(max_dc_volt) = 370.0;
    HAL_PIN(max_hv_temp) = 90.0;