Revision 55:d57335792cb7, committed 2022-05-10

Comitter:

qiudehua

Date:

Tue May 10 02:10:43 2022 +0000

Parent:

54:59575833d16f

Commit message:

GT_MOTOR_24NM_V03

Changed in this revision

--- a/CAN/CAN_com.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/CAN/CAN_com.cpp	Tue May 10 02:10:43 2022 +0000
@@ -12,6 +12,20 @@
  #define T_MIN -18.0f
  #define T_MAX 18.0f
  
+ /*
+ //为保证设置参数的最小分辨率,可启用下列参数
+ //注意：必须保证上、下转换时,MIN/MAX的一致,否则数据会出错!!!
+ #define P_MIN -150f
+ #define P_MAX 150f
+ #define V_MIN -270.0f
+ #define V_MAX 270.0f
+ #define KP_MIN 0.0f
+ #define KP_MAX 50.0f
+ #define KD_MIN 0.0f
+ #define KD_MAX 5.0f
+ #define T_MIN -5.0f
+ #define T_MAX 5.0f
+ */
 
 /// CAN Reply Packet Structure ///
 /// 16 bit position, between -4*pi and 4*pi
@@ -61,9 +75,13 @@
         
         controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
         controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
-        controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
+        controller->v_des /= GR;                                         // plan A&B&C, modify by 20210630
+        controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);        
+        controller->kp *= GR;                                            // plan A&B&C, modify by 20210630
         controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
+        //controller->kd /= GR;                                            // plan B&C, modify by 20210630
         controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
+        controller->t_ff *= GR;                                          // plan A&C, modify by 20210630
     //printf("Received   ");
     //printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
     //printf("\n\r");

--- a/CAN/CAN_com.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/CAN/CAN_com.h	Tue May 10 02:10:43 2022 +0000
@@ -5,6 +5,7 @@
 #include "user_config.h"
 #include "mbed.h"
 #include "../math_ops.h"
+#include "motor_config.h"
 
 void pack_reply(CANMessage *msg, float p, float v, float t);
 void unpack_cmd(CANMessage msg, ControllerStruct * controller);

--- a/Calibration/calibration.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/Calibration/calibration.cpp	Tue May 10 02:10:43 2022 +0000
@@ -232,7 +232,8 @@
             
         ps->WriteLUT(lut);                                                      // write lookup table to position sensor object
         //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging));  //compensation doesn't actually work yet....
-        memcpy(&ENCODER_LUT, lut, sizeof(lut));                                 // copy the lookup table to the flash array
+        //memcpy(&ENCODER_LUT, lut, sizeof(lut));                                 // copy the lookup table to the flash array        
+        memcpy(&ENCODER_LUT, lut, 512);                                 // copy the lookup table to the flash array  // modified on 20210615
         printf("\n\rEncoder Electrical Offset (rad) %f\n\r",  offset);
         
         if (!prefs->ready()) prefs->open();

--- a/Config/motor_config.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/Config/motor_config.h	Tue May 10 02:10:43 2022 +0000
@@ -4,10 +4,13 @@
 #define R_PHASE 0.13f           //Ohms
 #define L_D 0.00002f            //Henries
 #define L_Q 0.00002f            //Henries
-#define KT .08f                 //N-m per peak phase amp, = WB*NPP*3/2
+//#define KT .08f                 //N-m per peak phase amp, = WB*NPP*3/2
+//#define KT 0.07875f             //N-m per peak phase amp, = WB*NPP*3/2 = 0.0025*21*3/2 = 0.07875
+#define KT 0.2f               //N-m per peak phase amp, = WB*NPP*3/2 = 0.0025*21*3/2 = 0.07875
 #define NPP 21                  //Number of pole pairs
 #define GR 6.0f                 //Gear ratio
-#define KT_OUT 0.45f            //KT*GR
+//#define KT_OUT 0.45f            //KT*GR
+#define KT_OUT 1.2f           //KT*GR
 #define WB 0.0025f              //Flux linkage, Webers.  
 #define R_TH 1.25f              //Kelvin per watt
 #define INV_M_TH 0.03125f       //Kelvin per joule

--- a/DRV8323/DRV.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/DRV8323/DRV.cpp	Tue May 10 02:10:43 2022 +0000
@@ -68,6 +68,7 @@
         
 void DRV832x::print_faults(void)
 {
+    wait_us(10);
     uint16_t val1 = read_FSR1();
     wait_us(10);
     uint16_t val2 = read_FSR2();

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Hobbyking_Cheetah_V02.lib	Tue May 10 02:10:43 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/benkatz/code/Hobbyking_Cheetah/#59575833d16f

--- a/PositionSensor/PositionSensor.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/PositionSensor/PositionSensor.cpp	Tue May 10 02:10:43 2022 +0000
@@ -38,10 +38,12 @@
     int angle = raw + off_interp;                                               // Correct for nonlinearity with lookup table from calibration
     if(angle - old_counts > _CPR/2){
         rotations -= 1;
-        }
+        //if(rotations< -14) rotations = 14.5f;    // modify by 20210706
+    }
     else if (angle - old_counts < -_CPR/2){
-        rotations += 1;
-        }
+        rotations += 1;        
+        //if(rotations> 14) rotations = -15.5f;    // modify by 20210706
+    }
     
     old_counts = angle;
     oldModPosition = modPosition;
@@ -65,16 +67,42 @@
         }
     else{
         vel = (modPosition-oldModPosition)/dt;
-    }    
+    }  
     
-    int n = 40;
+    /*
+    int n = 21;
     float sum = vel;
     for (int i = 1; i < (n); i++){
         velVec[n - i] = velVec[n-i-1];
         sum += velVec[n-i];
-        }
+        }      
     velVec[0] = vel;
-    MechVelocity =  sum/((float)n);
+    MechVelocity =  sum/((float)n);                  // modify by 20210706
+    
+    float sumLow = MechVelocity;
+    for (int i = 1; i < (n); i++){
+        velVecLow[n - i] = velVecLow[n-i-1];
+        sumLow += velVecLow[n-i];
+        }  
+    velVecLow[0] = MechVelocity;
+    MechVelocityLow =  sumLow/((float)n);    // modify by 20210706    
+    */ 
+    
+    //*********************************************************
+    int n = 168;
+    float sum = vel;
+    float sumLow = vel;
+    for (int i = 1; i < (n); i++){
+        velVec[n - i] = velVec[n-i-1];
+        sumLow += velVec[n-i];
+        if(i > 147) sum +=velVec[n-i];
+        }    
+
+    velVec[0] = vel;
+    MechVelocity =  sum/21;                  // modify by 20210706
+    MechVelocityLow =  sumLow/((float)n);    // modify by 20210706 
+    //*********************************************************
+    
     ElecVelocity = MechVelocity*_ppairs;
     ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
     }
@@ -101,6 +129,7 @@
 
 float PositionSensorAM5147::GetMechVelocity(){
     return MechVelocity;
+    //return MechVelocityLow;
     }
 
 void PositionSensorAM5147::ZeroPosition(){

--- a/PositionSensor/PositionSensor.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/PositionSensor/PositionSensor.h	Tue May 10 02:10:43 2022 +0000
@@ -37,7 +37,8 @@
     virtual void ZeroEncoderCountDown(void);
     int _CPR, flag, rotations, _ppairs, raw;
     //int state;
-    float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40];
+    //float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40];
+    float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[168];
     int offset_lut[128];
 };
 
@@ -57,7 +58,9 @@
     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;
+    float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[168], MechVelocity, ElecVelocity, ElecVelocityFilt;
+    float MechVelocityLow,velVecLow[168];  // add by 20210706
     int raw, _CPR, rotations, old_counts, _ppairs;
     SPI *spi;
     DigitalOut *cs;

--- a/main.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/main.cpp	Tue May 10 02:10:43 2022 +0000
@@ -10,7 +10,7 @@
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
 
-#define VERSION_NUM "1.9"
+#define VERSION_NUM "0.3"
 
 
 float __float_reg[64];                                                          // Floats stored in flash
@@ -54,7 +54,7 @@
 
 PositionSensorAM5147 spi(16384, 0.0, NPP);  
 
-volatile int count = 0;
+volatile int counts = 0;
 volatile int state = REST_MODE;
 volatile int state_change;
 
@@ -75,11 +75,17 @@
             }
         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
             spi.ZeroPosition();
+            }            
+        else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFF))){
+            //Null instruction, which can be used to return data
             }
         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*GR, controller.i_q_filt*KT_OUT);    // modify by 20210617
+        //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT);   // 正式使用 plan A&C, modify by 20210630        
+        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT);      //  test v?, modify by 20210706     
         can.write(txMsg);
         }
     
@@ -166,7 +172,6 @@
 /// This runs at 40 kHz, regardless of of the mode the controller is in ///
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
   if (TIM1->SR & TIM_SR_UIF ) {
-
         ///Sample current always ///
         ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
         //volatile int delay;   
@@ -177,8 +182,11 @@
         controller.adc1_raw = ADC1->DR;
         controller.adc3_raw = ADC3->DR;
         controller.theta_elec = spi.GetElecPosition();
-        controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
-        controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
+        //controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
+        controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();   
+        controller.theta_mech = (1.0f)*spi.GetMechPosition();    // modified on 20210617
+        //controller.dtheta_mech = (1.0f)*spi.GetMechVelocity();   // modified on 20210618
+        
         controller.dtheta_elec = spi.GetElecVelocity();
         controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement
         ///
@@ -199,8 +207,11 @@
              
             case MOTOR_MODE:                                                   // Run torque control
                 if(state_change){
+                    
                     enter_torque_mode();
-                    count = 0;
+                    
+                    counts = 0;  //20210222
+                    
                     }
                 else{
                 /*
@@ -224,9 +235,8 @@
 
                 torque_control(&controller);
                 commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
-
                 controller.timeout++;
-                count++; 
+                counts ++; 
             
                 }     
                 break;
@@ -238,7 +248,7 @@
             case ENCODER_MODE:
                 print_encoder();
                 break;
-                }                 
+                }    
       }
   TIM1->SR = 0x0;                                                               // reset the status register
 }
@@ -269,7 +279,7 @@
                     break;
                 case 'm':
                     state = MOTOR_MODE;
-                    state_change = 1;
+                    state_change = 1;                    
                     break;
                 case 'e':
                     state = ENCODER_MODE;
@@ -360,11 +370,23 @@
     }
        
 int main() {
+    //*************************
+    //uint32_t    btr;    
+    //CAN_TypeDef *can = obj->CanHandle.Instance;
+    /*  
+    hcan->Instance->BTR = (uint32_t)((uint32_t)hcan->Init.Mode) | \
+                ((uint32_t)hcan->Init.SJW) | \
+                ((uint32_t)hcan->Init.BS1) | \
+                ((uint32_t)hcan->Init.BS2) | \
+               ((uint32_t)hcan->Init.Prescaler - 1U);
+    */
+    
+    
     controller.v_bus = V_BUS;
     controller.mode = 0;
     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
     wait(.1);
-    
+   
     gpio.enable->write(1);
     wait_us(100);
     drv.calibrate();
@@ -374,7 +396,7 @@
     drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);
     wait_us(100);
     drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88);
-    
+      
     //drv.enable_gd();
     zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
     drv.disable_gd();
@@ -387,6 +409,7 @@
     TIM1->CCR1 = 0x708*(1.0f);
     gpio.enable->write(0);
     */
+    
     reset_foc(&controller);                                                     // Reset current controller
     reset_observer(&observer);                                                 // Reset observer
     TIM1->CR1 ^= TIM_CR1_UDIS;
@@ -411,7 +434,7 @@
     if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
     if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
     if(isnan(I_BW) || I_BW==-1){I_BW = 1000;}
-    if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;}
+    if(isnan(I_MAX) || I_MAX ==-1){I_MAX=30;}
     if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;}
     if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;}
     if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;}
@@ -424,8 +447,9 @@
     init_controller_params(&controller);
 
     pc.baud(921600);                                                            // set serial baud rate
+    pc.attach(&serial_interrupt);                                               // attach serial interrupt
     wait(.01);
-    pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
+    pc.printf("\n\r\n\r GTGJ MOTOR\n\r\n\r");
     wait(.01);
     printf("\n\r Debug Info:\n\r");
     printf(" Firmware Version: %s\n\r", VERSION_NUM);
@@ -435,8 +459,7 @@
     printf(" CAN ID:  %d\n\r", CAN_ID);
     
 
-
-
+//*********
     //printf(" %d\n\r", drv.read_register(DCR));
     //wait_us(100);
     //printf(" %d\n\r", drv.read_register(CSACR));
@@ -444,25 +467,45 @@
     //printf(" %d\n\r", drv.read_register(OCPCR));
     //drv.disable_gd();
     
-    pc.attach(&serial_interrupt);                                               // attach serial interrupt
+//    pc.attach(&serial_interrupt);                                               // attach serial interrupt
     
     state_change = 1;
 
 
-    int counter = 0;
+    //**********************************************************************************************
+    /*
+    CAN_HandleTypeDef* hcan;  
+    //3M --> brp=2,TS1=0,TS2=2,SJW=1
+    //5M --> brp=2,TS1=0,TS2=0,SJW=1
+    btr = ((2 << CAN_BTR_TS2_Pos) & CAN_BTR_TS2) |
+          ((0 << CAN_BTR_TS1_Pos) & CAN_BTR_TS1) |
+          ((1 << CAN_BTR_SJW_Pos) & CAN_BTR_SJW) |
+          ((2 << CAN_BTR_BRP_Pos) & CAN_BTR_BRP);
+    hcan->Instance->BTR &= ~(CAN_BTR_TS2 | CAN_BTR_TS1 | CAN_BTR_SJW | CAN_BTR_BRP);
+    hcan->Instance->BTR |= btr;
+    */
+    //**********************************************************************************************
+
+    //CAN_TypeDef *can = obj->CanHandle.Instance;
+    //state_change = can->MSR;
+    
+//    int counter = 0;
     while(1) {
         drv.print_faults();
         wait(.1);
-       //printf("%.4f\n\r", controller.v_bus);
-       /*
+        //printf("%.4f\n\r", controller.v_bus);
+       //*
         if(state == MOTOR_MODE)
         {
             //printf("%.3f  %.3f  %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
-            //printf("%.3f  %.3f  %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
-            //printf("%.3f\n\r", controller.dtheta_mech);
-            wait(.002);
+            //printf("%.3f  %.3f  %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.i_q_ref,controller.dtheta_elec);
+            //printf("%.3f  %.3f  %.3f %.3f %.3f %.3f %.3f %.3f\n\r", controller.i_a, controller.i_b, controller.i_c,controller.theta_elec,controller.i_d, controller.i_q,controller.i_d_filt, controller.i_q_filt);
+            //printf("%.3f  %.3f  %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.i_q_ref,controller.theta_elec,controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT_OUT/GR);
+            //printf("%.3f\n\r", controller.dtheta_mech);            
+            //printf("%d   %.4f    %.4f\n\r", spi.GetRawPosition(), spi.GetMechPositionFixed(),spi.GetMechPosition());     
+//printf("%d   %d   %d\n\r", can->MSR, can->TSR,can->ESR); 
+            wait(.005);
         }
-        */
-
+        //*/
     }
 }

--- a/mbed-dev.lib	Thu Aug 08 17:39:43 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#36facd806e4a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 10 02:10:43 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file