12

Dependencies:   mbed-dev-f303 FastPWM3

Files at this revision

API Documentation at this revision

Comitter:
shaorui
Date:
Wed Jun 02 00:56:21 2021 +0000
Parent:
48:f083ea9d1d03
Commit message:
test for torque_max

Changed in this revision

CAN/CAN_com.cpp Show annotated file Show diff for this revision Revisions of this file
CAN/CAN_com.h 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/motor_config.h Show annotated file Show diff for this revision Revisions of this file
FlashWriter/FlashWriter.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
main.cpp Show annotated file Show diff for this revision Revisions of this file
structs.h Show annotated file Show diff for this revision Revisions of this file
diff -r f083ea9d1d03 -r f460323fc492 CAN/CAN_com.cpp
--- 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);
diff -r f083ea9d1d03 -r f460323fc492 CAN/CAN_com.h
--- 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
diff -r f083ea9d1d03 -r f460323fc492 Calibration/calibration.cpp
--- 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;
diff -r f083ea9d1d03 -r f460323fc492 Config/motor_config.h
--- 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.  
diff -r f083ea9d1d03 -r f460323fc492 FlashWriter/FlashWriter.cpp
--- 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;
diff -r f083ea9d1d03 -r f460323fc492 PositionSensor/PositionSensor.cpp
--- 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);
diff -r f083ea9d1d03 -r f460323fc492 main.cpp
--- 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);
+      //  }
 
     }
 }
diff -r f083ea9d1d03 -r f460323fc492 structs.h
--- 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;