eeprom_test

Dependencies:   mbed FastPWM

function_utilities/function_utilities.cpp

Committer:
Lightvalve
Date:
2019-08-29
Revision:
14:8e7590227d22
Parent:
13:747daba9cf59
Child:
15:bd0d12728506

File content as of revision 14:8e7590227d22:

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

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

// Board Information 
uint8_t BNO = 0;
uint8_t CONTROL_MODE = 0;
uint8_t OPERATING_MODE = 0;
uint8_t SETTING_SWITCH = 0;
uint8_t SETTING_SWITCH_OLD = 0;
uint8_t REFERENCE_MODE = 0;
uint16_t CAN_FREQ = 500;
uint8_t DIR_JOINT_ENC = 0;
uint8_t DIR_VALVE = 0;
uint8_t DIR_VALVE_ENC = 0;

double SUPPLY_VOLTAGE = 12.0;
double VALVE_VOLTAGE_LIMIT = 5.0;

double P_GAIN_VALVE_POSITION = 0.0;
double I_GAIN_VALVE_POSITION= 0.0;
double D_GAIN_VALVE_POSITION= 0.0;
double P_GAIN_JOINT_POSITION = 0.0;
double I_GAIN_JOINT_POSITION = 0.0;
double D_GAIN_JOINT_POSITION = 0.0;
double P_GAIN_JOINT_TORQUE = 0.0;
double I_GAIN_JOINT_TORQUE = 0.0;
double D_GAIN_JOINT_TORQUE = 0.0;

int16_t VALVE_DEADZONE_PLUS;
int16_t 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;
double PISTON_AREA_alpha;


int16_t PRES_SUPPLY;
int16_t PRES_RETURN;

int16_t ENC_LIMIT_PLUS;
int16_t ENC_LIMIT_MINUS;

int16_t STROKE;


int16_t VALVE_LIMIT_PLUS;
int16_t VALVE_LIMIT_MINUS;

int16_t ENC_PULSE_PER_POSITION;
int16_t TORQUE_SENSOR_PULSE_PER_TORQUE;
double PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
double PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;

int HOMEPOS_OFFSET;
int HOMEPOS_VALVE_OPENING;

double FRICTION;
double REF_PERIOD;
double REF_MAG;
int REF_NUM;


double DAC_REF;
double DAC_RESOL;
double PWM_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;
double CUR_TORQUE;
double CUR_PRES_A;
double CUR_PRES_B;
int CUR_PWM;
int CUR_VALVE_POSITION;

unsigned int    TMR2_COUNT_LED1;
unsigned int    TMR2_COUNT_LED2;
unsigned int    TMR2_COUNT_CAN_TX;

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];

////////////////////////////////////////////////////////////////////////////////
////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

double CUR_PRES_A_BAR;
double CUR_PRES_B_BAR;
double CUR_TORQUE_NM;
double CUR_TORQUE_NM_PRESS;

double PRES_A_VREF;
double PRES_B_VREF;
double TORQUE_VREF;

double VALVE_PWM_RAW;
double VALVE_PWM_RAW_FB;
double VALVE_PWM_RAW_FF;
int VALVE_PWM_VALVE_DZ;
int VALVE_INPUT_PWM;

double VALVE_GAIN_LPM_PER_V[10];
double VALVE_POS_VS_PWM[18];
long JOINT_VEL[100];

int VALVE_MAX_POS;
int VALVE_MIN_POS;
int DDV_CENTER;
int VALVE_POS_NUM;

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;

double TUNING_TIME;

double REFERENCE_FREQ;
double REFERENCE_MAG;

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

double CUR_PRES_DIFF_BAR = 0.0;
double CUR_PRES_A_sum = 0.0;
double CUR_PRES_B_sum = 0.0;
double CUR_PRES_A_mean = 0.0;
double CUR_PRES_B_mean = 0.0;
double CUR_TORQUE_sum = 0.0;
double CUR_TORQUE_mean = 0.0;
double PRES_A_NULL = 200.0;
double PRES_B_NULL = 200.0;
double TORQUE_NULL = 3900;

double Ref_Valve_Pos_Old = 0.0;

int VALVE_ID_timer = 0;
int VALVE_DZ_timer = 0;
int VALVE_FR_timer = 0;
int VALVE_POS_TMP = 0;
int JOINT_VEL_TMP = 0;
int DDV_POS_AVG = 0;
int VALVE_POS_AVG[100] = {0};
int VALVE_POS_AVG_OLD = 0;
int data_num = 0;
int ID_index = 0;
int DZ_index = 1;
int ID_index_array[100] = {0};
int FR_index_array[100] = {0};
int first_check = 0;
double init_time = 0.0;
int VEL_POINT = 0;
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;
double Ref_Vel_Test = 0.0;
long TMR2_FOR_SLOW_LOGGING = 0;
int velcount = 0;
char max_check = 0;
char min_check = 0; 

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

double CUR_FLOWRATE = 0.0;
double VALVE_FF_VOLTAGE = 0.0;

int pos_plus_end = 0;
int pos_minus_end = 0;

bool need_enc_init = false;

int temp_time = 0;

double CUR_VELOCITY_sum = 0.0;
double temp_vel_sum = 0.0;

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;

// 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;
double VALVE_VOLTAGE = 0.0;

double freq_fric_tune = 1.0;

bool FLAG_VALVE_OUTPUT_CALIB = false;

uint32_t TMR3_COUNT_CAN_TX = 0;

double I_REF = 0.0;
int TMR3_COUNT_IREF = 0;
double CUR_CURRENT = 0.0;
double u_CUR[3] = {0.0,0.0,0.0};

/*******************************************************************************
 * General math functions
 ******************************************************************************/


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

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

    double output = 0;
    int vn = (int) ((double) input / 10.);
    int en = input % 10;

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

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

void make_delay(void) {
    int i = 0;

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

}


/*******************************************************************************
 * ROM functions
 ******************************************************************************/
void ROM_INIT_DATA(void)
{
    static int16_t temp_check_num = LATEST_VERSION;
    spi_eeprom_ready();
    //if (spi_eeprom_read(RID_IS_FIRST) != temp_check_num) {
        spi_eeprom_ready();
        spi_eeprom_write(RID_IS_FIRST, temp_check_num);
        ROM_RESET_DATA();
    //}
    ROM_CALL_DATA();
}

void ROM_RESET_DATA(void) {

    spi_eeprom_write(RID_BNO, (int16_t) BNO);
    spi_eeprom_write(RID_OPERATING_MODE, (int16_t) OPERATING_MODE);
    spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
    spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC);
    spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE);
    spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC);
    spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.));
    spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.));
    spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
    spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
    spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
    spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
    spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
    spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
    spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
    spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
    spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
    spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
    spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
    spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN);
    spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN);
    spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
    spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF);
    spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS);
    spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A);
    spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B);
    spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A);
    spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B);
    spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
    spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
    spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS);
    spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS);
    spi_eeprom_write(RID_STROKE, (int16_t) STROKE);
    spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int16_t) VALVE_LIMIT_MINUS);
    spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int16_t) VALVE_LIMIT_PLUS);
    spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) ENC_PULSE_PER_POSITION);
    spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) TORQUE_SENSOR_PULSE_PER_TORQUE);
    spi_eeprom_ready();
    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
    PRES_SENSOR_A_PULSE_PER_BAR = 1.0;
    //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
    spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int) (6));
    spi_eeprom_ready();
    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;
    spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
    spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.));
    spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
    spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING);
    spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
    spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
    spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
    spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.0));
    spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.0));
    spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.0));
    spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[6] * 100.0));
    spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[8] * 100.0));
    spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.0));
    spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.0));
    spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.0));
    spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[7] * 100.0));
    spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[9] * 100.0));
    
    
}

void ROM_CALL_DATA(void) {
    int i = 0;
    BNO = spi_eeprom_read(RID_BNO);
    BNO = 11;
    OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
    CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);

    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 = (double) spi_eeprom_read(RID_VOLATGE_SUPPLY) / 10.0;
    VALVE_VOLTAGE_LIMIT = (double) spi_eeprom_read(RID_VOLTAGE_VALVE) / 10.0;

    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 = spi_eeprom_read(RID_VALVE_DEADZONE_PLUS);
    VALVE_DEADZONE_MINUS = spi_eeprom_read(RID_VALVE_DEADZONE_MINUS);

    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 = (double)PISTON_AREA_B/(double)PISTON_AREA_A;

    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 = spi_eeprom_read(RID_VALVE_LIMIT_MINUS);
    VALVE_LIMIT_PLUS = spi_eeprom_read(RID_VALVE_LIMIT_PLUS);

    ENC_PULSE_PER_POSITION = spi_eeprom_read(RID_ENC_PULSE_PER_POSITION);
    TORQUE_SENSOR_PULSE_PER_TORQUE = spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE);
    PRES_SENSOR_A_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR) * 0.01;
    PRES_SENSOR_B_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR) * 0.01;

    FRICTION = (double) spi_eeprom_read(RID_FRICTION) / 10.0;
    HOMEPOS_OFFSET = (int) spi_eeprom_read(RID_HOMEPOS_OFFSET);
    HOMEPOS_VALVE_OPENING = (int) spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);

    TORQUE_VREF = (double) spi_eeprom_read(RID_TORQUE_SENSOR_VREF) / 1000.0;

    PRES_A_VREF = (double) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) / 1000.0;
    PRES_B_VREF = (double) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) / 1000.0;

    VALVE_GAIN_LPM_PER_V[0] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_1) / 100.0;
    VALVE_GAIN_LPM_PER_V[2] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_2) / 100.0;
    VALVE_GAIN_LPM_PER_V[4] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_3) / 100.0;
    VALVE_GAIN_LPM_PER_V[6] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_4) / 100.0;
    VALVE_GAIN_LPM_PER_V[8] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_5) / 100.0;
    VALVE_GAIN_LPM_PER_V[1] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_1) / 100.0;
    VALVE_GAIN_LPM_PER_V[3] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_2) / 100.0;
    VALVE_GAIN_LPM_PER_V[5] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_3) / 100.0;
    VALVE_GAIN_LPM_PER_V[7] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_4) / 100.0;
    VALVE_GAIN_LPM_PER_V[9] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_5) / 100.0;
    
    for(i=0; i<18; i++)
    {
        VALVE_POS_VS_PWM[i] = (double) spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i);
    }
    for(i=0; i<100; i++)
    {
        JOINT_VEL[i] = (int32_t) ( (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 = (int) spi_eeprom_read(RID_VALVE_MAX_POS);
    VALVE_MIN_POS = (int) spi_eeprom_read(RID_VALVE_MIN_POS);
    DDV_CENTER = (int) spi_eeprom_read(RID_DDV_CNETER);
    
    VALVE_POS_NUM = (int) spi_eeprom_read(RID_VALVE_POS_NUM);
    
}

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

double KF_Y_11 = 0.;
double KF_Y_21 = 0.;
double KF_X_11 = 0.;
double KF_X_21 = 0.;

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;

    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;

    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;

}