Revision 45:35fa6884d0c6, committed 2020-01-16

Comitter:

Lightvalve

Date:

Thu Jan 16 12:05:15 2020 +0000

Parent:

44:fe7d5cfd2eea

Child:

46:2694daea349b

Commit message:

200116

Changed in this revision

--- a/CAN/function_CAN.cpp	Tue Jan 14 09:11:20 2020 +0000
+++ b/CAN/function_CAN.cpp	Thu Jan 16 12:05:15 2020 +0000
@@ -8,11 +8,7 @@
 // CAN ID Setting Variables
 int CID_RX_CMD = 100;
 int CID_RX_REF_POSITION = 200;
-int CID_RX_REF_TORQUE = 300;
-int CID_RX_REF_PRES_DIFF = 400;
-int CID_RX_REF_VOUT = 500;
-int CID_RX_REF_VALVE_POSITION = 600;
-int CID_RX_REF_CURRENT = 700;
+int CID_RX_REF_PWM = 300;
 
 int CID_TX_INFO = 1100;
 int CID_TX_POSITION = 1200;
@@ -32,11 +28,7 @@
 
     CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD);
     CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION);
-    CID_RX_REF_TORQUE = (int) (BNO + INIT_CID_RX_REF_TORQUE);
-    CID_RX_REF_PRES_DIFF = (int) (BNO + INIT_CID_RX_REF_PRES_DIFF);
-    CID_RX_REF_VOUT = (int) (BNO + INIT_CID_RX_REF_VOUT);
-    CID_RX_REF_VALVE_POSITION = (int) (BNO + INIT_CID_RX_REF_VALVE_POSITION);
-    CID_RX_REF_CURRENT = (int) (BNO + INIT_CID_RX_REF_CURRENT);
+    CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM);
 
     CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO);
     CID_TX_POSITION = (int) (BNO + INIT_CID_TX_POSITION);
@@ -88,9 +80,14 @@
             break;
         }
 
-        case CRX_SET_REF_UPDATE_ENABLE: {
-            flag_ref_enable = (int16_t) msg.data[1];
-
+        case CRX_SET_POS_TORQ_TRANS: {
+            MODE_POS_FT_TRANS = (int16_t) msg.data[1];
+            /*
+            MODE_POS_FT_TRANS == 0 : Position Control
+            MODE_POS_FT_TRANS == 1 : Trasition(Position->Torque)
+            MODE_POS_FT_TRANS == 2 : Torque Control (Convert to 2 automatically 3sec after transition)
+            MODE_POS_FT_TRANS == 3 : Transition(Toque->Position)
+            */
             break;
         }
 
@@ -724,23 +721,10 @@
         pos.ref = (double)temp_pos * 4.0f;
         vel.ref = (double)temp_vel * 100.0f;
         torq.ref = (double)temp_torq * 0.1f;
-    } else if(address==CID_RX_REF_TORQUE) {
-        int16_t temp_torq = (int16_t) (msg.data[0] | msg.data[1] << 8);
-        torq.ref = (double)temp_torq;
-    } else if(address==CID_RX_REF_PRES_DIFF) {
-        int16_t temp_presdiff = (int16_t) (msg.data[0] | msg.data[1] << 8);
-        torq.ref = (double)temp_presdiff;   
-    } else if(address==CID_RX_REF_VOUT) {
-        int16_t temp_PWM = (int16_t) (msg.data[0] | msg.data[1] << 8);
-        Vout.ref = (double)temp_PWM;
-    } else if(address==CID_RX_REF_VALVE_POSITION) {
-        int16_t temp_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
-        valve_pos.ref = (double)temp_valve_pos; 
-    } else if(address==CID_RX_REF_CURRENT) {
-        int16_t temp_cur = (int16_t) (msg.data[0] | msg.data[1] << 8);
-        cur.ref = (double)temp_cur; 
-    }
-        
+    } else if(address==CID_RX_REF_PWM) {
+        int16_t temp_vout = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        Vout.ref = (double)temp_vout;
+    }   
 }
 
 /******************************************************************************

--- a/CAN/function_CAN.h	Tue Jan 14 09:11:20 2020 +0000
+++ b/CAN/function_CAN.h	Thu Jan 16 12:05:15 2020 +0000
@@ -9,11 +9,7 @@
 // INIT_CID
 #define INIT_CID_RX_CMD                   100
 #define INIT_CID_RX_REF_POSITION          200
-#define INIT_CID_RX_REF_TORQUE            300
-#define INIT_CID_RX_REF_PRES_DIFF         400
-#define INIT_CID_RX_REF_VOUT              500
-#define INIT_CID_RX_REF_VALVE_POSITION    600
-#define INIT_CID_RX_REF_CURRENT           700
+#define INIT_CID_RX_REF_PWM            300
 
 #define INIT_CID_TX_INFO              1100
 #define INIT_CID_TX_POSITION          1200
@@ -31,7 +27,7 @@
 #define             CRX_SET_OPERATING_MODE          102
 #define             CRX_SET_ENC_ZERO                103
 #define             CRX_SET_FET_ON                  104
-#define             CRX_SET_REF_UPDATE_ENABLE       105
+#define             CRX_SET_POS_TORQ_TRANS          105
 #define             CRX_ASK_CAN_FREQ                6
 #define             CRX_SET_CAN_FREQ                106
 #define             CRX_ASK_CONTROL_MODE            7
@@ -238,3 +234,4 @@
 #endif
 
 
+

--- a/function_utilities/function_utilities.cpp	Tue Jan 14 09:11:20 2020 +0000
+++ b/function_utilities/function_utilities.cpp	Thu Jan 16 12:05:15 2020 +0000
@@ -8,22 +8,6 @@
 int Rom_Sector = 6;
 //FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
 
-
-//int CID_RX_CMD = 100;
-//int CID_RX_REF_POSITION = 200;
-//int CID_RX_REF_TORQUE = 300;
-//int CID_RX_REF_PRES_DIFF = 400;
-//int CID_RX_REF_VOUT = 500;
-//int CID_RX_REF_VALVE_POSITION = 600;
-//int CID_RX_REF_CURRENT = 700;
-//
-//int CID_TX_INFO = 1100;
-//int CID_TX_POSITION = 1200;
-//int CID_TX_TORQUE = 1300;
-//int CID_TX_PRES = 1400;
-//int CID_TX_VOUT = 1500;
-//int CID_TX_VALVE_POSITION = 1600;
-
 /*******************************************************************************
  * VARIABLE
  ******************************************************************************/
@@ -150,6 +134,8 @@
 
 int flag_data_request[5];
 
+int MODE_POS_FT_TRANS = 0;
+
 ////////////////////////////////////////////////////////////////////////////////
 ////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////
@@ -317,6 +303,8 @@
 int FINDHOME_GOTOLIMIT = 1;
 int FINDHOME_ZEROPOSE = 2;
 
+float alpha_trans = 0.0f;
+
 //int h1, h2, h3, h4, h5, h6;
 
 /*******************************************************************************

--- a/main.cpp	Tue Jan 14 09:11:20 2020 +0000
+++ b/main.cpp	Thu Jan 16 12:05:15 2020 +0000
@@ -76,11 +76,7 @@
 
 extern int CID_RX_CMD;
 extern int CID_RX_REF_POSITION;
-extern int CID_RX_REF_TORQUE;
-extern int CID_RX_REF_PRES_DIFF;
-extern int CID_RX_REF_VOUT;
-extern int CID_RX_REF_VALVE_POSITION;
-extern int CID_RX_REF_CURRENT;
+extern int CID_RX_REF_PWM;
 
 extern int CID_TX_INFO;
 extern int CID_TX_POSITION;
@@ -114,7 +110,7 @@
     MODE_VALVE_OPEN_LOOP,                               //1
     MODE_VALVE_POSITION_CONTROL,                        //2
 
-    MODE_JOINT_POSITION_TORQUE_CONTROL_PWM,             //3
+    MODE_JOINT_CONTROL,             //3
     MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION,  //4
     MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING,        //5
 
@@ -269,9 +265,7 @@
         Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
     }
 
-    //VELOCITY_COMP_GAIN = 20;
-    //CAN_TX_PRES((int16_t)(Ref_Valve_Pos_FF), (int16_t) (7));
-    Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + DDV_CENTER;
+    Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
 
     return Ref_Valve_Pos_FF;
 }
@@ -450,10 +444,34 @@
 float FREQ_TMR3 = (float)FREQ_1k;
 //float DT_TMR3 = (float)DT_5k;
 float DT_TMR3 = (float)DT_1k;
+int cnt_trans = 0;
 extern "C" void TIM3_IRQHandler(void)
 {
     if (TIM3->SR & TIM_SR_UIF ) {
         ENC_UPDATE();
+        
+        if(MODE_POS_FT_TRANS == 1){
+            alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
+            cnt_trans++;
+            if(cnt_trans>3*TMR_FREQ_5k)
+                MODE_POS_FT_TRANS = 2;
+        }
+        if(MODE_POS_FT_TRANS == 3){
+            alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
+            cnt_trans++;
+            if(cnt_trans>3*TMR_FREQ_5k)
+                MODE_POS_FT_TRANS = 0;
+        }
+        else if(MODE_POS_FT_TRANS == 2){
+            alpha_trans = 1.0;
+            cnt_trans = 0;
+        }
+        else{
+            alpha_trans = 0.0;
+            cnt_trans = 0;
+        }
+
+            
 
         // CONTROL LOOP ------------------------------------------------------------
 
@@ -473,42 +491,31 @@
                 break;
             }
 
-            case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: {
-                float PWM_RAW_POS_FB = 0.0f; // PWM by Position Feedback
-                float PWM_RAW_POS_FF = 0.0f; // PWM by Position Feedforward
-                float PWM_RAW_FORCE_FB = 0.0f; // PWM by Force Feedback
-
-                // feedback input for position control
-                pos.err = pos.ref - (float) pos.sen;
-                pos.err_diff = pos.err - pos.err_old;
-                pos.err_old = pos.err;
-                pos.err_sum += pos.err;
-                if (pos.err_sum > 1000) pos.err_sum = 1000;
-                if (pos.err_sum<-1000) pos.err_sum = -1000;
-                //            PWM_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff;
-                PWM_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err;
-                PWM_RAW_POS_FB = PWM_RAW_POS_FB * 0.01f;
+            case MODE_JOINT_CONTROL: {
+                
+                float VALVE_POS_RAW_FORCE_FB = 0.0f;
+                
+                pos.err = pos.ref - pos.sen; //[pulse]
+                vel.err = vel.ref - vel.sen; //[pulse/s]
+                double K_spring = 1.0f; //[N/mm]
+                double D_damper = 0.0001f;
+                torq.ref = torq.ref + (K_spring * pos.err + D_damper * vel.err) / ENC_PULSE_PER_POSITION; //[N]
+                
+                // torque feedback
+                torq.err = torq.ref - torq.sen; //[pulse]
+                torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
+                if (torq.err_sum > 1000) torq.err_sum = 1000;
+                if (torq.err_sum<-1000) torq.err_sum = -1000;
+                
 
-                // feedforward input for position control
-                float Ref_Vel_Act = vel.ref/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
-                float K_ff = 0.9f;
-                if(Ref_Vel_Act > 0) K_ff = 0.90f; // open
-                if(Ref_Vel_Act > 0) K_ff = 0.75f; // close
-                PWM_RAW_POS_FF = K_ff*Ref_Vel_Act/0.50f;
+                VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) / (float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION * 0.01f + DDV_JOINT_POS_FF(vel.ref));   
 
-                // torque feedback
-                //            torq.err = torq.ref - torq.sen;
-                //            torq.err_diff = torq.err - torq.err_old;
-                //            torq.err_old = torq.err;
-                //            torq.err_sum += torq.err;
-                //            if (torq.err_sum > 1000) torq.err_sum = 1000;
-                //            if (torq.err_sum<-1000) torq.err_sum = -1000;
-                //            VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
-                //            VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
-
-                PWM_RAW_FORCE_FB = 0.0f;
-
-                V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
+                if (VALVE_POS_RAW_FORCE_FB >= 0) {
+                    valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
+                } else{
+                    valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
+                }
+                VALVE_POS_CONTROL(valve_pos.ref);
 
                 break;
             }
@@ -549,10 +556,10 @@
 
                 valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
 
-                if (valve_pos.ref > DDV_CENTER) {
-                    valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
-                } else if(valve_pos.ref < DDV_CENTER) {
-                    valve_pos.ref = valve_pos.ref - DDV_CENTER + VALVE_DEADZONE_MINUS;
+                if (valve_pos.ref >= 0) {
+                    valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS;
+                } else if(valve_pos.ref < 0) {
+                    valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_MINUS;
                 }
                 VALVE_POS_CONTROL(valve_pos.ref);
 
@@ -651,30 +658,6 @@
             }
 
             case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
-                
-                float VALVE_POS_RAW_FORCE_FB = 0.0f;
-                
-                pos.err = pos.ref - pos.sen; //[pulse]
-                vel.err = vel.ref - vel.sen; //[pulse/s]
-                double K_spring = 1.0f;
-                double D_damper = 0.0001f;
-                torq.ref = (K_spring * pos.err + D_damper * vel.err) * ENC_PULSE_PER_POSITION; //[Nm]
-                
-                // torque feedback
-                torq.err = torq.ref - torq.sen; //[pulse]
-                torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
-                if (torq.err_sum > 100) torq.err_sum = 100;
-                if (torq.err_sum<-100) torq.err_sum = -100;
-                VALVE_POS_RAW_FORCE_FB = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * (float) TORQUE_SENSOR_PULSE_PER_TORQUE;
-
-                valve_pos.ref = DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
-
-                if (valve_pos.ref > DDV_CENTER) {
-                    valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
-                } else if(valve_pos.ref < DDV_CENTER) {
-                    valve_pos.ref = valve_pos.ref - DDV_CENTER + VALVE_DEADZONE_MINUS;
-                }
-                VALVE_POS_CONTROL(valve_pos.ref);
 
                 break;
             }

--- a/setting.h	Tue Jan 14 09:11:20 2020 +0000
+++ b/setting.h	Thu Jan 16 12:05:15 2020 +0000
@@ -24,23 +24,6 @@
 #define DT_20k      0.00005f
 #define DT_40k      0.000025f
 
-//extern int CID_RX_CMD;
-//extern int CID_RX_REF_POSITION;
-//extern int CID_RX_REF_TORQUE;
-//extern int CID_RX_REF_PRES_DIFF;
-//extern int CID_RX_REF_VOUT;
-//extern int CID_RX_REF_VALVE_POSITION;
-//extern int CID_RX_REF_CURRENT;
-//
-//extern int CID_TX_INFO;
-//extern int CID_TX_POSITION;
-//extern int CID_TX_TORQUE;
-//extern int CID_TX_PRES;
-//extern int CID_TX_VOUT;
-//extern int CID_TX_VALVE_POSITION;
-
-
-
 extern DigitalOut check;
 extern DigitalOut check_2;
 extern AnalogOut dac_1;
@@ -244,6 +227,8 @@
 
 extern int flag_data_request[5];
 
+extern int MODE_POS_FT_TRANS;
+
 ////////////////////////////////////////////////////////////////////////////////
 ////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////
@@ -407,6 +392,8 @@
 extern int FINDHOME_GOTOLIMIT;
 extern int FINDHOME_ZEROPOSE;
 
+extern float alpha_trans;
+
 //extern int h1, h2, h3, h4, h5, h6;
 
 
@@ -417,3 +404,5 @@
 
 
 
+
+