Diff: function_utilities/function_utilities.cpp

Revision:

11:82d8768d7351

Child:

12:6f2531038ea4

diff -r 83a6baa77a2e -r 82d8768d7351 function_utilities/function_utilities.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/function_utilities/function_utilities.cpp	Fri Aug 23 12:28:08 2019 +0000
@@ -0,0 +1,456 @@
+#include "setting.h"
+#include "SPI_EEP_ENC.h"
+#include "function_utilities.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 = 5000;
+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;
+double PRES_SENSOR_B_PULSE_PER_BAR;
+
+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;
+
+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 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 TMR2_COUNT_FINDHOME;
+int TMR2_COUNT_FLOWRATE;
+int TMR2_COUNT_DEADZONE;
+int TMR2_COUNT_PRES_NULL;
+int TMR2_COUNT_TORQUE_NULL;
+int TMR2_COUNT_PRES_CALIB;
+int TMR2_COUNT_REFERENCE;
+int TMR2_COUNT_JOINT;
+int TMR2_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;
+
+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
+};
+
+
+
+
+/*******************************************************************************
+ * 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_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.));
+    spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.));
+    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.));
+    spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.));
+    spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.));
+    spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.));
+    spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.));
+    spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.));
+    spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[6] * 100.));
+    spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[8] * 100.));
+    spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.));
+    spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.));
+    spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.));
+    spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[7] * 100.));
+    spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[9] * 100.));
+    
+    
+}
+
+void ROM_CALL_DATA(void) {
+    int i = 0;
+    BNO = spi_eeprom_read(RID_BNO);
+    BNO = 10;
+    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.;
+    VALVE_VOLTAGE_LIMIT = (double) spi_eeprom_read(RID_VOLTAGE_VALVE) / 10.;
+
+    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.;
+    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.;
+
+    PRES_A_VREF = (double) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) / 1000.;
+    PRES_B_VREF = (double) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) / 1000.;
+
+    VALVE_GAIN_LPM_PER_V[0] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_1) / 100.;
+    VALVE_GAIN_LPM_PER_V[2] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_2) / 100.;
+    VALVE_GAIN_LPM_PER_V[4] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_3) / 100.;
+    VALVE_GAIN_LPM_PER_V[6] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_4) / 100.;
+    VALVE_GAIN_LPM_PER_V[8] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_5) / 100.;
+    VALVE_GAIN_LPM_PER_V[1] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_1) / 100.;
+    VALVE_GAIN_LPM_PER_V[3] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_2) / 100.;
+    VALVE_GAIN_LPM_PER_V[5] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_3) / 100.;
+    VALVE_GAIN_LPM_PER_V[7] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_4) / 100.;
+    VALVE_GAIN_LPM_PER_V[9] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_5) / 100.;
+    
+    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] = (long) ( (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 = (long) (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 = (long) KF_X_21;
+
+    CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
+    //    CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur;
+    CUR_VELOCITY = (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(long value) {
+
+    spi_enc_set_clear();
+    enc_offset = value;
+    CUR_POSITION = value;
+    ENC_pos_old = ENC_pos_cur = value;
+
+}