Sungwoo Kim
/
HydraulicControlBoard_PostLIGHT_Original
Original Version of STM Board
Diff: function_utilities/function_utilities.cpp
- Revision:
- 177:8e9cf31d63f4
- Parent:
- 174:c828479f53f9
- Child:
- 179:d5377766d7ea
diff -r 589ea3edcf3c -r 8e9cf31d63f4 function_utilities/function_utilities.cpp --- a/function_utilities/function_utilities.cpp Tue Nov 24 10:16:10 2020 +0000 +++ b/function_utilities/function_utilities.cpp Tue Nov 24 13:04:54 2020 +0000 @@ -530,7 +530,7 @@ writer.write(RID_VALVE_MIN_POS, (int) VALVE_MIN_POS); //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); writer.write(RID_D_DAMPER, (int) D_DAMPER); @@ -619,24 +619,24 @@ VALVE_MIN_POS = spi_eeprom_read(RID_VALVE_MIN_POS); //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f; VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM); - + K_SPRING = spi_eeprom_read(RID_K_SPRING); D_DAMPER = spi_eeprom_read(RID_D_DAMPER); - - - - - + + + + + /* - + BNO = flashReadInt(Rom_Sector, RID_BNO); -// BNO = 1; + // BNO = 1; OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE); -// OPERATING_MODE = 5; + // OPERATING_MODE = 5; SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE); -// SENSING_MODE = 0; + // SENSING_MODE = 0; CURRENT_CONTROL_MODE = flashReadInt(Rom_Sector, RID_CURRENT_CONTROL_MODE); -// CURRENT_CONTROL_MODE = 0; + // CURRENT_CONTROL_MODE = 0; FLAG_VALVE_DEADZONE = flashReadInt(Rom_Sector, RID_FLAG_VALVE_DEADZONE); CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ); DIR_JOINT_ENC = flashReadInt(Rom_Sector, RID_JOINT_ENC_DIR); @@ -674,14 +674,14 @@ //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; + // ENC_PULSE_PER_POSITION = (float) 1024.0f; 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 + // 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; -// PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f; + // PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f; PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f; -// PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f; + // PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f; FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f; HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET); HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING); @@ -708,14 +708,14 @@ VALVE_MIN_POS = flashReadInt(Rom_Sector, RID_VALVE_MIN_POS); //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); D_DAMPER = flashReadInt(Rom_Sector, RID_D_DAMPER); - -// ROM_RESET_DATA(); + + // ROM_RESET_DATA(); */ - + } /******************************************************************************* @@ -757,12 +757,12 @@ // // 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