Revision 55:6ab83cbbbca7, committed 2021-03-26

Comitter:

eatinghsieh

Date:

Fri Mar 26 05:28:00 2021 +0000

Parent:

54:59575833d16f

Commit message:

Test set parameter mode

Changed in this revision

--- a/CAN/CAN_com.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/CAN/CAN_com.cpp	Fri Mar 26 05:28:00 2021 +0000
@@ -12,6 +12,20 @@
  #define T_MIN -18.0f
  #define T_MAX 18.0f
  
+ #define kp_L_MAX 125.0f
+ #define kp_L_MIN 0.0f
+ #define kp_M1_MAX 250.0f
+ #define kp_M1_MIN 126.0f
+ #define kp_M2_MAX 375.0f
+ #define kp_M2_MIN 251.0f
+ #define kp_H_MAX   500.0f
+ #define kp_H_MIN   376.0f
+ #define range_L_MAX 256.0f
+ #define range_L_MIN 256.0f
+ #define range_M_MAX 256.0f
+ #define range_M_MIN 256.0f
+ #define range_H_MAX 256.0f
+ #define range_H_MIN 256.0f
 
 /// CAN Reply Packet Structure ///
 /// 16 bit position, between -4*pi and 4*pi
@@ -35,7 +49,60 @@
     msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
     msg->data[5] = t_int&0xFF;
     }
+
+/// CAN Command Packet Structure ///
+/// 16 bit position command, between -4*pi and 4*pi
+/// 12 bit velocity command, between -30 and + 30 rad/s
+/// 12 bit kp, between 0 and 500 N-m/rad
+/// 12 bit kd, between 0 and 100 N-m*s/rad
+/// 12 bit feed forward torque, between -18 and 18 N-m
+/// CAN Packet is 8 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], kp[11-8]]
+/// 4: [kp[7-0]]
+/// 5: [kd[11-4]]
+/// 6: [kd[3-0], torque[11-8]]
+/// 7: [torque[7-0]]
+void unpack_cmd(CANMessage msg, ControllerStruct * controller){
+        int p_int = (msg.data[0]<<16)|(msg.data[1]<<8)|msg.data[2];
+        int v_int = (msg.data[3]<<12)|(msg.data[4]<<4)|(msg.data[5]>>4);
+        int t_int = ((msg.data[5]&0xF)<<16)|(msg.data[6]<<8)|msg.data[7];
+        
+        controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 24);
+        controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 20);
+        controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 20);
+    //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");
+    }       
+        
+void unpack_param(CANMessage msg, ControllerStruct * controller){
+                int  kp_L_int = msg.data[0];
+                int  kp_M1_int = msg.data[1];
+                int  kp_M2_int = msg.data[2];
+                int  kp_H_int   = msg.data[3];
+                int  kd_int = msg.data[4];
+                int  range_L_int = msg.data[5];
+                int  range_M_int = msg.data[6];
+                int  range_H_int = msg.data[7];
+
     
+        controller->kp_L = uint_to_float(kp_L_int, kp_L_MIN, kp_L_MAX, 8);
+        controller->kp_M1 = uint_to_float(kp_M1_int, kp_M1_MIN, kp_M1_MAX, 8);
+        controller->kp_M2 = uint_to_float(kp_M2_int, kp_M2_MIN, kp_M2_MAX, 8);
+        controller->kp_H = uint_to_float(kp_H_int, kp_H_MIN, kp_H_MAX, 8);
+        controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 8);
+        controller->range_L = uint_to_float(range_L_int, range_L_MIN, range_L_MAX, 8);
+        controller->range_M = uint_to_float(range_M_int, range_M_MIN, range_M_MAX, 8);
+        controller->range_H = uint_to_float(range_H_int, range_H_MIN, range_H_MAX, 8);
+    //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");
+    }
+/*
 /// CAN Command Packet Structure ///
 /// 16 bit position command, between -4*pi and 4*pi
 /// 12 bit velocity command, between -30 and + 30 rad/s
@@ -67,5 +134,5 @@
     //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	Fri Mar 26 05:28:00 2021 +0000
@@ -8,6 +8,7 @@
 
 void pack_reply(CANMessage *msg, float p, float v, float t);
 void unpack_cmd(CANMessage msg, ControllerStruct * controller);
+void unpack_param(CANMessage msg, ControllerStruct * controller);
 
 
 #endif
\ No newline at end of file

--- a/FOC/foc.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/FOC/foc.cpp	Fri Mar 26 05:28:00 2021 +0000
@@ -51,7 +51,7 @@
 
 void linearize_dtc(float *dtc){
     /// linearizes the output of the inverter, which is not linear for small duty cycles ///
-    float sgn = 1.0f-(2.0f*(dtc<0));
+    float sgn = 1.0f-(2.0f*(*dtc<0));
     if(abs(*dtc) >= .01f){
         *dtc = *dtc*.986f+.014f*sgn;
         }
@@ -218,11 +218,32 @@
        
     }
     
+void torque_control(ControllerStruct *controller){
+        float error = controller->p_des - controller->theta_mech;
+        float kp;
     
+        if( error <= controller->range_L )  
+            kp = controller->kp_H;
+        else if ( error > controller->range_L && error <= controller->range_M )  
+            kp = controller->kp_M2;
+        else if ( error > controller->range_M && error <= controller->range_H )  
+            kp = controller->kp_M1; 
+        else
+            kp = controller->kp_L;
+        
+    float torque_ref = kp*error + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
+    //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
+    controller->i_q_ref = torque_ref/KT_OUT;    
+    controller->i_d_ref = 0.0f;
+    }
+        
+        
+/*    
 void torque_control(ControllerStruct *controller){
     float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
     //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
     controller->i_q_ref = torque_ref/KT_OUT;    
     controller->i_d_ref = 0.0f;
     }
+*/
  
\ No newline at end of file

--- a/main.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/main.cpp	Fri Mar 26 05:28:00 2021 +0000
@@ -9,6 +9,7 @@
 #define MOTOR_MODE 2
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
+#define SETPARAM_MODE 6
 
 #define VERSION_NUM "1.9"
 
@@ -76,9 +77,16 @@
         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]==0xFB))){
+                        state = SETPARAM_MODE;
+                        state_change = 1;
+                }
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
             }
+                else if(state == SETPARAM_MODE){
+            unpack_param(rxMsg, &controller);
+            }
         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
         can.write(txMsg);
         }
@@ -217,7 +225,10 @@
                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                     controller.i_d_ref = 0;
                     controller.i_q_ref = 0;
-                    controller.kp = 0;
+                    controller.kp_H = 0;
+                                        controller.kp_L = 0;
+                                        controller.kp_M1 = 0;
+                                        controller.kp_M2 = 0;
                     controller.kd = 0;
                     controller.t_ff = 0;
                     } 
@@ -402,6 +413,7 @@
     prefs.load();                                                               // Read flash
     
     can.filter(CAN_ID , 0xFFF, CANStandard, 0);
+        //can.filter(1 , 0xFFF, CANStandard, 1);    // Example, show how to add can dataframe filter
                                                                     
     txMsg.id = CAN_MASTER;
     txMsg.len = 6;

--- a/mbed-dev.lib	Thu Aug 08 17:39:43 2019 +0000
+++ b/mbed-dev.lib	Fri Mar 26 05:28:00 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#36facd806e4a
+https://github.com/aiRobots-athome/mbed-dev.git/#38c17c2e85b8f9cc9015267d0036f1580cbe5f8f

--- a/structs.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/structs.h	Fri Mar 26 05:28:00 2021 +0000
@@ -33,7 +33,10 @@
     int timeout;                                            // Watchdog counter
     int mode;
     int ovp_flag;                                           // Over-voltage flag
-    float p_des, v_des, kp, kd, t_ff;                       // Desired position, velocity, gians, torque
+    // float p_des, v_des, kp, kd, t_ff;                    // Desired position, velocity, gians, torque
+        float p_des, v_des, t_ff;                                       // Desired position, velocity, torque
+        float kp_L, kp_M1, kp_M2, kp_H, kd ;                                            // kp, kd
+        float range_L, range_M, range_H;                                                    // The range of error
     float v_ref, fw_int;                                     // output voltage magnitude, field-weakening integral
     float cogging[128];
     } ControllerStruct;