![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: function_utilities/function_utilities.cpp
- Revision:
- 34:bb2ca2fc2a8e
- Parent:
- 33:91b17819ec30
- Child:
- 35:34ce7b0347b8
diff -r 91b17819ec30 -r bb2ca2fc2a8e function_utilities/function_utilities.cpp --- a/function_utilities/function_utilities.cpp Tue Nov 12 11:29:36 2019 +0000 +++ b/function_utilities/function_utilities.cpp Tue Nov 19 13:13:28 2019 +0000 @@ -28,7 +28,7 @@ * VARIABLE ******************************************************************************/ -// Board Information +// Board Information uint8_t BNO = 0; uint8_t CONTROL_MODE = 0; uint8_t OPERATING_MODE = 0; @@ -246,7 +246,7 @@ long TMR2_FOR_SLOW_LOGGING = 0; //int velcount = 0; char max_check = 0; -char min_check = 0; +char min_check = 0; float valve_pos_err = 0.0f, valve_pos_err_old = 0.0f, valve_pos_err_diff = 0.0f, valve_pos_err_sum = 0.0f; float joint_pos_err = 0.0f, joint_pos_err_old = 0.0f, joint_pos_err_diff = 0.0f, joint_pos_err_diff_fil = 0.0f, joint_pos_err_sum = 0.0f; @@ -313,14 +313,16 @@ ******************************************************************************/ -float dabs(float tx) { +float dabs(float tx) +{ if (tx >= 0.0f) return tx; else return -tx; } -float change_int_to_efloat(int input) { +float change_int_to_efloat(int input) +{ int i = 0; float output = 0; @@ -335,7 +337,8 @@ return output; } -void make_delay(void) { +void make_delay(void) +{ int i = 0; for (i = 0; i < 100000; i++) { @@ -348,10 +351,11 @@ /******************************************************************************* * ROM functions ******************************************************************************/ -void ROM_RESET_DATA(void) { +void ROM_RESET_DATA(void) +{ FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector if (!writer.ready()) writer.open(); - + writer.write(RID_BNO,(int) BNO); // write at address, 쓸때도 4byte씩 씀 writer.write(RID_OPERATING_MODE,(int) OPERATING_MODE); writer.write(RID_CAN_FREQ,(int) CAN_FREQ); @@ -391,28 +395,26 @@ writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) TORQUE_SENSOR_PULSE_PER_TORQUE); writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f)); writer.write(RID_PRES_SENSOR_B_PULSE_PER_BAR,(int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f)); - writer.write(RID_FRICTION,(int) (FRICTION * 10.0f)); - writer.write(RID_HOMEPOS_OFFSET,(int) HOMEPOS_OFFSET); - writer.write(RID_HOMEPOS_VALVE_OPENING,(int) HOMEPOS_VALVE_OPENING); - writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0f)); + writer.write(RID_FRICTION,(int) (FRICTION * 10.0f)); + writer.write(RID_HOMEPOS_OFFSET,(int) HOMEPOS_OFFSET); + writer.write(RID_HOMEPOS_VALVE_OPENING,(int) HOMEPOS_VALVE_OPENING); + writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0f)); writer.write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0f)); writer.write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0f)); - writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0f)); - writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0f)); - writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0f)); - writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0f)); - writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0f)); - writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0f)); - writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0f)); - writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0f)); - writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0f)); + writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0f)); + writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0f)); + writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0f)); + writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0f)); + writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0f)); + writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0f)); + writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0f)); + writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0f)); + writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0f)); writer.write(RID_VALVE_GAIN_MINUS_5,(int) (VALVE_GAIN_LPM_PER_V[9] * 100.0f)); - for(int i=0; i<25; i++) - { + for(int i=0; i<25; i++) { writer.write(RID_VALVE_POS_VS_PWM_0 + i, (int) VALVE_POS_VS_PWM[i]); } - for(int i=0; i<100; i++) - { + for(int i=0; i<100; i++) { writer.write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int) (JOINT_VEL[i] & 0xFFFF)); writer.write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int) ((JOINT_VEL[i] >> 16) & 0xFFFF)); } @@ -421,13 +423,14 @@ writer.write(RID_DDV_CENTER, (int) (DDV_CENTER * 10.0f)); writer.write(RID_VALVE_POS_NUM, (int) VALVE_POS_NUM); - writer.close(); - + writer.close(); + } -void ROM_CALL_DATA(void) { +void ROM_CALL_DATA(void) +{ BNO = flashReadInt(Rom_Sector, RID_BNO); - BNO = 5; + BNO = 1; OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE); CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ); DIR_JOINT_ENC = flashReadInt(Rom_Sector, RID_JOINT_ENC_DIR); @@ -487,12 +490,10 @@ VALVE_GAIN_LPM_PER_V[5] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01f; VALVE_GAIN_LPM_PER_V[7] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01f; VALVE_GAIN_LPM_PER_V[9] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01f; - for(int i=0; i<25; i++) - { + for(int i=0; i<25; i++) { VALVE_POS_VS_PWM[i] = (float) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i)); } - for(int i=0; i<100; i++) - { + for(int i=0; i<100; i++) { JOINT_VEL[i] = ( ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ; } VALVE_MAX_POS = flashReadInt(Rom_Sector, RID_VALVE_MAX_POS); @@ -525,7 +526,8 @@ long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0; long enc_offset = 0; -void ENC_UPDATE(void) { +void ENC_UPDATE(void) +{ ENC_pos_cur = spi_enc_read(); ENC_pos_diff = ENC_pos_cur - ENC_pos_old; @@ -552,7 +554,8 @@ ENC_pos_old = ENC_pos_cur; } -void ENC_SET_ZERO(void) { +void ENC_SET_ZERO(void) +{ spi_enc_set_clear(); CUR_POSITION = 0; @@ -560,7 +563,8 @@ } -void ENC_SET(int32_t value_e) { +void ENC_SET(int32_t value_e) +{ spi_enc_set_clear(); enc_offset = value_e; CUR_POSITION = value_e;