Diff: function_utilities/function_utilities.cpp

Revision:

34:bb2ca2fc2a8e

Parent:

33:91b17819ec30

Child:

35:34ce7b0347b8

--- 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;