Diff: function_utilities/function_utilities.cpp

Revision:

54:647072f5307a

Parent:

52:8ea76864368a

Child:

55:b25725257569

--- a/function_utilities/function_utilities.cpp	Wed Feb 19 05:48:57 2020 +0000
+++ b/function_utilities/function_utilities.cpp	Tue Feb 25 12:56:39 2020 +0000
@@ -15,14 +15,14 @@
 // Board Information
 uint8_t BNO = 0;
 uint8_t CONTROL_MODE = 0;
-uint8_t OPERATING_MODE = 0; // (00 : Moog & Rot, 01 : Moog & Lin, 10 : KNR & Rot, 11 : KNR & Lin)
+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 CONTROL_UTILITY_MODE = 0;
 uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control)
 uint8_t FLAG_VALVE_DEADZONE = 0;
-uint8_t SETTING_SWITCH = 0;
-uint8_t SETTING_SWITCH_OLD = 0;
 uint8_t REFERENCE_MODE = 0;
-uint16_t CAN_FREQ = 500;
+int16_t CAN_FREQ = 500;
+int CAN_FREQUENCY = 500;
 int16_t DIR_JOINT_ENC = 0;
 int16_t DIR_VALVE = 0;
 int16_t DIR_VALVE_ENC = 0;
@@ -42,6 +42,7 @@
 
 int16_t K_SPRING = 0.0;
 int16_t D_DAMPER = 0.0;
+int16_t flag_delay_test = 0;
 
 //float P_GAIN_VALVE_POSITION_OPP = 0.0f;
 //float I_GAIN_VALVE_POSITION_OPP= 0.0f;
@@ -124,12 +125,12 @@
 float CUR_TORQUE;
 float CUR_PRES_A;
 float CUR_PRES_B;
-int CUR_PWM;
 int CUR_VALVE_POSITION;
 
 unsigned int    TMR2_COUNT_LED1;
 unsigned int    TMR2_COUNT_LED2;
-unsigned int    TMR2_COUNT_CAN_TX;
+unsigned int    TMR2_COUNT_CAN_TX = 0;
+unsigned int    TMR3_COUNT_TEST = 0;
 
 int num_err;
 int flag_err[8];
@@ -142,25 +143,20 @@
 
 int MODE_POS_FT_TRANS = 0;
 
-////////////////////////////////////////////////////////////////////////////////
-////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////
-
-float CUR_PRES_A_BAR;
-float CUR_PRES_B_BAR;
-float CUR_TORQUE_NM;
-float CUR_TORQUE_NM_PRESS;
+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 PRES_A_VREF;
-float PRES_A_VREF_TEST;
-float PRES_B_VREF;
-float PRES_B_VREF_TEST;
-float TORQUE_VREF;
+float PRES_A_VREF = 0.0f;
+float PRES_B_VREF = 0.0f;
+float TORQUE_VREF = 0.0f;
 
-float VALVE_PWM_RAW_FB;
-float VALVE_PWM_RAW_FF;
-int VALVE_PWM_VALVE_DZ;
-int VALVE_INPUT_PWM;
+float VALVE_PWM_RAW_FB = 0.0f;
+float VALVE_PWM_RAW_FF = 0.0f;
+float VALVE_PWM_RAW = 0.0f;
+int VALVE_PWM_VALVE_DZ = 0;
 
 float VALVE_GAIN_LPM_PER_V[10];
 float VALVE_POS_VS_PWM[25];
@@ -175,27 +171,20 @@
 float VALVE_DZ_PLUS_OFFSET;
 int VALVE_CENTER_OFFSET_times10;
 
-int TMR3_COUNT_FINDHOME;
-int TMR3_COUNT_FLOWRATE;
-int TMR3_COUNT_DEADZONE;
-int TMR3_COUNT_PRES_NULL;
-int TMR3_COUNT_TORQUE_NULL;
-int TMR3_COUNT_PRES_CALIB;
-int TMR3_COUNT_REFERENCE;
-int TMR3_COUNT_JOINT;
-int TMR3_COUNT_ROTARY_FRIC_TUNE;
+int TMR3_COUNT_FINDHOME = 0;
+int TMR3_COUNT_FLOWRATE = 0;
+int TMR3_COUNT_DEADZONE = 0;
+int TMR3_COUNT_PRES_NULL = 0;
+int TMR3_COUNT_TORQUE_NULL = 0;
+int TMR3_COUNT_PRES_CALIB = 0;
+int TMR3_COUNT_REFERENCE = 0;
+int TMR3_COUNT_JOINT = 0;
+int TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
 
-bool FLAG_REFERENCE_VALVE_PWM;
-bool FLAG_REFERENCE_VALVE_POSITION;
-bool FLAG_REFERENCE_JOINT_POSITION;
-bool FLAG_REFERENCE_JOINT_TORQUE;
-bool FLAG_REFERENCE_PRES_DIFF;
-bool FLAG_REFERENCE_CURRENT;
+float TUNING_TIME = 600.0f;  // sec
 
-float TUNING_TIME;
-
-float REFERENCE_FREQ;
-float REFERENCE_MAG;
+float REFERENCE_FREQ = 1.0f;
+float REFERENCE_MAG = 0.0f;
 
 bool FLAG_FIND_HOME;
 
@@ -278,6 +267,8 @@
 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;
@@ -296,11 +287,20 @@
 
 float freq_fric_tune = 1.0f;
 
-bool FLAG_VALVE_OUTPUT_CALIB = false;
-
 uint32_t TMR3_COUNT_CAN_TX = 0;
 
-float I_REF = 0.0f;
+// Current Control Variables
+double I_REF = 0.0f;
+double I_REF_fil = 0.0f;
+double I_ERR = 0.0f;
+double I_ERR_INT = 0.0f;
+double I_REF_fil_old = 0.0f;
+double I_REF_fil_diff = 0.0f;
+
+// system id
+int cnt_sysid = 0;
+double freq_sysid_Iref = 0.0f;
+
 int TMR3_COUNT_IREF = 0;
 float CUR_CURRENT = 0.0f;
 float u_CUR[3] = {0.0f,0.0f,0.0f};
@@ -311,6 +311,20 @@
 
 float alpha_trans = 0.0f;
 
+float V_out=0.0f;
+float V_rem=0.0f; // for anti-windup
+float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
+
+float PWM_out=0.0f;
+
+double K_v = 0.0f; // valve flowrate gain
+double mV_PER_mA = 600.0f; // current >> voltage
+double mV_PER_pulse = 0.6f; // pulse >> voltage
+double mA_PER_pulse = 0.001f; // pulse >> current
+
+int timer_while = 0;
+int while_index = 0;
+
 //int h1, h2, h3, h4, h5, h6;
 
 /*******************************************************************************
@@ -549,24 +563,25 @@
     ENC_pos_cur = spi_enc_read();
     ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
 
-    if (ENC_pos_diff > 1300 || ENC_pos_diff<-1300) {
-        //MOT_E_STOP(0);
-    }
-
-    ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k);
-
-    KF_Y_11 = ENC_pos_cur;
-    KF_Y_21 = ENC_VEL_RAW;
-    KF_X_11 = KF_G1_11 * KF_X_11 + KF_G1_12 * KF_X_21 + KF_G2_11 * KF_Y_11 + KF_G2_12*KF_Y_21;
-    KF_X_21 = KF_G1_21 * KF_X_11 + KF_G1_22 * KF_X_21 + KF_G2_21 * KF_Y_11 + KF_G2_22*KF_Y_21;
-    ENC_VEL_KF = (int32_t) KF_X_21;
-
-    pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
-    //    CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur;
-    vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF;
-
-    //    CUR_POSITION = ENC_pos_cur;
-    //    CUR_VELOCITY = ENC_VEL_KF;
+    //Kalman Filter
+//    ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k);
+//    KF_Y_11 = ENC_pos_cur;
+//    KF_Y_21 = ENC_VEL_RAW;
+//    KF_X_11 = KF_G1_11 * KF_X_11 + KF_G1_12 * KF_X_21 + KF_G2_11 * KF_Y_11 + KF_G2_12*KF_Y_21;
+//    KF_X_21 = KF_G1_21 * KF_X_11 + KF_G1_22 * KF_X_21 + KF_G2_21 * KF_Y_11 + KF_G2_22*KF_Y_21;
+//    ENC_VEL_KF = (int32_t) KF_X_21;
+//
+//    pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
+//    vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF;
+    
+    //Low Pass Filter
+    
+    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);
+    
+    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;
 }