![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
for learning
Diff: function_utilities/function_utilities.cpp
- Revision:
- 14:8e7590227d22
- Parent:
- 13:747daba9cf59
- Child:
- 15:bd0d12728506
--- a/function_utilities/function_utilities.cpp Tue Aug 27 06:50:54 2019 +0000 +++ b/function_utilities/function_utilities.cpp Thu Aug 29 07:38:00 2019 +0000 @@ -1,6 +1,7 @@ #include "setting.h" #include "SPI_EEP_ENC.h" #include "function_utilities.h" +#include "function_CAN.h" /******************************************************************************* * VARIABLE @@ -13,7 +14,7 @@ uint8_t SETTING_SWITCH = 0; uint8_t SETTING_SWITCH_OLD = 0; uint8_t REFERENCE_MODE = 0; -uint16_t CAN_FREQ = 5000; +uint16_t CAN_FREQ = 500; uint8_t DIR_JOINT_ENC = 0; uint8_t DIR_VALVE = 0; uint8_t DIR_VALVE_ENC = 0; @@ -65,8 +66,8 @@ 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; +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; @@ -87,6 +88,7 @@ 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; @@ -95,6 +97,7 @@ int INIT_REF_VEL; int INIT_REF_TORQUE; int INIT_REF_PRES_DIFF; +int INIT_REF_CURRENT; int CUR_POSITION; int CUR_VELOCITY; @@ -145,21 +148,22 @@ 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; +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; @@ -188,11 +192,6 @@ 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; @@ -226,7 +225,7 @@ 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_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; @@ -272,6 +271,13 @@ 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 ******************************************************************************/ @@ -312,14 +318,15 @@ /******************************************************************************* * ROM functions ******************************************************************************/ -void ROM_INIT_DATA(void) { +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) { + //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(); } @@ -362,24 +369,30 @@ 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_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.)); - 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.)); + 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)); } @@ -387,7 +400,7 @@ void ROM_CALL_DATA(void) { int i = 0; BNO = spi_eeprom_read(RID_BNO); - BNO = 10; + BNO = 11; OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE); CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ); @@ -395,8 +408,8 @@ 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.; + 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); @@ -443,25 +456,25 @@ 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.; + 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.; + 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.; - PRES_B_VREF = (double) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) / 1000.; + 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.; - 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.; + 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++) { @@ -520,9 +533,9 @@ 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; + pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset; // CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur; - CUR_VELOCITY = (DIR_JOINT_ENC) * ENC_VEL_KF; + vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF; // CUR_POSITION = ENC_pos_cur; // CUR_VELOCITY = ENC_VEL_KF;