![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
for learning
Diff: function_utilities/function_utilities.cpp
- Revision:
- 11:82d8768d7351
- Child:
- 12:6f2531038ea4
--- /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; + +}