Kim GiJeong
eeprom_test
Diff: function_utilities/function_utilities.cpp
- Revision:
- 62:851cf7b7aa7a
- Parent:
- 59:0ad14153b58f
- Child:
- 63:a3c7f31742c9
--- a/function_utilities/function_utilities.cpp Fri Apr 10 02:00:46 2020 +0000
+++ b/function_utilities/function_utilities.cpp Tue May 12 06:23:22 2020 +0000
@@ -15,15 +15,13 @@
// 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;
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;
@@ -38,11 +36,11 @@
float I_GAIN_JOINT_POSITION = 0.0f;
float D_GAIN_JOINT_POSITION = 0.0f;
float P_GAIN_JOINT_TORQUE = 0.0f;
-float I_GAIN_JOINT_TORQUE = 0.0;
-float D_GAIN_JOINT_TORQUE = 0.0;
+float I_GAIN_JOINT_TORQUE = 0.0f;
+float D_GAIN_JOINT_TORQUE = 0.0f;
-int16_t K_SPRING = 0.0;
-int16_t D_DAMPER = 0.0;
+float K_SPRING = 0.0f;
+float D_DAMPER = 0.0f;
int16_t flag_delay_test = 0;
//float P_GAIN_VALVE_POSITION_OPP = 0.0f;
@@ -73,6 +71,7 @@
int16_t PISTON_AREA_A;
int16_t PISTON_AREA_B;
float PISTON_AREA_alpha;
+float alpha3 = 1.0f;
int16_t PRES_SUPPLY;
@@ -84,8 +83,8 @@
int16_t STROKE;
-int16_t VALVE_LIMIT_PLUS;
-int16_t VALVE_LIMIT_MINUS;
+//int16_t VALVE_LIMIT_PLUS;
+//int16_t VALVE_LIMIT_MINUS;
float ENC_PULSE_PER_POSITION;
float TORQUE_SENSOR_PULSE_PER_TORQUE;
@@ -126,7 +125,6 @@
float CUR_TORQUE;
float CUR_PRES_A;
float CUR_PRES_B;
-int CUR_PWM;
int CUR_VALVE_POSITION;
unsigned int TMR2_COUNT_LED1;
@@ -145,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];
@@ -171,34 +164,25 @@
int VALVE_MAX_POS;
int VALVE_MIN_POS;
-float DDV_CENTER;
int VALVE_POS_NUM;
float VALVE_CENTER_OFFSET;
float VALVE_DZ_MINUS_OFFSET;
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;
-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;
+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;
-float TUNING_TIME;
+float TUNING_TIME = 600.0f; // sec
-float REFERENCE_FREQ;
-float REFERENCE_MAG;
+float REFERENCE_FREQ = 1.0f;
+float REFERENCE_MAG = 0.0f;
bool FLAG_FIND_HOME;
@@ -218,8 +202,8 @@
float CUR_PRES_B_mean = 0.0f;
float CUR_TORQUE_sum = 0.0f;
float CUR_TORQUE_mean = 0.0f;
-float PRES_A_NULL = 2048.0f;
-float PRES_B_NULL = 1.0f;
+float PRES_A_NULL = 300.0f;
+float PRES_B_NULL = 300.0f;
float TORQUE_NULL = 3900.0f;
float Ref_Valve_Pos_Old = 0.0f;
@@ -239,7 +223,6 @@
int ID_index_array[50] = {0};
int first_check = 0;
float init_time = 0.0f;
-int VEL_POINT = 0;
int DZ_case = 0;
int START_POS = 0;
int FINAL_POS = 0;
@@ -281,6 +264,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;
@@ -299,11 +284,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};
@@ -314,6 +308,23 @@
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;
+
+float K_LPF = 0.0f;
+float D_LPF = 0.0f;
+
//int h1, h2, h3, h4, h5, h6;
/*******************************************************************************
@@ -363,7 +374,7 @@
{
FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
if (!writer.ready()) writer.open();
-
+ //BNO = 1;
writer.write(RID_BNO,(int) BNO); // write at address, 쓸때도 4byte씩 씀
writer.write(RID_OPERATING_MODE,(int) OPERATING_MODE);
writer.write(RID_SENSING_MODE, (int) SENSING_MODE);
@@ -400,10 +411,10 @@
writer.write(RID_ENC_LIMIT_MINUS,(int) ENC_LIMIT_MINUS);
writer.write(RID_ENC_LIMIT_PLUS,(int) ENC_LIMIT_PLUS);
writer.write(RID_STROKE,(int) STROKE);
- writer.write(RID_VALVE_LIMIT_MINUS,(int) VALVE_LIMIT_MINUS);
- writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS);
+ //writer.write(RID_VALVE_LIMIT_MINUS,(int) VALVE_LIMIT_MINUS);
+ //writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS);
writer.write(RID_ENC_PULSE_PER_POSITION,(int) (ENC_PULSE_PER_POSITION*10.0f));
- writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f));
+ writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f));
writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
writer.write(RID_PRES_SENSOR_B_PULSE_PER_BAR,(int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
writer.write(RID_FRICTION,(int) (FRICTION * 10.0f));
@@ -431,7 +442,7 @@
}
writer.write(RID_VALVE_MAX_POS, (int) VALVE_MAX_POS);
writer.write(RID_VALVE_MIN_POS, (int) VALVE_MIN_POS);
- writer.write(RID_DDV_CENTER, (int) (DDV_CENTER * 10.0f));
+ //writer.write(RID_DDV_CENTER, (int) (DDV_CENTER * 10.0f));
writer.write(RID_VALVE_POS_NUM, (int) VALVE_POS_NUM);
writer.write(RID_K_SPRING, (int) K_SPRING);
@@ -447,8 +458,10 @@
OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE);
CURRENT_CONTROL_MODE = flashReadInt(Rom_Sector, RID_CURRENT_CONTROL_MODE);
+ CURRENT_CONTROL_MODE = 0;
FLAG_VALVE_DEADZONE = flashReadInt(Rom_Sector, RID_FLAG_VALVE_DEADZONE);
CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ);
+ CAN_FREQ = 500;
DIR_JOINT_ENC = flashReadInt(Rom_Sector, RID_JOINT_ENC_DIR);
DIR_VALVE = flashReadInt(Rom_Sector, RID_VALVE_DIR);
DIR_VALVE_ENC = flashReadInt(Rom_Sector, RID_VALVE_ENC_DIR);
@@ -475,16 +488,17 @@
PISTON_AREA_A = flashReadInt(Rom_Sector, RID_PISTON_AREA_A);
PISTON_AREA_B = flashReadInt(Rom_Sector, RID_PISTON_AREA_B);
PISTON_AREA_alpha = (float)PISTON_AREA_B/(float)PISTON_AREA_A;
+ alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
PRES_SUPPLY = flashReadInt(Rom_Sector, RID_PRES_SUPPLY);
PRES_RETURN = flashReadInt(Rom_Sector, RID_PRES_RETURN);
ENC_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_MINUS);
ENC_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_PLUS);
STROKE = flashReadInt(Rom_Sector, RID_STROKE);
- VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
- VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
+ //VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
+ //VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
ENC_PULSE_PER_POSITION = (float) (flashReadInt(Rom_Sector, RID_ENC_PULSE_PER_POSITION)) * 0.1f;
// ENC_PULSE_PER_POSITION = (float) 1024.0f;
- TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.01f;
+ TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f;
//TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle
// TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee
PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
@@ -515,7 +529,7 @@
}
VALVE_MAX_POS = flashReadInt(Rom_Sector, RID_VALVE_MAX_POS);
VALVE_MIN_POS = flashReadInt(Rom_Sector, RID_VALVE_MIN_POS);
- DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
+ //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
VALVE_POS_NUM = flashReadInt(Rom_Sector, RID_VALVE_POS_NUM);
K_SPRING = flashReadInt(Rom_Sector, RID_K_SPRING);
@@ -552,24 +566,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;
}
@@ -579,6 +594,7 @@
spi_enc_set_clear();
CUR_POSITION = 0;
+ pos.sen = 0.0f;
ENC_pos_old = ENC_pos_cur = 0;
}
@@ -588,9 +604,11 @@
spi_enc_set_clear();
enc_offset = value_e;
CUR_POSITION = value_e;
+ pos.sen = (float) value_e;
ENC_pos_old = ENC_pos_cur = value_e;
}
+