rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
Lightvalve
Date:
Fri Jul 30 06:04:10 2021 +0000
Parent:
238:4e660aa77eb7
Child:
240:fb5b57e3f157
Child:
257:c93d3eabff75
Commit message:
210730

Changed in this revision

SPI_EEP_ENC/SPI_EEP_ENC.h Show annotated file Show diff for this revision Revisions of this file
function_utilities/function_utilities.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
setting.h Show annotated file Show diff for this revision Revisions of this file
--- a/SPI_EEP_ENC/SPI_EEP_ENC.h	Tue Jul 13 01:40:56 2021 +0000
+++ b/SPI_EEP_ENC/SPI_EEP_ENC.h	Fri Jul 30 06:04:10 2021 +0000
@@ -91,7 +91,7 @@
 #define             RID_VALVE_POS_NUM                   54
 //#define             RID_DDV_CENTER                      55
 #define             RID_VALVE_CENTER_OFFSET             56
-#define             RID_VALVE_ELECTRIC_CENTER           57
+#define             RID_VALVE_ELECTRIC_CENTER           59
 
 #define             RID_VALVE_GAIN_PLUS_1              60
 #define             RID_VALVE_GAIN_MINUS_1             61
--- a/function_utilities/function_utilities.cpp	Tue Jul 13 01:40:56 2021 +0000
+++ b/function_utilities/function_utilities.cpp	Fri Jul 30 06:04:10 2021 +0000
@@ -41,8 +41,8 @@
 float I_GAIN_JOINT_TORQUE_FF = 0.0f;
 float D_GAIN_JOINT_TORQUE_FF = 0.0f;
 
-int16_t K_SPRING = 0.0;
-int16_t D_DAMPER = 0.0;
+int16_t K_SPRING = 0;
+int16_t D_DAMPER = 0;
 
 int16_t flag_delay_test = 0;
 
@@ -242,6 +242,7 @@
 float joint_pos_err = 0.0f, joint_pos_err_old = 0.0f, joint_pos_err_diff = 0.0f, joint_pos_err_diff_fil = 0.0f, joint_pos_err_sum = 0.0f;
 float joint_torq_err = 0.0f, joint_torq_err_old = 0.0f, joint_torq_err_diff = 0.0f, joint_torq_err_sum = 0.0f;
 float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f;
+float force_ref_filter = 0.0f;
 
 float CUR_FLOWRATE = 0.0f;
 float VALVE_FF_VOLTAGE = 0.0f;
--- a/main.cpp	Tue Jul 13 01:40:56 2021 +0000
+++ b/main.cpp	Fri Jul 30 06:04:10 2021 +0000
@@ -526,6 +526,7 @@
 int cnt_trans = 0;
 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
 int can_rest =0;
+float force_ref_act_can = 0.0f;
 
 extern "C" void TIM3_IRQHandler(void)
 {
@@ -1118,7 +1119,7 @@
                     VALVE_POS_CONTROL_DZ(valve_pos.ref);
                     V_out = Vout.ref;
                 } else if (CURRENT_CONTROL_MODE == 0) { //PWM
-                    V_out = valve_pos.ref;
+                    I_REF = valve_pos.ref;
                 } else {
                     I_REF = valve_pos.ref * 0.001f; // Unit : pulse >> mA
                     float I_MAX = 10.0f; // Max : 10mA
@@ -1164,8 +1165,12 @@
                     temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
                 } else {
                     float force_ref_act = force.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : N
-                    force.err = force_ref_act - force.sen;
-//                    force.err = 0.0f - force.sen;
+                    //////////////////////////////////////////////////force_reference_filter////////////////////////////////////////////////////////////////////
+                    float alpha_torque_ref = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*1.0f));
+                    force_ref_filter = (1.0f-alpha_torque_ref) * force_ref_filter + alpha_torque_ref * force_ref_act;
+                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+                    force_ref_act_can = force_ref_filter;
+                    force.err = force_ref_filter - force.sen;
                     force.err_int += force.err/((float)TMR_FREQ_5k);
                     temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
                 }
@@ -1367,7 +1372,7 @@
                 // Moog Valve Current Control Gain
                 double R_model = 500.0f; // ohm
                 double L_model = 1.2f;
-                double w0 = 2.0f * 3.14f * 150.0f;
+                double w0 = 2.0f * 3.14f * 100.0f;
                 double KP_I = 0.1f * L_model*w0;
                 double KI_I = 0.1f * R_model*w0;
 
@@ -1478,8 +1483,8 @@
 
             // Valve Position (ID:1300)
             if (flag_data_request[1] == HIGH) {
-//                CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
-                CAN_TX_PWM((int16_t)(alpha3));
+                CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
+//                CAN_TX_PWM((int16_t)(alpha3));
             }
 
             // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
@@ -1499,7 +1504,7 @@
 
                 valve_pos_ref_can = (float)valve_pos.ref;
 
-                CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_ref_can);
+                CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) force_ref_act_can);
             }
 
             TMR2_COUNT_CAN_TX = 0;
--- a/setting.h	Tue Jul 13 01:40:56 2021 +0000
+++ b/setting.h	Fri Jul 30 06:04:10 2021 +0000
@@ -335,6 +335,7 @@
 extern float joint_pos_err, joint_pos_err_old, joint_pos_err_diff, joint_pos_err_diff_fil, joint_pos_err_sum;
 extern float joint_torq_err, joint_torq_err_old, joint_torq_err_diff, joint_torq_err_sum;
 extern float VALVE_PWM_RAW_POS, VALVE_PWM_RAW_TORQ;
+extern float force_ref_filter;
 
 extern float CUR_FLOWRATE;
 extern float VALVE_FF_VOLTAGE;