File content as of revision 192:637092202815:

#include "setting.h"
#include "SPI_EEP_ENC.h"
#include "function_utilities.h"
#include "function_CAN.h"
#include "stm32f4xx_flash.h"
#include "FlashWriter.h"

int Rom_Sector = 6;
//FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector

/*******************************************************************************
 * VARIABLE
 ******************************************************************************/

// 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, 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 REFERENCE_MODE = 0;
int16_t CAN_FREQ = 500;
int16_t DIR_JOINT_ENC = 0;
int16_t DIR_VALVE = 0;
int16_t DIR_VALVE_ENC = 0;

float SUPPLY_VOLTAGE = 12.0f;
float VALVE_VOLTAGE_LIMIT = 12.0f;  //v

float P_GAIN_VALVE_POSITION = 0.0f;
float I_GAIN_VALVE_POSITION= 0.0f;
float D_GAIN_VALVE_POSITION= 0.0f;
float P_GAIN_JOINT_POSITION = 0.0f;
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.0f;
float D_GAIN_JOINT_TORQUE = 0.0f;
float P_GAIN_JOINT_TORQUE_FF = 0.0f;
float I_GAIN_JOINT_TORQUE_FF = 0.0f;
float D_GAIN_JOINT_TORQUE_FF = 0.0f;

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;
//float D_GAIN_VALVE_POSITION_OPP= 0.0f;
//float P_GAIN_JOINT_POSITION_OPP = 0.0f;
//float I_GAIN_JOINT_POSITION_OPP = 0.0f;
//float D_GAIN_JOINT_POSITION_OPP = 0.0f;
//float P_GAIN_JOINT_TORQUE_OPP = 0.0f;
//float I_GAIN_JOINT_TORQUE_OPP = 0.0;
//float D_GAIN_JOINT_TORQUE_OPP = 0.0;

float VALVE_DEADZONE_PLUS;
float VALVE_DEADZONE_MINUS;

int16_t VELOCITY_COMP_GAIN;
int16_t COMPLIANCE_GAIN;

int16_t VALVE_CENTER;

int16_t VALVE_FF;

int16_t BULK_MODULUS;

int16_t CHAMBER_VOLUME_A;
int16_t CHAMBER_VOLUME_B;

int16_t PISTON_AREA_A;
int16_t PISTON_AREA_B;
float PISTON_AREA_alpha;
float alpha3 = 1.0f;


int16_t PRES_SUPPLY;
int16_t PRES_RETURN;

int16_t ENC_LIMIT_PLUS;
int16_t ENC_LIMIT_MINUS;

int16_t STROKE;

float Amm = 236.4f;
float beta = 1300000000.0f;
float Ps = 10000000.0f; //100bar = 100*10^5 Pa
float Pt = 0.0f;    // 0bar = 0Pa
//float Kv = 0.00000002635f;          // Q = Kv*xv*sqrt(Ps-Pa)    => 100bar full opening 5LPM    (full opening : xv = 1)  [unit] m^3.5/kg^0.5
float gamma_hat = 1075.0f; // Kv*beta*A/(sqrt(2)*V)   0.00000002635f * 1300000000.0f *  / (sqrt(2.0f)*(1256.6f + 236.4f * 39.75f) * 0.000000001f / 2)     [unit] m^3.5/kg^0.5
float V_adapt = 0.0000053f;           // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2
float x_4_des_old = 0.0f;

//int16_t VALVE_LIMIT_PLUS;
//int16_t VALVE_LIMIT_MINUS;

float ENC_PULSE_PER_POSITION;
float TORQUE_SENSOR_PULSE_PER_TORQUE;
float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;

int HOMEPOS_OFFSET;
int HOMEPOS_VALVE_OPENING;

float FRICTION;
float REF_PERIOD;
float REF_MAG;
int REF_NUM;


float DAC_REF;
float DAC_RESOL;

int REF_POSITION;
int REF_VELOCITY;
int16_t REF_TORQUE;
int16_t REF_PRES_DIFF;
int16_t REF_PWM;
int16_t REF_VALVE_POSITION;
int16_t REF_CURRENT;

int REF_MOVE_TIME_5k;
int INIT_REF_PWM;
int INIT_REF_VALVE_POS;
int INIT_REF_POS;
int INIT_REF_VEL;
int INIT_REF_TORQUE;
int INIT_REF_PRES_DIFF;
int INIT_REF_CURRENT;

int CUR_POSITION;
int CUR_VELOCITY;
float CUR_TORQUE;
float CUR_PRES_A;
float CUR_PRES_B;
int CUR_VALVE_POSITION;

unsigned int    TMR2_COUNT_LED1;
unsigned int    TMR2_COUNT_LED2;
unsigned int    TMR2_COUNT_CAN_TX = 0;
unsigned int    TMR3_COUNT_TEST = 0;

int num_err;
int flag_err[8];
int flag_err_old[8];
int flag_err_rt;

int flag_ref_enable;

int flag_data_request[5];

int MODE_POS_FT_TRANS = 0;
int NN_Control_Flag = 0;

int cnt_buffer = 0;

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 = 0.0f;
float PRES_B_VREF = 0.0f;
float TORQUE_VREF = 0.0f;

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];
long JOINT_VEL[100];

int VALVE_MAX_POS;
int VALVE_MIN_POS;
int VALVE_POS_NUM;
float VALVE_CENTER_OFFSET;
float VALVE_DZ_MINUS_OFFSET;
float VALVE_DZ_PLUS_OFFSET;

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 = 600.0f;  // sec

float REFERENCE_FREQ = 1.0f;
float REFERENCE_MAG = 0.0f;

bool FLAG_FIND_HOME;

int MODE_JUMP_STATUS;
enum _JUMP_STATUS {
    JUMP_NO_ACT = 0,                                //0
    JUMP_START,                                //1
    JUMP_TAKEOFF,                                  //2
    JUMP_FLYING,                                 //3
    JUMP_LANDING,                                  //4
};

float CUR_PRES_DIFF_BAR = 0.0f;
float CUR_PRES_A_sum = 0.0f;
float CUR_PRES_B_sum = 0.0f;
float CUR_PRES_A_mean = 0.0f;
float CUR_PRES_B_mean = 0.0f;
float CUR_TORQUE_sum = 0.0f;
float CUR_TORQUE_mean = 0.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;

int VALVE_ID_timer = 0;
int VALVE_DZ_timer = 0;
int VALVE_FR_timer = 0;
//int VALVE_HPL_timer = 0;
int VALVE_POS_TMP = 0;
int JOINT_VEL_TMP = 0;
int DDV_POS_AVG = 0;
int VALVE_POS_AVG[50] = {0};
int VALVE_POS_AVG_OLD = 0;
int data_num = 0;
int ID_index = 0;
int DZ_index = 1;
int ID_index_array[50] = {0};
int first_check = 0;
float init_time = 0.0f;
int DZ_case = 0;
int START_POS = 0;
int FINAL_POS = 0;
int DZ_DIRECTION = 0;
int FIRST_DZ = 0;
int SECOND_DZ = 0;
int DZ_NUM = 0;
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;

float valve_pos_err = 0.0f, valve_pos_err_old = 0.0f, valve_pos_err_diff = 0.0f, valve_pos_err_sum = 0.0f;
float joint_pos_err = 0.0f, joint_pos_err_old = 0.0f, joint_pos_err_diff = 0.0f, joint_pos_err_diff_fil = 0.0f, joint_pos_err_sum = 0.0f;
float joint_torq_err = 0.0f, joint_torq_err_old = 0.0f, joint_torq_err_diff = 0.0f, joint_torq_err_sum = 0.0f;
float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f;

float CUR_FLOWRATE = 0.0f;
float VALVE_FF_VOLTAGE = 0.0f;

int pos_plus_end = 0;
int pos_minus_end = 0;

bool need_enc_init = false;

int temp_time = 0;

float CUR_VELOCITY_sum = 0.0f;
float temp_vel_sum = 0.0f;

int DZ_dir = 0;
int DZ_temp_cnt = 0;
int DZ_temp_cnt2 = 0;
int DZ_end = 2;
int flag_flowrate = 0;
int fl_temp_cnt = 0;
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;
int cnt_vel_findhome = 0;
int FINDHOME_VELOCITY = 0;
int FINDHOME_VELOCITY_OLD = 0;
int FINDHOME_POSITION = 0;
int FINDHOME_POSITION_OLD = 0;

// valve gain
int check_vel_pos_init = 0;
int check_vel_pos_fin = 0;
int check_vel_pos_interv = 0;
int valve_gain_repeat_cnt = 0;
float VALVE_VOLTAGE = 0.0f;

float freq_fric_tune = 1.0f;

uint32_t TMR3_COUNT_CAN_TX = 0;

// 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 cnt_freq_test = 0;
int cnt_step_test = 0;
int buffer_data_size = 0;
int cnt_send_buffer = 0;
float freq_test_valve_ref = 1.0f;
float ref_array[10000];
int pos_array[10000];
int flag_every_reference = 0;

int TMR3_COUNT_IREF = 0;
float CUR_CURRENT = 0.0f;
float u_CUR[3] = {0.0f,0.0f,0.0f};
int FINDHOME_STAGE = 0;
int FINDHOME_INIT = 0;
int FINDHOME_GOTOLIMIT = 1;
int FINDHOME_ZEROPOSE = 2;

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 RL_timer = 0;

float K_LPF = 0.0f;
float D_LPF = 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
 ******************************************************************************/


float dabs(float tx)
{
    if (tx >= 0.0f)
        return tx;
    else
        return -tx;
}

float change_int_to_efloat(int input)
{
    int i = 0;

    float output = 0;
    int vn = (int) ((float) input / 10.0f);
    int en = input % 10;

    float temp = 1.;
    for (i = 0; i < en; i++)
        temp *= 0.1f;

    output = (float) vn*temp;
    return output;
}

void make_delay(void)
{
    int i = 0;

    for (i = 0; i < 1000000; i++) {
        ;
    }

}


/*******************************************************************************
 * 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)
{
    BNO = spi_eeprom_read(RID_BNO);
    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);
    SUPPLY_VOLTAGE = (float) (spi_eeprom_read(RID_VOLATGE_SUPPLY)) *0.1f;
    VALVE_VOLTAGE_LIMIT = (float) (spi_eeprom_read(RID_VOLTAGE_VALVE)) * 0.1f;
    P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
    I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
    D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
    P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
    I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
    D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
    P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
    I_GAIN_JOINT_TORQUE = spi_eeprom_read( RID_I_GAIN_JOINT_TORQUE);
    D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
    VALVE_DEADZONE_PLUS = (float) (spi_eeprom_read( RID_VALVE_DEADZONE_PLUS)) * 0.1f;
    VALVE_DEADZONE_MINUS = (float) (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS)) * 0.1f;
    VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
    COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
    VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
    VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
    BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
    CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
    CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
    PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
    PISTON_AREA_B = spi_eeprom_read(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 = spi_eeprom_read(RID_PRES_SUPPLY);
    PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
    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;
//    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
    FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
    HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
    HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
    TORQUE_VREF = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_VREF)) *0.001f;
    PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f;
    PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f;
    VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f;
    VALVE_GAIN_LPM_PER_V[2] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_2)) * 0.01f;
    VALVE_GAIN_LPM_PER_V[4] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_3)) * 0.01f;
    VALVE_GAIN_LPM_PER_V[6] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_4)) * 0.01f;
    VALVE_GAIN_LPM_PER_V[8] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_5)) * 0.01f;
    VALVE_GAIN_LPM_PER_V[1] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_1)) * 0.01f;
    VALVE_GAIN_LPM_PER_V[3] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_2)) * 0.01f;
    VALVE_GAIN_LPM_PER_V[5] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_3)) * 0.01f;
    VALVE_GAIN_LPM_PER_V[7] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_4)) * 0.01f;
    VALVE_GAIN_LPM_PER_V[9] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_5)) * 0.01f;
    for(int i=0; i<25; i++) {
        VALVE_POS_VS_PWM[i] = (float) (spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i));
    }
    for(int i=0; i<100; i++) {
        JOINT_VEL[i] = ( ((spi_eeprom_read( RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((spi_eeprom_read(RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 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();

    */

}

/*******************************************************************************
 * ENCODER functions
 ******************************************************************************/
// A-KHA
#define     KF_G1_11    0.083920206005350f
#define     KF_G1_12    0.000013905329560f
#define     KF_G1_21    -0.000575742328210f
#define     KF_G1_22    0.799999939711725f
// K
#define     KF_G2_11    0.916079793994650f
#define     KF_G2_12    0.000002878711641f
#define     KF_G2_21    0.000575742328210f
#define     KF_G2_22    0.199999945139809f

float KF_Y_11 = 0.0f;
float KF_Y_21 = 0.0f;
float KF_X_11 = 0.0f;
float KF_X_21 = 0.0f;

long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
long enc_offset = 0;

void ENC_UPDATE(void)
{

    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);
    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;
}

void ENC_SET_ZERO(void)
{

    spi_enc_set_clear();
    CUR_POSITION = 0;
    ENC_pos_old = ENC_pos_cur = 0;

}

void ENC_SET(int32_t value_e)
{
    spi_enc_set_clear();
    enc_offset = value_e;
    CUR_POSITION = value_e;
    ENC_pos_old = ENC_pos_cur = value_e;

}