Revision 34:51647c6c500d, committed 2017-11-08

Comitter:

benkatz

Date:

Wed Nov 08 15:18:18 2017 +0000

Parent:

33:3ab3cce8b44d

Child:

35:69b24894c11d

Commit message:

testing new things

Changed in this revision

--- a/Config/current_controller_config.h	Wed Aug 30 18:10:27 2017 +0000
+++ b/Config/current_controller_config.h	Wed Nov 08 15:18:18 2017 +0000
@@ -3,9 +3,9 @@
 
 #define K_D .05f                     // Volts/Amp
 #define K_Q .05f                     // Volts/Amp
-#define K_SCALE 0.0001f            // K_loop/Loop BW (Hz)
-#define KI_D 0.06f                  // 1/samples
-#define KI_Q 0.06f                  // 1/samples
+#define K_SCALE 0.00043f            // K_loop/Loop BW (Hz)
+#define KI_D 0.0255f                  // 1/samples
+#define KI_Q 0.0255f                  // 1/samples
 #define V_BUS 24.0f                 // Volts
 
 #define D_INT_LIM V_BUS/(K_D*KI_D)  // Amps*samples

--- a/Config/motor_config.h	Wed Aug 30 18:10:27 2017 +0000
+++ b/Config/motor_config.h	Wed Nov 08 15:18:18 2017 +0000
@@ -4,7 +4,7 @@
 #define R_PHASE 0.105f          //Ohms
 #define L_D 0.00003f            //Henries
 #define L_Q 0.00003f            //Henries
-#define KT .07f                 //N-m per peak phase amp (= RMS amps/1.5)
+#define KT .075f                 //N-m per peak phase amp (= RMS amps/1.5)
 #define NPP 21                  //Number of pole pairs
 #define GR 6.0f                 //Gear ratio
 #define KT_OUT 0.45f            //Effective output torque constatnt

--- a/FOC/foc.cpp	Wed Aug 30 18:10:27 2017 +0000
+++ b/FOC/foc.cpp	Wed Nov 08 15:18:18 2017 +0000
@@ -91,12 +91,13 @@
        //dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
        controller->i_d = 0.6666667f*(c*controller->i_a + (0.86602540378f*s-.5f*c)*controller->i_b + (-0.86602540378f*s-.5f*c)*controller->i_c);   ///Faster DQ0 Transform
        controller->i_q = 0.6666667f*(-s*controller->i_a - (-0.86602540378f*c-.5f*s)*controller->i_b - (0.86602540378f*c-.5f*s)*controller->i_c);
-
-       //float cogging_current = 0.05f*s*controller->i_q_ref;
+        controller->i_q_filt = .95f*controller->i_q_filt + .05f*controller->i_q;
+        float s_cog = sinf(12.0f*theta);
+       float cogging_current =-0.33f*s_cog + .25f*s;
        
        /// PI Controller ///
        float i_d_error = controller->i_d_ref - controller->i_d;
-       float i_q_error = controller->i_q_ref - controller->i_q;// + cogging_current;
+       float i_q_error = controller->i_q_ref - controller->i_q + cogging_current - 2.0f;
        float v_d_ff = 2.0f*(2*controller->i_d_ref*R_PHASE);   //feed-forward voltage
        float v_q_ff =  controller->dtheta_elec*WB*1.73205081f;
        controller->d_int += i_d_error;   
@@ -109,7 +110,7 @@
        controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*KI_D*controller->d_int;// + v_d_ff;  
        controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*KI_Q*controller->q_int;// + v_q_ff; 
        
-       //controller->v_q = 4.0f;;
+       //controller->v_q = 4.0f;
        //controller->v_d = 0.0f;
        
        //controller->v_d = v_d_ff;

--- a/PositionSensor/PositionSensor.cpp	Wed Aug 30 18:10:27 2017 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Nov 08 15:18:18 2017 +0000
@@ -72,6 +72,7 @@
     velVec[0] = vel;
     MechVelocity =  sum/(float)n;
     ElecVelocity = MechVelocity*_ppairs;
+    ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
     }
 
 int PositionSensorAM5147::GetRawPosition(){

--- a/PositionSensor/PositionSensor.h	Wed Aug 30 18:10:27 2017 +0000
+++ b/PositionSensor/PositionSensor.h	Wed Nov 08 15:18:18 2017 +0000
@@ -53,7 +53,7 @@
     virtual int GetCPR(void);
     virtual void WriteLUT(int new_lut[128]);
 private:
-    float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[16], MechVelocity, ElecVelocity;
+    float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[16], MechVelocity, ElecVelocity, ElecVelocityFilt;
     int raw, _CPR, rotations, old_counts, _ppairs;
     SPI *spi;
     DigitalOut *cs;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Tables/imax_vs_w.h	Wed Nov 08 15:18:18 2017 +0000
@@ -0,0 +1,9 @@
+#ifndef IMAX_VS_W
+#define IMAX_VS_W
+
+/*max achievable magnitude current vs speed, for positive and negative works
+Speed ranges from 0 to 220 rad/s
+*/
+
+float imax_pw[16] = {40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 36.0f, 32.0f, 28.0f, 24.0f, 21.0f, 17.0f, 14.0f, 11.0f, 7.0f, 4.0f, 2.0f};
+float imax_nw[16] = {40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f, 40.0f};
\ No newline at end of file

--- a/main.cpp	Wed Aug 30 18:10:27 2017 +0000
+++ b/main.cpp	Wed Nov 08 15:18:18 2017 +0000
@@ -16,7 +16,6 @@
 float __float_reg[64];                                                          // Floats stored in flash
 int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
 
-
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "structs.h"
@@ -83,16 +82,16 @@
 /// 2: [velocity[11-4]]
 /// 3: [velocity[3-0], current[11-8]]
 /// 4: [current[7-0]]
-void pack_reply(CANMessage *msg, float p, float v, float i){
+void pack_reply(CANMessage *msg, float p, float v, float t){
     int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
     int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
-    int i_int = float_to_uint(i, -I_MAX, I_MAX, 12);
+    int t_int = float_to_uint(t, -T_MAX, T_MAX, 12);
     msg->data[0] = CAN_ID;
     msg->data[1] = p_int>>8;
     msg->data[2] = p_int&0xFF;
     msg->data[3] = v_int>>4;
-    msg->data[4] = ((v_int&0xF)<<4) + (i_int>>8);
-    msg->data[5] = i_int&0xFF;
+    msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
+    msg->data[5] = t_int&0xFF;
     }
     
 /// CAN Command Packet Structure ///
@@ -153,7 +152,7 @@
             }
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
-            pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q);
+            pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
             can.write(txMsg);
             }
         }
@@ -266,16 +265,16 @@
                     controller.i_d_ref = 0;
                     controller.i_q_ref = 0;
                     } 
-                //controller.i_q_ref = .5; 
+                //controller.i_q_ref = .4f;
                 commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
                 spi.Sample();                                                   // Sample position sensor
                 //toggle.write(0);
                 controller.timeout += 1;
                 
-                if(count == 1){
+                if(count == 1000){
                      count = 0;
                      //wait(.001);
-                    //printf(" Started commutating\n\r");
+                    //printf("%f\n\r", controller.theta_elec);
                      }
                      }
                      

--- a/math_ops.cpp	Wed Aug 30 18:10:27 2017 +0000
+++ b/math_ops.cpp	Wed Nov 08 15:18:18 2017 +0000
@@ -21,6 +21,12 @@
     /// Returns minimum of x, y, z ///
     return (x < y ? (x < z ? x : z) : (y < z ? y : z));
     }
+
+float roundf(float x){
+    /// Returns nearest integer ///
+    
+    return x < 0.0f ? ceilf(x - 0.5f) : floorf(x + 0.5f);
+    }
     
 void limit_norm(float *x, float *y, float limit){
     /// Scales the lenght of vector (x, y) to be <= limit ///

--- a/math_ops.h	Wed Aug 30 18:10:27 2017 +0000
+++ b/math_ops.h	Wed Nov 08 15:18:18 2017 +0000
@@ -9,6 +9,7 @@
 float fminf(float x, float y);
 float fmaxf3(float x, float y, float z);
 float fminf3(float x, float y, float z);
+float roundf(float x);
 void limit_norm(float *x, float *y, float limit);
 int float_to_uint(float x, float x_min, float x_max, int bits);
 float uint_to_float(int x_int, float x_min, float x_max, int bits);

--- a/structs.h	Wed Aug 30 18:10:27 2017 +0000
+++ b/structs.h	Wed Nov 08 15:18:18 2017 +0000
@@ -20,8 +20,8 @@
     float i_a, i_b, i_c;
     float v_bus;
     float theta_mech, theta_elec;
-    float dtheta_mech, dtheta_elec;
-    float i_d, i_q;
+    float dtheta_mech, dtheta_elec, dtheta_elec_filt;
+    float i_d, i_q, i_q_filt;
     float v_d, v_q;
     float dtc_u, dtc_v, dtc_w;
     float v_u, v_v, v_w;