bldc driver firmware based on hobbyking cheetah compact

Dependencies:   BLDC_V2 mbed-dev-f303 FastPWM3

Dependents:   BLDC_V2

Files at this revision

API Documentation at this revision

Comitter:
Wooden
Date:
Wed Apr 07 10:12:43 2021 +0000
Parent:
47:f4ecf3e0576a
Commit message:
wooden_bldc_test

Changed in this revision

BLDC_V2.lib Show annotated file Show diff for this revision Revisions of this file
Calibration/calibration.cpp Show annotated file Show diff for this revision Revisions of this file
Config/current_controller_config.h Show annotated file Show diff for this revision Revisions of this file
Config/hw_config.h Show annotated file Show diff for this revision Revisions of this file
Config/motor_config.h Show annotated file Show diff for this revision Revisions of this file
FOC/foc.cpp Show annotated file Show diff for this revision Revisions of this file
PositionSensor/PositionSensor.cpp Show annotated file Show diff for this revision Revisions of this file
PositionSensor/PositionSensor.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/BLDC_V2.lib	Wed May 13 09:53:27 2020 +0000
+++ b/BLDC_V2.lib	Wed Apr 07 10:12:43 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/benkatz/code/Hobbyking_Cheetah_Compact/#6cc428f3431d
+https://os.mbed.com/users/Wooden/code/BLDC_V2/#f4ecf3e0576a
--- a/Calibration/calibration.cpp	Wed May 13 09:53:27 2020 +0000
+++ b/Calibration/calibration.cpp	Wed Apr 07 10:12:43 2021 +0000
@@ -19,6 +19,7 @@
     float theta_ref = 0;
     float theta_actual = 0;
     float v_d = .15f;                                                             //Put all volts on the D-Axis
+    // float v_d = .08f;
     float v_q = 0.0f;
     float v_u, v_v, v_w = 0;
     float dtc_u, dtc_v, dtc_w = .5f;
@@ -90,7 +91,8 @@
     int raw_b[n] = {0};
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .15f;                                                             // Put volts on the D-Axis
+    float v_d = .25f;                                                             // Put volts on the D-Axis
+    // float v_d = .08f;
     float v_q = 0.0f;
     float v_u, v_v, v_w = 0;
     float dtc_u, dtc_v, dtc_w = .5f;
--- a/Config/current_controller_config.h	Wed May 13 09:53:27 2020 +0000
+++ b/Config/current_controller_config.h	Wed Apr 07 10:12:43 2021 +0000
@@ -4,15 +4,22 @@
 // Current controller///
 #define K_D .05f                    // Loop gain,  Volts/Amp
 #define K_Q .05f                    // Loop gain,  Volts/Amp
-//#define K_SCALE 0.0001f             // K_loop/Loop BW (Hz) 0.0042
-#define K_SCALE 0.00165f
+#define K_SCALE 0.0001f             // K_loop/Loop BW (Hz) 0.0042
+//#define K_SCALE 0.00165f
+// #define K_SCALE 0.0000000825f
 // #define K_SCALE 0.0008
 //#define K_SCALE 0.0004
 #define KI_D 0.0255f                // PI zero, in radians per sample
 #define KI_Q 0.0255f                // PI zero, in radians per sample
 #define V_BUS 24.0f                 // Volts
+// #define V_BUS 48.0f
 #define OVERMODULATION 1.2f         // 1.0 = no overmodulation
 
+#define kp_design 0.6f
+#define ki_design 0.086f
+// #define alpha 0.86425f   // 1/(2piTf+1), fc=1000
+#define alpha 0.76104f      // 1 - 2PiDtFc/(2PiDtFc+1), Fc=2k hz
+
 #define D_INT_LIM V_BUS/(K_D*KI_D)  // Amps*samples
 #define Q_INT_LIM V_BUS/(K_Q*KI_Q)  // Amps*samples
 
--- a/Config/hw_config.h	Wed May 13 09:53:27 2020 +0000
+++ b/Config/hw_config.h	Wed Apr 07 10:12:43 2021 +0000
@@ -6,9 +6,11 @@
 #define PIN_W PA_10
 #define ENABLE_PIN PB_5        // Enable gate drive pin
 #define LED         PB_0        // LED Pin
-#define I_SCALE 0.080566406f  // Amps per A/D Count    3.3v/4096/20/0.0005
+// #define I_SCALE 0.080566406f  // Amps per A/D Count    3.3v/4096/20/0.0005
+// #define I_SCALE 0.026855f       // Amps per A/D Count    3.3v/4096/20/0.0015   for 1.5m omega current shunt
+#define I_SCALE 0.040283f
 // #define V_SCALE 0.00884f        // Bus volts per A/D Count    
-#define V_SCALE 0.015087891    // Bus volts per A/D count, 3.3/4096*(39+2.2)/2.2
+#define V_SCALE 0.015087891f    // Bus volts per A/D count, 3.3/4096*(39+2.2)/2.2
 #define DTC_MAX 0.95f          // Max phase duty cycle
 #define DTC_MIN 0.05f          // Min phase duty cycle
 #define PWM_ARR 0x8CA           /// timer autoreload value
--- a/Config/motor_config.h	Wed May 13 09:53:27 2020 +0000
+++ b/Config/motor_config.h	Wed Apr 07 10:12:43 2021 +0000
@@ -1,19 +1,24 @@
 #ifndef MOTOR_CONFIG_H
 #define MOTOR_CONFIG_H
 
-#define R_PHASE 0.105f          //Ohms
+// #define R_PHASE 0.105f          //Ohms
+#define R_PHASE 0.1265f
 //#define L_D 0.00003f            //Henries
-#define L_D 0.00005f 
+//#define L_D 0.00005f 
+#define L_D 0.000066f
 //#define L_Q 0.00003f            //Henries
-#define L_Q 0.00005f 
-//#define KT .075f                 //N-m per peak phase amp, = WB*NPP*3/2
-#define KT .05f
+//#define L_Q 0.00005f 
+#define L_Q 0.000066f
+#define KT .075f                 //N-m per peak phase amp, = WB*NPP*3/2
+// #define KT .05f
 #define NPP 21                  //Number of pole pairs
 #define GR 6.0f                 //Gear ratio
-// #define KT_OUT 0.45f            //KT*GR
-#define KT_OUT 0.30f       // xmotor, 这个可能是受到了磁铁和保持架影响
-//#define WB 0.0024f               //Webers.  
-#define WB 0.0016f
+#define KT_OUT 0.45f            //KT*GR
+// #define KT_OUT 0.30f
+// xmotor, 这个可能是受到了磁铁和保持架影响
+// #define KT_OUT 0.055f
+#define WB 0.0024f               //Webers.  
+// #define WB 0.0016f
 
 
 #endif
--- a/FOC/foc.cpp	Wed May 13 09:53:27 2020 +0000
+++ b/FOC/foc.cpp	Wed Apr 07 10:12:43 2021 +0000
@@ -1,4 +1,5 @@
 
+// He version
 #include "foc.h"
 using namespace FastMath;
 
@@ -81,12 +82,16 @@
        /// Commutation Loop ///
        controller->loop_count ++;   
        if(PHASE_ORDER){                                                                          // Check current sensor ordering
-           controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    // Calculate phase currents from ADC readings
-           controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+           //controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    // Calculate phase currents from ADC readings
+           //controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+           controller->i_b = alpha * controller->i_b + (1.0f - alpha) * I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);  // low pass filter, freq=1-alpha / dt
+           controller->i_c = alpha * controller->i_c + (1.0f - alpha) * I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
            }
-        else{
-            controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    
-           controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
+       else{
+           // controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    
+           // controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
+           controller->i_b = alpha * controller->i_b + (1.0f - alpha) * I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+           controller->i_c = alpha * controller->i_c + (1.0f - alpha) * I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
            }
        controller->i_a = -controller->i_b - controller->i_c;       
        
@@ -114,13 +119,14 @@
        
        /// PI Controller ///
 //       float i_d_error = controller->i_d_ref - controller->i_d;
-       float i_d_error = controller->i_d_ref - observer->i_d_est;
-//       float i_q_error = controller->i_q_ref - controller->i_q;//  + cogging_current*0.6;
-       float i_q_error = controller->i_q_ref - observer->i_q_est;// + cogging_current;
-       //float i_q_error = controller->i_q_ref - observer->i_q_est;
+      float i_d_error = controller->i_d_ref - observer->i_d_est;
+//       float i_q_error = controller->i_q_ref - controller->i_q + cogging_current*0.6;
+//       float i_q_error = controller->i_q_ref - controller->i_q;
+      float i_q_error = controller->i_q_ref - observer->i_q_est;// + cogging_current;
+//       float i_q_error = controller->i_q_ref - observer->i_q_est;
        
-       float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
-       float v_q_ff =  2.0f*(controller->i_q_ref*R_PHASE  + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
+       float v_d_ff = (controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
+       float v_q_ff =  (controller->i_q_ref*R_PHASE  + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
        
        controller->d_int += i_d_error;   
        controller->q_int += i_q_error;
@@ -128,9 +134,12 @@
        //v_d_ff = 0;
        //v_q_ff = 0;
        
-       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q));        // Limit integrators to prevent windup
-       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; 
+       // limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q));        // Limit integrators to prevent windup
+       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q));
+       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_d = kp_design * i_d_error + ki_design * DT * controller->d_int;
+         //controller->v_q = kp_design * i_q_error + ki_design * DT * controller->q_int;
        
 //       controller->v_q = 0.0f;  // always zero
 //       controller->v_d = 0.0f;
@@ -146,8 +155,10 @@
         //controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - (0.86602540378f*c-.5f*s)*controller->v_q;
        svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
 
-       observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
-       observer->i_q_dot =  0.5f*(controller->v_q - 2.0f*(observer->i_q_est*R_PHASE  + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
+       // observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
+       // observer->i_q_dot =  0.5f*(controller->v_q - 2.0f*(observer->i_q_est*R_PHASE  + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
+       observer->i_d_dot = (controller->v_d - (observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
+       observer->i_q_dot = (controller->v_q - (observer->i_q_est*R_PHASE  + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
        
        if(PHASE_ORDER){                                                         // Check which phase order to use, 
             TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);                        // Write duty cycles
@@ -177,9 +188,20 @@
     
     
 void torque_control(ControllerStruct *controller){
+    // controller->t_ff= 0.5f;
+    // controller->kd = 0.1f;
     float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
+    if(torque_ref>18.0f){
+        torque_ref = 18.0f;
+    }
+    else{
+        if(torque_ref <-18.0f){
+            torque_ref = -18.0f;    
+        }
+    }    
     //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
-    controller->i_q_ref = torque_ref/KT_OUT;    
+    controller->i_q_ref = torque_ref/KT_OUT;  
+    // controller->i_q_ref = 1.0f;
     controller->i_d_ref = 0.0f;
     }
 
@@ -188,4 +210,4 @@
 void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
     
     }
-*/    
\ No newline at end of file
+*/    
--- a/PositionSensor/PositionSensor.cpp	Wed May 13 09:53:27 2020 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Apr 07 10:12:43 2021 +0000
@@ -24,6 +24,10 @@
     oldVel = 0;
     raw = 0;
     flag_first_time = true;
+    for(int i=0;i<40;i++){
+        velVec[i] = 0;
+        // MechPositionVec[i] = 0;
+        }
     }
     
 void PositionSensorAM5147::Sample(float dt){
@@ -72,14 +76,20 @@
     
     int n = 40;
     float sum = vel;
+    //float sum2 = MechPosition;
     for (int i = 1; i < (n); i++){
         velVec[n - i] = velVec[n-i-1];
         sum += velVec[n-i];
+        //MechPositionVec[n-i] = MechPositionVec[n-i-1];
+        //sum2 += MechPositionVec[n-i];
         }
     velVec[0] = vel;
+    //MechPositionVec[0] = MechPosition;
+    //MechPosition = sum2 / ((float)n);
     MechVelocity =  sum/((float)n);
     ElecVelocity = MechVelocity*_ppairs;
-    ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+    // ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+    ElecVelocityFilt = 0.76094f*ElecVelocityFilt + 0.23906f*ElecVelocity;    // 2khz speed filter
     }
 
 int PositionSensorAM5147::GetRawPosition(){
@@ -99,7 +109,8 @@
     }
 
 float PositionSensorAM5147::GetElecVelocity(){
-    return ElecVelocity;
+    //return ElecVelocity;
+    return ElecVelocityFilt;
     }
 
 float PositionSensorAM5147::GetMechVelocity(){
--- a/PositionSensor/PositionSensor.h	Wed May 13 09:53:27 2020 +0000
+++ b/PositionSensor/PositionSensor.h	Wed Apr 07 10:12:43 2021 +0000
@@ -57,7 +57,7 @@
     virtual int GetCPR(void);
     virtual void WriteLUT(int new_lut[128]);
 private:
-    float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt;
+    float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt, MechPositionVec[40];
     int raw, _CPR, rotations, old_counts, _ppairs;
     bool flag_first_time;   // avoid the first time angle - old_counts
     SPI *spi;
--- a/main.cpp	Wed May 13 09:53:27 2020 +0000
+++ b/main.cpp	Wed Apr 07 10:12:43 2021 +0000
@@ -82,7 +82,9 @@
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
             }
-        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+        // pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q*KT_OUT); //return real torque
+//        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.v_bus/2.0f);
         can.write(txMsg);
         }
     
@@ -117,6 +119,7 @@
     pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
     wait_us(10);
     pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
+    //pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "1", "100", I_BW);
     wait_us(10);
     pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
     wait_us(10);
@@ -196,10 +199,13 @@
         controller.adc3_raw = ADC3->DR;
         spi.Sample(DT);                                                           // sample position sensor
         controller.theta_elec = spi.GetElecPosition();
+//        controller.theta_elec = 0;
         controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
         controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
         controller.dtheta_elec = spi.GetElecVelocity();
-        controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
+//        controller.dtheta_elec = 0;
+//        controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
+         controller.v_bus = 24.0;
         ///
         LLLL ++;
         if(LLLL==10000){
@@ -375,6 +381,7 @@
                 switch (cmd_id){
                     case 'b':
                         I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
+                        //I_BW = fmaxf(fminf(atof(cmd_val), 100.0f), 1.0f);
                         break;
                     case 'i':
                         CAN_ID = atoi(cmd_val);
@@ -469,6 +476,7 @@
     
     controller.v_bus = V_BUS;
     controller.mode = 0;
+    // controller.kd = 0.1;
     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
 
     wait(.1);
@@ -502,11 +510,14 @@
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
     
-    pc.baud(921600);                                                            // set serial baud rate
-    // pc.baud(460800);
+    // pc.baud(921600);                                                            // set serial baud rate
+    pc.baud(460800);
     
     wait(.01);
-    pc.printf("\n\r\n\r 3.3, With obs, vdff, no cogging\n\r\n\r");
+    //pc.printf("\n\r\n\r 3.3, With obs, vdff, no cogging\n\r\n\r");
+    // pc.printf("aloha, it do is 48v, low cal");
+    pc.printf("origin control with current filter 2000hz and current shunt is 1.5mo\n\r");
+    pc.printf("vdff_cogging_\n\r");
     wait(.01);
     pc.printf("\n\r Debug Info:\n\r");
     pc.printf(" Firmware Version: %s\n\r", VERSION_NUM);