From eb79172aa1745190fecf603e7d383f8c275ccede Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@rapyuta-robotics.com>
Date: Sun, 18 Dec 2016 23:51:45 +0900
Subject: [PATCH] change pll object structure

---
 MotorControl/low_level.c | 28 ++++++++++++++++------------
 MotorControl/low_level.h |  2 ++
 2 files changed, 18 insertions(+), 12 deletions(-)

diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c
index 8978d798..613db740 100644
--- a/MotorControl/low_level.c
+++ b/MotorControl/low_level.c
@@ -60,9 +60,11 @@ Motor_t motors[] = {
             .encoder_timer = &htim3,
             .encoder_offset = 0,
             .encoder_state = 0,
-            .phase = 0.0f,
-            .pll_pos = 0.0f,
-            .pll_vel = 0.0f
+            .phase = 0.0f, // [rad]
+            .pll_pos = 0.0f, // [rad]
+            .pll_vel = 0.0f, // [rad/s]
+            .pll_kp = 0.0f, // [rad/s / rad]
+            .pll_ki = 0.0f // [(rad/s^2) / rad]
         }
     },
     {   //M1
@@ -95,8 +97,10 @@ Motor_t motors[] = {
             .encoder_offset = 0,
             .encoder_state = 0,
             .phase = 0.0f,
-            .pll_pos = 0.0f,
-            .pll_vel = 0.0f
+            .pll_pos = 0.0f, // [rad]
+            .pll_vel = 0.0f, // [rad/s]
+            .pll_kp = 0.0f, // [rad/s / rad]
+            .pll_ki = 0.0f // [(rad/s^2) / rad]
         }
     }
 };
@@ -606,6 +610,10 @@ static void scan_motor(Motor_t* motor, float omega, float voltage_magnitude) {
     }
 }
 
+static void update_pll(Motor_t* motor) {
+
+}
+
 static void update_enc(Motor_t* motor) {
     //@TODO stick parameter into struct
     static const float rad_per_enc = 7.0 * 2 * M_PI * (1.0f / (float)(600 * 4));
@@ -632,10 +640,6 @@ static void FOC_voltage(Motor_t* motor, float v_d, float v_q) {
     }
 }
 
-static void update_pll(Motor_t* motor) {
-
-}
-
 static void FOC_current(Motor_t* motor, float Id_des, float Iq_des) {
     Current_control_t* ictrl = &motor->current_control;
 
@@ -707,14 +711,14 @@ void motor_thread(void const * argument) {
         FOC_voltage(motor, 0.0f, 0.0f);
     }
 
-    float current_control_bandwidth = 500.0f; // [rad/s]
+    float current_control_bandwidth = 2000.0f; // [rad/s]
     motor->current_control.p_gain = current_control_bandwidth * L;
     float plant_pole = R/L;
     motor->current_control.i_gain = plant_pole * motor->current_control.p_gain;
 
     // scan_motor(motor, 50.0f, test_current * R);
-    FOC_voltage(motor, 0.0f, 0.8f);
-    // FOC_current(motor, 0.0f, 0.0f);
+    // FOC_voltage(motor, 0.0f, 0.8f);
+    FOC_current(motor, 0.0f, 0.0f);
 
     //De-energize motor
     queue_voltage_timings(motor, 0.0f, 0.0f);
diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h
index bb9d4b9b..6d034644 100644
--- a/MotorControl/low_level.h
+++ b/MotorControl/low_level.h
@@ -27,6 +27,8 @@ typedef struct {
     float phase;
     float pll_pos;
     float pll_vel;
+    float pll_kp;
+    float pll_ki;
 } Rotor_t;
 
 typedef struct {