![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
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);