Revision 50:f460323fc492, committed 2021-06-02

Comitter:

shaorui

Date:

Wed Jun 02 00:56:21 2021 +0000

Parent:

48:f083ea9d1d03

Commit message:

test for torque_max

Changed in this revision

--- a/CAN/CAN_com.cpp	Thu Jan 09 01:42:00 2020 +0000
+++ b/CAN/CAN_com.cpp	Wed Jun 02 00:56:21 2021 +0000
@@ -50,7 +50,7 @@
         int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
         int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
         
-        controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
+       // 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->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);

--- a/CAN/CAN_com.h	Thu Jan 09 01:42:00 2020 +0000
+++ b/CAN/CAN_com.h	Wed Jun 02 00:56:21 2021 +0000
@@ -8,10 +8,11 @@
 
  #define P_MIN -12.5f
  #define P_MAX 12.5f
- #define V_MIN -45.0f
- #define V_MAX 45.0f
+ #define V_MIN -5.0f
+ #define V_MAX 5.0f
  #define KP_MIN 0.0f
- #define KP_MAX 500.0f
+ //#define KP_MAX 500.0f
+ #define KP_MAX 600.0f
  #define KD_MIN 0.0f
  #define KD_MAX 5.0f
  #define T_MIN -18.0f

--- a/Calibration/calibration.cpp	Thu Jan 09 01:42:00 2020 +0000
+++ b/Calibration/calibration.cpp	Wed Jun 02 00:56:21 2021 +0000
@@ -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 = .20f;                                                             // 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	Thu Jan 09 01:42:00 2020 +0000
+++ b/Config/motor_config.h	Wed Jun 02 00:56:21 2021 +0000
@@ -7,8 +7,8 @@
 //#define KT .075f                 //N-m per peak phase amp, = WB*NPP*3/2
 #define KT .08f  
 #define NPP 21                  //Number of pole pairs
-//#define GR 1.0f  
-#define GR 49.0f                 //Gear ratio
+#define GR 49.0f  
+//#define GR 151.0f                 //Gear ratio
 //#define KT_OUT 0.45f            //KT*GR
 #define KT_OUT 4.0f   
 #define WB 0.0024f               //Webers.  

--- a/FlashWriter/FlashWriter.cpp	Thu Jan 09 01:42:00 2020 +0000
+++ b/FlashWriter/FlashWriter.cpp	Wed Jun 02 00:56:21 2021 +0000
@@ -13,7 +13,7 @@
 }
 
 void FlashWriter::open() {
-    FLASH_Unlock();
+    FLASH_Unlock();//Unlocks the FLASH control register access
     FLASH_ClearFlag( FLASH_FLAG_EOP |  FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
     FLASH_EraseSector(__SECTORS[__sector], VoltageRange_3);
     __ready = true;

--- a/PositionSensor/PositionSensor.cpp	Thu Jan 09 01:42:00 2020 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Jun 02 00:56:21 2021 +0000
@@ -13,7 +13,7 @@
     rotations = 0;
     spi = new SPI(PC_12, PC_11, PC_10);
     spi->format(16, 1);                                                          // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
-    spi->frequency(25000000);
+    spi->frequency(10000000);
     
     cs = new DigitalOut(PA_15);
     cs->write(1);

--- a/main.cpp	Thu Jan 09 01:42:00 2020 +0000
+++ b/main.cpp	Wed Jun 02 00:56:21 2021 +0000
@@ -13,7 +13,8 @@
 
 
 float __float_reg[64];                                                          // Floats stored in flash
-int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
+int __int_reg[256]; 
+int flag=0;                                                            // Ints stored in flash.  Includes position sensor calibration lookup table
 
 #include "mbed.h"
 #include "PositionSensor.h"
@@ -49,15 +50,16 @@
 
 PositionSensorAM5147 spi(16384, 0.0, NPP);    //14 bits encoder, 21 NPP
 
-volatile int count = 0;
 volatile int state = REST_MODE;
 volatile int state_change;
-
+volatile int count = 0;
+int reg_count=0;
 void onMsgReceived() {
     //msgAvailable = true;
    
     can.read(rxMsg);  
     if((rxMsg.id == CAN_ID)){
+        flag=1;
         controller.timeout = 0;
         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]==0xFC))){
             state = MOTOR_MODE;
@@ -147,6 +149,10 @@
     
 void print_encoder(void){
     printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
+     for(reg_count=0;reg_count<=263;reg_count++)
+    {
+        printf("%d  %d\n\r",reg_count,__int_reg[reg_count]);
+    }
     wait(.05);
     }
 
@@ -175,6 +181,7 @@
             case REST_MODE:                                                     // Do nothing
                 if(state_change){
                     enter_menu_state();
+                    flag=0;//轨迹位置停止增加
                     }
                 break;
             
@@ -188,6 +195,7 @@
                 if(state_change){
                     enter_torque_mode();
                     count = 0;
+                    controller.p_init_pos= controller.theta_mech;//shaorui add for torque test
                     }
                 else{
                 /*
@@ -199,7 +207,11 @@
                     printf("OVP Triggered!\n\r");
                     }
                     */  
-
+                if(flag==1)
+               {
+                controller.p_des =controller.p_init_pos + controller.v_des*count/(40000);
+                //shaorui end
+                }   
                 torque_control(&controller);    
                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                     controller.i_d_ref = 0;
@@ -210,16 +222,7 @@
                     } 
                 commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
                 controller.timeout += 1;
-                
-                /*
-                count++;
-                if(count == 4000){
-                     printf("%.4f\n\r", controller.dtheta_mech);
-                     count = 0;
-                     }
-                     */
-                     
-            
+                count++; //shaorui add for torque_trajectory time count
                 }     
                 break;
             case SETUP_MODE:
@@ -395,6 +398,22 @@
 
     
     while(1) {
+        //if(state == MOTOR_MODE)
+       // {
+            /*
+        printf("p_des: %.3f p_real: %.3f E: %.3f \n\r", controller.p_des*360/(2*PI),controller.theta_mech*360/(2*PI),(controller.p_des-controller.theta_mech)*360/(2*PI)); 
+        
+        printf("v_des(r/min): %.3f v_real(r/min): %.3f E(./s): %.3f \n\r",  
+        controller.v_des*GR*60/(2*PI),controller.dtheta_mech*GR*60/(2*PI),(controller.v_des-controller.dtheta_mech)*360/(2*PI));
+        printf("kp %.3f,kd:%.3f\n\r",controller.kp,controller.kd);
+        printf("i_q_ref: %.3f\n\r", controller.i_q_ref);
+        //printf("i_d: %.3f \n\r", controller.i_d);*/
+       // printf("v_real(r/min): %.3f kp: %.3f kd: %.3f vdes: %.3f  pdes: %.3f iq: %.3f iq_f: %.3f\n", controller.dtheta_mech*GR*60/(2*PI),controller.kp,controller.kd, controller.v_des*GR*60/(2*PI),controller.p_des,controller.i_q,controller.i_q_filt); 
+         printf("v_real(r/min): %.3f vdes: %.3f  \n\r", controller.dtheta_mech*GR*60/(2*PI),controller.v_des*GR*60/(2*PI));
+//        printf("%04.3f%03.3f%01.3f%01.3f%03.3f%01.3f%01.3f\n", controller.dtheta_mech*GR*60/(2*PI),controller.kp,controller.kd, controller.v_des*GR*60/(2*PI),controller.p_des,controller.i_q,controller.i_q_filt); 
+        printf("q: %.3f q_filt: %.3f  \n\r", controller.i_q,controller.i_q_filt);
+        wait(1);
+      //  }
 
     }
 }

--- a/structs.h	Thu Jan 09 01:42:00 2020 +0000
+++ b/structs.h	Wed Jun 02 00:56:21 2021 +0000
@@ -34,6 +34,7 @@
     int mode;
     int ovp_flag;
     float p_des, v_des, kp, kd, t_ff;
+    float v_eding,p_init_pos;
     float cogging[128];
     } ControllerStruct;