Yeseong Jeong
20210203
Diff: function_utilities/function_utilities.cpp
- Revision:
- 224:985dba42f261
- Parent:
- 218:066030f7951f
- Child:
- 225:9c0becc196ba
--- a/function_utilities/function_utilities.cpp Mon Dec 28 12:10:50 2020 +0000
+++ b/function_utilities/function_utilities.cpp Mon Dec 28 14:27:11 2020 +0000
@@ -5,9 +5,6 @@
#include "stm32f4xx_flash.h"
#include "FlashWriter.h"
-int Rom_Sector = 6;
-//FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
-
/*******************************************************************************
* VARIABLE
******************************************************************************/
@@ -249,7 +246,6 @@
int one_period_end = 0;
float Ref_Vel_Test = 0.0f;
long TMR2_FOR_SLOW_LOGGING = 0;
-//int velcount = 0;
char max_check = 0;
char min_check = 0;
@@ -351,71 +347,10 @@
float K_LPF = 0.0f;
float D_LPF = 0.0f;
-float totq_sen_past = 0.0f;
+float torq_sen_past = 0.0f;
float torq_ref_past = 0.0f;
float output_normalized = 0.0f;
-int batch = 0;
-float train_set_x[batch_size] = {0.0f};
-float train_set_error[batch_size] = {0.0f};
-float train_set_count[batch_size] = {0.0f};
-float state_array[batch_size][num_input_RL] = {0.0f};
-float V[batch_size] = {0.0f};
-float r[batch_size] = {0.0f};
-float td_target[batch_size] = {0.0f};
-float delta[batch_size] = {0.0f};
-float advantage[batch_size] = {0.0f};
-float return_G[batch_size] = {0.0f};
-float mean = 0.0f;
-float deviation = 0.0f;
-float mean_old = 0.0f;
-float deviation_old = 0.0f;
-float mean_before_SP = 0.0f;
-float deviation_before_SP = 0.0f;
-float mean_before_SP_array[batch_size] = {0.0f};
-float deviation_before_SP_array[batch_size] = {0.0f};
-float mean_array[batch_size] = {0.0f};
-float mean_array_old[batch_size] = {0.0f};
-float deviation_array[batch_size] = {0.0f};
-float deviation_array_old[batch_size] = {0.0f};
-
-float hx_c_sum[num_hidden_unit1] = {0.0f};
-float hx_c_sum_array[batch_size][num_hidden_unit1] = {0.0f};
-float hxh_c_sum[num_hidden_unit2] = {0.0f};
-float hxh_c_sum_array[batch_size][num_hidden_unit2] = {0.0f};
-float hxhh_c_sum = 0.0f;
-float hxhh_c_sum_array[batch_size] = {0.0f};
-
-float hx_a_sum[num_hidden_unit1] = {0.0f};
-float hx_a_sum_array[batch_size][num_hidden_unit1] = {0.0f};
-float hxh_a_sum[num_hidden_unit2] = {0.0f};
-float hxh_a_sum_array[batch_size][num_hidden_unit2] = {0.0f};
-float hxhh_a_sum[2] = {0.0f};
-float hxhh_a_sum_array[batch_size][2] = {0.0f};
-
-float action = 0.0f;
-float action_array[batch_size] = {0.0f};
-float ratio[batch_size] = {1.0f};
-float pi[batch_size] = {0.0f};
-float pi_old[batch_size] = {0.0f};
-float epsilon = 0.2f;
-float surr1[batch_size] = {0.0f};
-float surr2[batch_size] = {0.0f};
-float loss[batch_size] = {0.0f};
-float loss_batch = 0.0f;
-float gamma = 0.996f;
-float lmbda = 0.95f;
-char Update_Done_Flag = 1;
-char Update_Case = 0;
-float reward_sum = 0.0f;
-
-float virt_pos = 0.0f;
-float logging1 = 0.0f;
-float logging2 = 0.0f;
-float logging3 = 0.0f;
-float logging4 = 0.0f;
-float logging5 = 0.0f;
-
/*******************************************************************************
* General math functions
@@ -460,87 +395,6 @@
/*******************************************************************************
* ROM functions
******************************************************************************/
-void ROM_RESET_DATA(void)
-{
- FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
- if (!writer.ready()) writer.open();
- 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);
- writer.write(RID_CURRENT_CONTROL_MODE, (int) CURRENT_CONTROL_MODE);
- writer.write(RID_FLAG_VALVE_DEADZONE, (int) FLAG_VALVE_DEADZONE);
- writer.write(RID_CAN_FREQ,(int) CAN_FREQ);
- writer.write(RID_JOINT_ENC_DIR,(int) DIR_JOINT_ENC);
- writer.write(RID_VALVE_DIR,(int) DIR_VALVE);
- writer.write(RID_VALVE_ENC_DIR,(int) DIR_VALVE_ENC);
- writer.write(RID_VOLATGE_SUPPLY,(int) (SUPPLY_VOLTAGE * 10.0f));
- writer.write(RID_VOLTAGE_VALVE,(int) (VALVE_VOLTAGE_LIMIT * 10.0f));
- writer.write(RID_P_GAIN_VALVE_POSITION,(int) P_GAIN_VALVE_POSITION);
- writer.write(RID_I_GAIN_VALVE_POSITION,(int) I_GAIN_VALVE_POSITION);
- writer.write(RID_D_GAIN_VALVE_POSITION,(int) D_GAIN_VALVE_POSITION);
- writer.write(RID_P_GAIN_JOINT_POSITION,(int) P_GAIN_JOINT_POSITION);
- writer.write(RID_I_GAIN_JOINT_POSITION,(int) I_GAIN_JOINT_POSITION);
- writer.write(RID_D_GAIN_JOINT_POSITION,(int) D_GAIN_JOINT_POSITION);
- writer.write(RID_P_GAIN_JOINT_TORQUE,(int) P_GAIN_JOINT_TORQUE);
- writer.write(RID_I_GAIN_JOINT_TORQUE,(int) I_GAIN_JOINT_TORQUE);
- writer.write(RID_D_GAIN_JOINT_TORQUE,(int) D_GAIN_JOINT_TORQUE);
- writer.write(RID_VALVE_DEADZONE_PLUS,(int) (VALVE_DEADZONE_PLUS * 10.0f));
- writer.write(RID_VALVE_DEADZONE_MINUS,(int) (VALVE_DEADZONE_MINUS * 10.0f));
- writer.write(RID_VELOCITY_COMP_GAIN,(int) VELOCITY_COMP_GAIN);
- writer.write(RID_COMPLIANCE_GAIN,(int) COMPLIANCE_GAIN);
- writer.write(RID_VALVE_CNETER,(int) VALVE_CENTER);
- writer.write(RID_VALVE_FF,(int) VALVE_FF);
- writer.write(RID_BULK_MODULUS,(int) BNO);
- writer.write(RID_CHAMBER_VOLUME_A,(int) CHAMBER_VOLUME_A);
- writer.write(RID_CHAMBER_VOLUME_B,(int) CHAMBER_VOLUME_B);
- writer.write(RID_PISTON_AREA_A,(int) PISTON_AREA_A);
- writer.write(RID_PISTON_AREA_B,(int) PISTON_AREA_B);
- writer.write(RID_PRES_SUPPLY,(int) PRES_SUPPLY);
- writer.write(RID_PRES_RETURN,(int) PRES_RETURN);
- 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_ENC_PULSE_PER_POSITION,(int) (ENC_PULSE_PER_POSITION*10.0f));
- writer.write(RID_ENC_PULSE_PER_POSITION,(int) (ENC_PULSE_PER_POSITION));
- 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));
- writer.write(RID_HOMEPOS_OFFSET,(int) HOMEPOS_OFFSET);
- writer.write(RID_HOMEPOS_VALVE_OPENING,(int) HOMEPOS_VALVE_OPENING);
- writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0f));
- writer.write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0f));
- writer.write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0f));
- writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0f));
- writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0f));
- writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0f));
- writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0f));
- writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0f));
- writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0f));
- writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0f));
- writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0f));
- writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0f));
- writer.write(RID_VALVE_GAIN_MINUS_5,(int) (VALVE_GAIN_LPM_PER_V[9] * 100.0f));
- for(int i=0; i<25; i++) {
- writer.write(RID_VALVE_POS_VS_PWM_0 + i, (int) VALVE_POS_VS_PWM[i]);
- }
- for(int i=0; i<100; i++) {
- writer.write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int) (JOINT_VEL[i] & 0xFFFF));
- writer.write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int) ((JOINT_VEL[i] >> 16) & 0xFFFF));
- }
- 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_VALVE_POS_NUM, (int) VALVE_POS_NUM);
-
- writer.write(RID_K_SPRING, (int) K_SPRING);
- writer.write(RID_D_DAMPER, (int) D_DAMPER);
-
- writer.close();
-
-}
void ROM_CALL_DATA(void)
{
@@ -548,12 +402,9 @@
BNO = 1;
OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
SENSING_MODE = spi_eeprom_read(RID_SENSING_MODE);
-// SENSING_MODE = 1;
CURRENT_CONTROL_MODE = spi_eeprom_read(RID_CURRENT_CONTROL_MODE);
-// CURRENT_CONTROL_MODE = 1;
FLAG_VALVE_DEADZONE = spi_eeprom_read(RID_FLAG_VALVE_DEADZONE);
CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
-// CAN_FREQ = 500;
DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
@@ -586,13 +437,8 @@
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);
- //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) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION)) * 0.1f;
ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION));
TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(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) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
// PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
@@ -621,109 +467,16 @@
}
VALVE_MAX_POS = spi_eeprom_read(RID_VALVE_MAX_POS);
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;
- OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
- // OPERATING_MODE = 5;
- SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE);
- // SENSING_MODE = 0;
- 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);
- 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);
- SUPPLY_VOLTAGE = (float) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1f;
- VALVE_VOLTAGE_LIMIT = (float) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1f;
- P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION);
- I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION);
- D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION);
- P_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_POSITION);
- I_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_POSITION);
- D_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_POSITION);
- P_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_TORQUE);
- I_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_TORQUE);
- D_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_TORQUE);
- VALVE_DEADZONE_PLUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_PLUS)) * 0.1f;
- VALVE_DEADZONE_MINUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_MINUS)) * 0.1f;
- VELOCITY_COMP_GAIN = flashReadInt(Rom_Sector, RID_VELOCITY_COMP_GAIN);
- COMPLIANCE_GAIN = flashReadInt(Rom_Sector, RID_COMPLIANCE_GAIN);
- VALVE_CENTER = flashReadInt(Rom_Sector, RID_VALVE_CNETER);
- VALVE_FF = flashReadInt(Rom_Sector, RID_VALVE_FF);
- BULK_MODULUS = flashReadInt(Rom_Sector, RID_BULK_MODULUS);
- CHAMBER_VOLUME_A = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_A);
- CHAMBER_VOLUME_B = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_B);
- 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_A/(float)PISTON_AREA_B;
- 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);
- 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.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;
- // 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;
- 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);
- TORQUE_VREF = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001f;
- PRES_A_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_A_SENSOR_VREF) * 0.001f;
- PRES_B_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_B_SENSOR_VREF) * 0.001f;
- VALVE_GAIN_LPM_PER_V[0] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01f;
- VALVE_GAIN_LPM_PER_V[2] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01f;
- VALVE_GAIN_LPM_PER_V[4] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01f;
- VALVE_GAIN_LPM_PER_V[6] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01f;
- VALVE_GAIN_LPM_PER_V[8] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01f;
- VALVE_GAIN_LPM_PER_V[1] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01f;
- VALVE_GAIN_LPM_PER_V[3] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01f;
- VALVE_GAIN_LPM_PER_V[5] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01f;
- VALVE_GAIN_LPM_PER_V[7] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01f;
- VALVE_GAIN_LPM_PER_V[9] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01f;
- for(int i=0; i<25; i++) {
- VALVE_POS_VS_PWM[i] = (float) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i));
- }
- for(int i=0; i<100; i++) {
- JOINT_VEL[i] = ( ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ;
- }
- 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;
- 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();
-
- */
+// K_SPRING = spi_eeprom_read(RID_K_SPRING);
+// D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
}
/*******************************************************************************
* ENCODER functions
+
******************************************************************************/
// A-KHA
#define KF_G1_11 0.083920206005350f
@@ -751,17 +504,6 @@
ENC_pos_cur = spi_enc_read();
ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
- //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);