asdgas
Dependencies: mbed Eigen FastPWM
function_utilities/function_utilities.cpp
- Committer:
- Lightvalve
- Date:
- 2019-08-27
- Revision:
- 13:747daba9cf59
- Parent:
- 12:6f2531038ea4
- Child:
- 14:8e7590227d22
File content as of revision 13:747daba9cf59:
#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 }; 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_PWM = 0.0; double Ref_Valve_Pos = 0.0; double Ref_Joint_Pos = 0.0; double Ref_Joint_Vel = 0.0; double Ref_Joint_Torq = 0.0; 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_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; /******************************************************************************* * 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] = (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; 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(int32_t value_e) { spi_enc_set_clear(); enc_offset = value_e; CUR_POSITION = value_e; ENC_pos_old = ENC_pos_cur = value_e; }