Diff: function_utilities/function_utilities.cpp

Revision:

230:2c3e5ecbe7e1

Parent:

227:699c3e572283

Child:

231:e00e71ca3e80

diff -r 7a1c46b9b471 -r 2c3e5ecbe7e1 function_utilities/function_utilities.cpp
--- a/function_utilities/function_utilities.cpp	Tue Apr 20 00:42:45 2021 +0000
+++ b/function_utilities/function_utilities.cpp	Wed Apr 21 04:20:39 2021 +0000
@@ -14,6 +14,8 @@
 uint8_t CONTROL_MODE = 0;
 uint8_t OPERATING_MODE = 0; // (00 : Moog & Rot, 01 : Moog & Lin, 10 : KNR & Rot, 11 : KNR & Lin, 101 : SW & Lin)
 uint8_t SENSING_MODE = 0; // (0 : torque, 1: pressure)
+uint8_t SUPPLY_PRESSURE_UPDATE = 0; // (0 : Update Off (constant Ps) , 1 : Update On (variable Ps))
+
 uint8_t CONTROL_UTILITY_MODE = 0;
 uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control)
 uint8_t FLAG_VALVE_DEADZONE = 0;
@@ -74,9 +76,8 @@
 float PISTON_AREA_alpha;
 float alpha3 = 1.0f;
 
-
-int16_t PRES_SUPPLY;
-int16_t PRES_RETURN;
+float PRES_SUPPLY_NOM = 70.0f;
+float PRES_SUPPLY = 70.0f;
 
 int16_t ENC_LIMIT_PLUS;
 int16_t ENC_LIMIT_MINUS;
@@ -96,8 +97,8 @@
 //int16_t VALVE_LIMIT_PLUS;
 //int16_t VALVE_LIMIT_MINUS;
 
-float ENC_PULSE_PER_POSITION;
-float TORQUE_SENSOR_PULSE_PER_TORQUE;
+float ENC_PULSE_PER_POSITION = 1.0f;
+float TORQUE_SENSOR_PULSE_PER_TORQUE = 1.0f;
 float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
 float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
 
@@ -113,11 +114,13 @@
 float DAC_REF;
 float DAC_RESOL;
 
-float REF_POSITION;
-float REF_VELOCITY;
-float REF_TORQUE;
-float REF_TORQUE_OLD;
-float REF_PRES_DIFF;
+float REF_FORCE = 0.0;
+float REF_TORQUE = 0.0;
+float REF_POSITION = 0.0;
+float REF_VELOCITY = 0.0;
+
+float REF_POSITION_FINDHOME = 0.0;
+
 int16_t REF_PWM;
 int16_t REF_VALVE_POSITION;
 int16_t REF_CURRENT;
@@ -125,19 +128,11 @@
 int REF_MOVE_TIME_5k;
 int INIT_REF_PWM;
 int INIT_REF_VALVE_POS;
-int32_t INIT_REF_POS;
 int INIT_REF_VEL;
 int INIT_REF_TORQUE;
 int INIT_REF_PRES_DIFF;
 int INIT_REF_CURRENT;
 
-int CUR_POSITION;
-int CUR_VELOCITY;
-float CUR_TORQUE;
-float CUR_PRES_A;
-float CUR_PRES_B;
-int CUR_VALVE_POSITION;
-
 unsigned int    TMR2_COUNT_LED1;
 unsigned int    TMR2_COUNT_LED2;
 unsigned int    TMR2_COUNT_CAN_TX = 0;
@@ -158,14 +153,12 @@
 int cnt_buffer = 0;
 
 float CUR_CURRENT_mA = 0.0f;
-float CUR_PRES_A_BAR = 0.0f;
-float CUR_PRES_B_BAR = 0.0f;
 float CUR_TORQUE_NM = 0.0f;
 float CUR_TORQUE_NM_PRESS = 0.0f;
 
+float FORCE_VREF = 0.0f;
 float PRES_A_VREF = 0.0f;
 float PRES_B_VREF = 0.0f;
-float TORQUE_VREF = 0.0f;
 
 float VALVE_PWM_RAW_FB = 0.0f;
 float VALVE_PWM_RAW_FF = 0.0f;
@@ -209,16 +202,10 @@
     JUMP_LANDING,                                  //4
 };
 
-float CUR_PRES_DIFF_BAR = 0.0f;
-float CUR_PRES_A_sum = 0.0f;
-float CUR_PRES_B_sum = 0.0f;
-float CUR_PRES_A_mean = 0.0f;
-float CUR_PRES_B_mean = 0.0f;
-float CUR_TORQUE_sum = 0.0f;
-float CUR_TORQUE_mean = 0.0f;
-float PRES_A_NULL = 300.0f;
-float PRES_B_NULL = 300.0f;
-float TORQUE_NULL = 3900.0f;
+
+float PRES_A_NULL_pulse = 300.0f;
+float PRES_B_NULL_pulse = 300.0f;
+float FORCE_NULL_pulse = 3900.0f;
 
 float Ref_Valve_Pos_Old = 0.0f;
 
@@ -277,17 +264,6 @@
 int fl_temp_cnt2 = 0;
 int cur_vel_sum = 0;
 
-float Cur_Valve_Open_pulse = 0.0f;
-
-// find home
-int CUR_VELOCITY_OLD = 0;
-int cnt_findhome = 0;
-int cnt_vel_findhome = 0;
-int FINDHOME_VELOCITY = 0;
-int FINDHOME_VELOCITY_OLD = 0;
-int FINDHOME_POSITION = 0;
-int FINDHOME_POSITION_OLD = 0;
-
 int cnt_finddz = 0;
 int cnt_vel_finddz = 0;
 int flag_finddz = 0;
@@ -357,7 +333,9 @@
 
 float PWM_out=0.0f;
 
-double K_v = 0.0f; // valve flowrate gain
+double K_v = 1.0f; // valve flowrate gain 1
+double C_d = 0.16f; // valve flowrate gain 2
+
 double mV_PER_mA = 600.0f; // current >> voltage
 double mV_PER_pulse = 0.6f; // pulse >> voltage
 double mA_PER_pulse = 0.001f; // pulse >> current
@@ -453,8 +431,8 @@
     PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
     PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
     alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
-    PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY);
-    PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
+    PRES_SUPPLY_NOM = spi_eeprom_read(RID_PRES_SUPPLY);
+    PRES_SUPPLY = PRES_SUPPLY_NOM;
     ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
     ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
     STROKE = spi_eeprom_read(RID_STROKE);
@@ -467,7 +445,7 @@
     FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
     HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
     HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
-    TORQUE_VREF = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_VREF)) *0.001f;
+    FORCE_VREF = (float) (spi_eeprom_read(RID_FORCE_SENSOR_VREF)) *0.001f;
     PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f;
     PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f;
     VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f;
@@ -497,62 +475,38 @@
 
 /*******************************************************************************
  * ENCODER functions
- 
+
  ******************************************************************************/
-// A-KHA
-#define     KF_G1_11    0.083920206005350f
-#define     KF_G1_12    0.000013905329560f
-#define     KF_G1_21    -0.000575742328210f
-#define     KF_G1_22    0.799999939711725f
-// K
-#define     KF_G2_11    0.916079793994650f
-#define     KF_G2_12    0.000002878711641f
-#define     KF_G2_21    0.000575742328210f
-#define     KF_G2_22    0.199999945139809f
-
-float KF_Y_11 = 0.0f;
-float KF_Y_21 = 0.0f;
-float KF_X_11 = 0.0f;
-float KF_X_21 = 0.0f;
-
-long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
-long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
-long enc_offset = 0;
+long ENC_pulse = 0, ENC_pulse_old = 0, ENC_pulse_diff = 0;
+long ENC_pulse_offset = 0;
 
 void ENC_UPDATE(void)
 {
-
-    ENC_pos_cur = spi_enc_read();
-    ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
-
-    //Low Pass Filter
+    ENC_pulse = spi_enc_read(); // Unit : pulse
+    ENC_pulse_diff = ENC_pulse - ENC_pulse_old;
 
-    double NEW_POSITION = (double) ((DIR_JOINT_ENC) * ENC_pos_cur + enc_offset);
-    double NEW_VELOCITY = (double) ((DIR_JOINT_ENC) * ENC_pos_diff * (int) FREQ_10k);
+    pos.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse + ENC_pulse_offset)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg or mm
+    vel.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse_diff * (long)FREQ_10k)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg/s or mm/s
 
-    double alpha_update_pos = 1.0f/(1.0f + FREQ_10k/(2.0f*3.14f*100.0f));
-    pos.sen = NEW_POSITION;
-    vel.sen = (1.0f - alpha_update_pos) * vel.sen + alpha_update_pos * NEW_VELOCITY; // pulse/s
-
-    ENC_pos_old = ENC_pos_cur;
+    ENC_pulse_old = ENC_pulse;
 }
 
 void ENC_SET_ZERO(void)
 {
-
     spi_enc_set_clear();
-    CUR_POSITION = 0;
-    ENC_pos_old = ENC_pos_cur = 0;
-
+    pos.Reset();
+    ENC_pulse_offset = 0;
+    ENC_pulse = ENC_pulse_old = ENC_pulse_diff = 0;
 }
 
-void ENC_SET(int32_t value_e)
+void ENC_SET(long value_e)
 {
     spi_enc_set_clear();
-    enc_offset = value_e;
-    CUR_POSITION = value_e;
-    ENC_pos_old = ENC_pos_cur = value_e;
-
+    ENC_pulse_offset = value_e;
+    pos.Reset();
+    pos.sen = value_e/ENC_PULSE_PER_POSITION;
+    ENC_pulse = ENC_pulse_old = value_e;
+    ENC_pulse_diff = 0;
 }