Revision 45:26801179208e, committed 2018-06-27

Comitter:

benkatz

Date:

Wed Jun 27 03:44:44 2018 +0000

Parent:

44:8040fa2fcb0d

Child:

46:2d4b1dafcfe3

Commit message:

Lots of changes, seems to have broken calibration

Changed in this revision

--- a/Calibration/calibration.cpp	Mon Jun 11 00:04:06 2018 +0000
+++ b/Calibration/calibration.cpp	Wed Jun 27 03:44:44 2018 +0000
@@ -16,7 +16,7 @@
     printf("\n\r Checking phase ordering\n\r");
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .15f;                                                             //Put all volts on the D-Axis
+    float v_d = .05f;                                                             //Put all volts on the D-Axis
     float v_q = 0.0f;
     float v_u, v_v, v_w = 0;
     float dtc_u, dtc_v, dtc_w = .5f;
@@ -88,7 +88,7 @@
     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 = .05f;                                                             // Put volts on the D-Axis
     float v_q = 0.0f;
     float v_u, v_v, v_w = 0;
     float dtc_u, dtc_v, dtc_w = .5f;

--- a/Config/motor_config.h	Mon Jun 11 00:04:06 2018 +0000
+++ b/Config/motor_config.h	Wed Jun 27 03:44:44 2018 +0000
@@ -1,14 +1,14 @@
 #ifndef MOTOR_CONFIG_H
 #define MOTOR_CONFIG_H
 
-#define R_PHASE 0.105f          //Ohms
+#define R_PHASE 0.13f          //Ohms
 #define L_D 0.00003f            //Henries
 #define L_Q 0.00003f            //Henries
 #define KT .075f                 //N-m per peak phase amp, = WB*NPP*3/2
 #define NPP 21                  //Number of pole pairs
 #define GR 6.0f                 //Gear ratio
 #define KT_OUT 0.45f            //KT*GR
-#define WB 0.0024f               //Webers.  
+#define WB 0.0024f               //Flux linkage, Webers.  
 
 
 

--- a/DRV8323/DRV.cpp	Mon Jun 11 00:04:06 2018 +0000
+++ b/DRV8323/DRV.cpp	Wed Jun 27 03:44:44 2018 +0000
@@ -109,4 +109,10 @@
 {
     uint16_t val = (read_register(DCR)) | (0x1<<2);    
     write_register(DCR, val);
+    }
+    
+void DRV832x::calibrate(void)
+{
+    uint16_t val = 0x1<<4 + 0x1<<3 + 0x1<<2;
+    write_register(CSACR, val);
     }
\ No newline at end of file

--- a/DRV8323/DRV.h	Mon Jun 11 00:04:06 2018 +0000
+++ b/DRV8323/DRV.h	Wed Jun 27 03:44:44 2018 +0000
@@ -35,6 +35,7 @@
         void write_CSACR(int CSA_FET, int VREF_DIV, int LS_REF, int CSA_GAIN, int DIS_SEN, int CSA_CAL_A, int CSA_CAL_B, int CSA_CAL_C, int SEN_LVL);
         void enable_gd(void);
         void disable_gd(void);
+        void calibrate(void);
         void print_faults();
         
     private:

--- a/FOC/foc.cpp	Mon Jun 11 00:04:06 2018 +0000
+++ b/FOC/foc.cpp	Wed Jun 27 03:44:44 2018 +0000
@@ -56,6 +56,15 @@
     *offset_1 = adc1_offset/n;
     *offset_2 = adc2_offset/n;
     }
+    
+void init_controller_params(ControllerStruct *controller){
+    controller->ki_d = KI_D;
+    controller->ki_q = KI_Q;
+    controller->k_d = K_SCALE*I_BW;
+    controller->k_q = K_SCALE*I_BW;
+    controller->alpha = 1.0f - 1.0f/(1.0f - DT*I_BW*2.0f*PI);
+    
+    }
 
 void reset_foc(ControllerStruct *controller){
     TIM1->CCR3 = (PWM_ARR>>1)*(0.5f);
@@ -71,6 +80,12 @@
     controller->v_q = 0;
     controller->v_d = 0;
     }
+    
+void limit_current_ref (ControllerStruct *controller){
+    float i_q_max_limit = (0.5774f*controller->v_bus - controller->dtheta_elec*WB)/R_PHASE;
+    float i_q_min_limit = (-0.5774f*controller->v_bus - controller->dtheta_elec*WB)/R_PHASE;
+    controller->i_q_ref = fmaxf(fminf(i_q_max_limit, controller->i_q_ref), i_q_min_limit);
+    }
 
 
 void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta){
@@ -109,44 +124,45 @@
         observer->i_q_est += K_O*observer->e_q + .001f*observer->e_q_int;
         
         
-        float scog12 = FastSin(12.0f*theta);
-        float scog1 = s;
-        float cogging_current = 0.25f*scog1 - 0.3f*scog12;
+        //float scog12 = FastSin(12.0f*theta);
+        //float scog1 = s;
+        //float cogging_current = 0.25f*scog1 - 0.3f*scog12;
+        
+        // Filter the current references to the desired closed-loopbandwidth
+        // Allows calculation of desired di/dt for inductance, etc
+        controller->did_dt = controller->i_d_ref_filt;
+        controller->diq_dt = controller->i_q_ref_filt;
+        controller->i_d_ref_filt = (1.0f-controller->alpha)*controller->i_d_ref_filt + controller->alpha*controller->i_d_ref;
+        controller->i_q_ref_filt = (1.0f-controller->alpha)*controller->i_q_ref_filt + controller->alpha*controller->i_q_ref;
+        controller->did_dt = (controller->i_d_ref_filt - controller->did_dt)/DT;
+        controller->diq_dt = (controller->i_q_ref_filt - controller->diq_dt)/DT;
        
        /// 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;
        
-       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));
+       // Calculate feed-forward voltages //
+       float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE + L_D*controller->did_dt - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
+       float v_q_ff =  2.0f*(controller->i_q_ref*R_PHASE + L_Q*controller->diq_dt + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
        
+       // Integrate Error //
        controller->d_int += i_d_error;   
        controller->q_int += i_q_error;
-       
-       //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; 
-       
-       //controller->v_q = 4.0f;
-       //controller->v_d = 0.0f;
-       
-       //controller->v_d = v_d_ff;
-       //controller->v_q = v_q_ff; 
+
+       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(controller->k_q*controller->ki_q));        
+       controller->v_d = controller->k_d*(i_d_error + controller->ki_d*controller->d_int);// + v_d_ff;  
+       controller->v_q = controller->k_q*(i_q_error + controller->ki_q*controller->q_int);// + v_q_ff; 
        
        limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
-       abc(controller->theta_elec+0.5f*DT*controller->dtheta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
-    
-        //controller->v_u = c*controller->v_d - s*controller->v_q;                // Faster Inverse DQ0 transform
-        //controller->v_v = (0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
-        //controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - (0.86602540378f*c-.5f*s)*controller->v_q;
+       abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
        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;
        
+       //controller->dtc_u = 0.5f;
+       //controller->dtc_v = 0.6f;
+       //controller->dtc_w = 0.5f;
        if(PHASE_ORDER){                                                         // Check which phase order to use, 
             TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);                        // Write duty cycles
             TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_v);
@@ -168,7 +184,7 @@
            //printf("%.2f  %.2f  %.2f\n\r", controller->i_a, controller->i_b, controller->i_c);
            //printf("%f\n\r", controller->dtheta_mech*GR);
            //pc.printf("%f    %f    %f\n\r", controller->i_a, controller->i_b, controller->i_c);
-           //pc.printf("%f    %f\n\r", controller->i_d, controller->i_q);
+           //printf("%f    %f\n\r", controller->k_d, controller->k_q);
            //pc.printf("%d    %d\n\r", controller->adc1_raw, controller->adc2_raw);
             }
     }

--- a/FOC/foc.h	Mon Jun 11 00:04:06 2018 +0000
+++ b/FOC/foc.h	Wed Jun 27 03:44:44 2018 +0000
@@ -17,6 +17,8 @@
 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w);
 void zero_current(int *offset_1, int *offset_2);
 void reset_foc(ControllerStruct *controller);
+void init_controller_params(ControllerStruct *controller);
 void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta);
 void torque_control(ControllerStruct *controller);
+void limit_current_ref (ControllerStruct *controller);
 #endif

--- a/PositionSensor/PositionSensor.cpp	Mon Jun 11 00:04:06 2018 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Jun 27 03:44:44 2018 +0000
@@ -25,10 +25,8 @@
     }
     
 void PositionSensorAM5147::Sample(){
-    GPIOA->ODR &= ~(1 << 15);
     raw = spi->write(readAngleCmd);
     raw &= 0x3FFF;                                                              //Extract last 14 bits
-    GPIOA->ODR |= (1 << 15);
     int off_1 = offset_lut[raw>>7];
     int off_2 = offset_lut[((raw>>7)+1)%128];
     int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7);        // Interpolate between lookup table entries

--- a/main.cpp	Mon Jun 11 00:04:06 2018 +0000
+++ b/main.cpp	Wed Jun 27 03:44:44 2018 +0000
@@ -1,5 +1,5 @@
 /// high-bandwidth 3-phase motor control, for robots
-/// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others
+/// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, David Otten, and others
 /// Hardware documentation can be found at build-its.blogspot.com
 /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
 
@@ -9,7 +9,7 @@
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
 
-#define VERSION_NUM "1.6"
+#define VERSION_NUM "1.7"
 
 
 float __float_reg[64];                                                          // Floats stored in flash
@@ -41,7 +41,7 @@
 Serial pc(PA_2, PA_3);
 
 
-CAN          can(PB_8, PB_9);      // CAN Rx pin name, CAN Tx pin name
+CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name
 CANMessage   rxMsg;
 CANMessage   txMsg;
 
@@ -164,15 +164,16 @@
         ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
         //volatile int delay;   
         //for (delay = 0; delay < 55; delay++);
+
+        spi.Sample();                                                           // sample position sensor
         controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
         controller.adc1_raw = ADC1->DR;
         controller.adc3_raw = ADC3->DR;
-        spi.Sample();                                                           // sample position sensor
         controller.theta_elec = spi.GetElecPosition();
         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.v_bus = 24.0f;//0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
         ///
         
         /// Check state machine state, and run the appropriate function ///
@@ -213,20 +214,22 @@
                     controller.kd = 0;
                     controller.t_ff = 0;
                     } 
-                //commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
-                TIM1->CCR3 = (PWM_ARR)*(0.5f);                        // Write duty cycles
-                TIM1->CCR2 = (PWM_ARR)*(0.5f);
-                TIM1->CCR1 = (PWM_ARR)*(0.5f);
+                controller.i_q_ref = 2.0f;
+                commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
+                //TIM1->CCR3 = (PWM_ARR)*(0.5f);                        // Write duty cycles
+                //TIM1->CCR2 = (PWM_ARR)*(0.5f);
+                //TIM1->CCR1 = (PWM_ARR)*(0.5f);
                 controller.timeout += 1;
                 
                 /*
                 count++;
                 if(count == 4000){
-                     //printf("%.4f\n\r", controller.dtheta_mech);
-                     
+                     printf("%d  %d  %.4f  %.4f  %.4f\n\r", controller.adc1_raw, controller.adc2_raw, controller.i_a, controller.i_b, controller.i_c);
+                     //printf("%.3f\n\r", controller.dtheta_mech);
                      count = 0;
                      }
-                 */    
+                     */
+                   
                      
             
                 }     
@@ -351,7 +354,6 @@
     }
        
 int main() {
-    can.frequency(1000000);                                                     // set bit rate to 1Mbps
     controller.v_bus = V_BUS;
     controller.mode = 0;
     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
@@ -359,12 +361,17 @@
     
     gpio.enable->write(1);
     wait_us(100);
+    drv.calibrate();
+    wait_us(100);
     drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
     wait_us(100);
     drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, 0x3);
     wait_us(100);
     
+    
+    //drv.enable_gd();
     zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
+    //drv.disable_gd();
 
 
     
@@ -377,7 +384,6 @@
     TIM1->CCR1 = 0x708*(1.0f);
     gpio.enable->write(0);
     */
-
     reset_foc(&controller);                                                     // Reset current controller
     TIM1->CR1 ^= TIM_CR1_UDIS;
     //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
@@ -401,7 +407,8 @@
     int lut[128] = {0};
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
-    
+    init_controller_params(&controller);
+
     pc.baud(921600);                                                            // set serial baud rate
     wait(.01);
     pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
@@ -429,19 +436,5 @@
 
     int counter = 0;
     while(1) {
-counter++;
-        if(counter>40000)
-        {   
-            //gpio.enable->write(1);
-            //wait_us(100);
-            //printf(" %d\n\r", drv.read_register(DCR));
-            //wait_us(100);
-            //printf(" %d\n\r", drv.read_register(CSACR));
-            drv.print_faults();
-            //printf("%d\n\r", drv.read_register(DCR));
-            counter = 0;
-            //gpio.enable->write(0);
-            }
-        wait_us(25);
     }
 }

--- a/structs.h	Mon Jun 11 00:04:06 2018 +0000
+++ b/structs.h	Wed Jun 27 03:44:44 2018 +0000
@@ -17,23 +17,25 @@
     }COMStruct;
     
 typedef struct{
-    int adc1_raw, adc2_raw, adc3_raw;
-    float i_a, i_b, i_c;
-    float v_bus;
-    float theta_mech, theta_elec;
-    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;
-    float d_int, q_int;
-    int adc1_offset, adc2_offset;
-    float i_d_ref, i_q_ref;
-    int loop_count;
-    int timeout;
+    int adc1_raw, adc2_raw, adc3_raw;                       // Raw ADC Values
+    float i_a, i_b, i_c;                                    // Phase currents
+    float v_bus;                                            // DC link voltage
+    float theta_mech, theta_elec;                           // Rotor mechanical and electrical angle
+    float dtheta_mech, dtheta_elec, dtheta_elec_filt;       // Rotor mechanical and electrical angular velocit
+    float i_d, i_q, i_q_filt;                               // D/Q currents
+    float v_d, v_q;                                         // D/Q voltages
+    float dtc_u, dtc_v, dtc_w;                              // Terminal duty cycles
+    float v_u, v_v, v_w;                                    // Terminal voltages
+    float k_d, k_q, ki_d, ki_q, alpha;                      // Current loop gains, current reference filter coefficient
+    float d_int, q_int;                                     // Current error integrals
+    int adc1_offset, adc2_offset;                           // ADC offsets
+    float i_d_ref, i_q_ref, i_d_ref_filt, i_q_ref_filt;     // Current references
+    float did_dt, diq_dt;                                   // Current reference derivatives
+    int loop_count;                                         // Degubbing counter
+    int timeout;                                            // Watchdog counter
     int mode;
-    int ovp_flag;
-    float p_des, v_des, kp, kd, t_ff;
+    int ovp_flag;                                           // Over-voltage flag
+    float p_des, v_des, kp, kd, t_ff;                       // Desired position, velocity, gians, torque
     float cogging[128];
     } ControllerStruct;