Diff: main.cpp

Revision:

50:f460323fc492

Parent:

47:55bdc4d5096b

--- 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);
+      //  }
 
     }
 }