From 72d03eda66fd5ec531a068f2e19c13d7e5550d10 Mon Sep 17 00:00:00 2001
From: crinq <crinq@crinq.de>
Date: Wed, 9 Sep 2015 07:02:31 +0200
Subject: [PATCH] pmsm fix, curpid pwm res, mitsu params

---
 src/comps/curpid.comp | 8 ++++++++
 src/comps/pmsm.comp   | 8 ++++----
 src/link.c            | 4 ++--
 3 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/src/comps/curpid.comp b/src/comps/curpid.comp
index 85ab3742..1ee8dabe 100644
--- a/src/comps/curpid.comp
+++ b/src/comps/curpid.comp
@@ -29,6 +29,9 @@ HAL_PIN(kind) = 0.0;
 HAL_PIN(id_error) = 0.0;
 HAL_PIN(iq_error) = 0.0;
 
+HAL_PIN(res) = 1200.0;
+HAL_PIN(volt_res) = 0.0;
+
 MEM(float id_error_sum) = 0.0;
 MEM(float iq_error_sum) = 0.0;
 
@@ -37,6 +40,7 @@ RT(
    float rq = MAX(PIN(rq), 0.1);
    float ld = MAX(PIN(ld), 0.001);
    float lq = MAX(PIN(lq), 0.001);
+   float volt_res = PIN(volt) / MAX(PIN(res), 1.0);
    float volt = MAX(PIN(volt) * PIN(max_pwm) * 1.15, 0.1);
 
    float ff = PIN(ff);
@@ -70,8 +74,12 @@ RT(
    float ud = CLAMP(ff * rd * idc + kpd * id_error + kpd * kid * id_error_sum + kind * indd, -volt / 2.0, volt / 2.0);
    float uq = CLAMP(ff * rq * iqc + kpq * iq_error + kpq * kiq * iq_error_sum + kind * indq, -volt / 2.0, volt / 2.0);
 
+   ud = ((int)(ud / volt_res)) * volt_res;
+   uq = ((int)(uq / volt_res)) * volt_res;
+
    PIN(ud) = ud;
    PIN(uq) = uq;
+   PIN(volt_res) = volt_res;
 
    PIN(id_error) = id_error;
    PIN(iq_error) = iq_error;
diff --git a/src/comps/pmsm.comp b/src/comps/pmsm.comp
index 693ff157..bce769b3 100644
--- a/src/comps/pmsm.comp
+++ b/src/comps/pmsm.comp
@@ -41,16 +41,16 @@ RT(
    float indd = vel_e * psi_q;
    float indq = vel_e * psi_d;
 
-   id += (ud - r * id - indd) / ld * period / 4.0;
+   id += (ud - r * id + indd) / ld * period / 4.0;
    iq += (uq - r * iq - indq) / lq * period / 4.0;
 
-   id += (ud - r * id - indd) / ld * period / 4.0;
+   id += (ud - r * id + indd) / ld * period / 4.0;
    iq += (uq - r * iq - indq) / lq * period / 4.0;
 
-   id += (ud - r * id - indd) / ld * period / 4.0;
+   id += (ud - r * id + indd) / ld * period / 4.0;
    iq += (uq - r * iq - indq) / lq * period / 4.0;
 
-   id += (ud - r * id - indd) / ld * period / 4.0;
+   id += (ud - r * id + indd) / ld * period / 4.0;
    iq += (uq - r * iq - indq) / lq * period / 4.0;
 
    float t = 3.0 / 2.0 * p * (psi_m * iq + (ld - lq) * id * iq);
diff --git a/src/link.c b/src/link.c
index 0c3324ac..fd69d550 100644
--- a/src/link.c
+++ b/src/link.c
@@ -713,10 +713,10 @@ void set_mitsubishi(){ // TODO
    set_hal_pin("conf0.fb_rev", 0.0);
    set_hal_pin("conf0.out_rev", 0.0);
    set_hal_pin("conf0.fb_res", 16384.0);
-   set_hal_pin("conf0.r", 1.5);
+   set_hal_pin("conf0.r", 1.2); // 0.75, 1.2
    set_hal_pin("conf0.l", 0.0025);
    set_hal_pin("conf0.j", KGCM2(0.5));
-   set_hal_pin("conf0.psi", 0.34);
+   set_hal_pin("conf0.psi", 0.3); //0.3
 
    set_hal_pin("conf0.pos_p", 50.0);
    set_hal_pin("conf0.acc_p", 0.1);