Revision 49:e7bcfc244d40, committed 2020-02-05

Comitter:

Lightvalve

Date:

Wed Feb 05 08:35:58 2020 +0000

Parent:

48:889798ff9329

Child:

50:3c630b5eba9f

Commit message:

20200205

Changed in this revision

--- a/CAN/function_CAN.cpp	Wed Jan 29 13:50:24 2020 +0000
+++ b/CAN/function_CAN.cpp	Wed Feb 05 08:35:58 2020 +0000
@@ -8,7 +8,8 @@
 // CAN ID Setting Variables
 int CID_RX_CMD = 100;
 int CID_RX_REF_POSITION = 200;
-int CID_RX_REF_PWM = 300;
+int CID_RX_REF_VALVE_POS = 300;
+int CID_RX_REF_PWM = 400;
 
 int CID_TX_INFO = 1100;
 int CID_TX_POSITION = 1200;
@@ -29,6 +30,7 @@
 
     CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD);
     CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION);
+    CID_RX_REF_VALVE_POS = (int) (BNO + INIT_CID_RX_REF_VALVE_POS);
     CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM);
 
     CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO);
@@ -322,6 +324,20 @@
 
             break;
         }
+        
+        case CRX_ASK_FLOWFORCE_GAIN: {
+            CAN_TX_FLOWFORCE_GAIN();
+
+            break;
+        }
+
+        case CRX_SET_FLOWFORCE_GAIN: {
+            FLOWFORCE_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+            ROM_RESET_DATA();
+
+            break;
+        }
 
         case CRX_ASK_VALVE_FF: {
             CAN_TX_VALVE_FF();
@@ -716,13 +732,16 @@
         pos.ref = (double)temp_pos * 4.0f;
         vel.ref = (double)temp_vel * 100.0f;
         torq.ref = (double)temp_torq * 0.1f;
-    } else if(address==CID_RX_REF_PWM) {
+    } else if(address==CID_RX_REF_VALVE_POS) {
         int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
         if(temp_ref_valve_pos >= 0){
             valve_pos.ref = (double)DDV_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)DDV_CENTER)/10000.0f  ;
         }else{
             valve_pos.ref = (double)DDV_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)DDV_CENTER)/10000.0f;
         }
+    } else if(address==CID_RX_REF_PWM){
+        int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        Vout.ref = (double) temp_ref_pwm;
     }   
 }
 
@@ -828,7 +847,7 @@
 }
 
 void CAN_TX_VOLTAGE_SUPPLY(void) {
-    int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10);
+    int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10.0f);
 
     CANMessage temp_msg;
 
@@ -842,7 +861,7 @@
 }
 
 void CAN_TX_VOLTAGE_VALVE(void) {
-    int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10);
+    int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f);
 
     CANMessage temp_msg;
 
@@ -936,6 +955,18 @@
     can.write(temp_msg);
 }
 
+void CAN_TX_FLOWFORCE_GAIN(void) {
+    CANMessage temp_msg;
+    
+    temp_msg.id = CID_TX_INFO;
+    temp_msg.len = 3;
+    temp_msg.data[0] = (uint8_t) CTX_SEND_FLOWFORCE_GAIN;
+    temp_msg.data[1] = (uint8_t) FLOWFORCE_GAIN;
+    temp_msg.data[2] = (uint8_t) (FLOWFORCE_GAIN >> 8);
+
+    can.write(temp_msg);
+}
+
 void CAN_TX_VALVE_FF(void) {
     CANMessage temp_msg;
     
@@ -1195,7 +1226,13 @@
 void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex) {
     CANMessage temp_msg;
     int16_t valve_pos_vs_pwm;
-    valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
+//    valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
+    
+    if(VALVE_POS_VS_PWM[canindex]>=DDV_CENTER) {
+        valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+    } else {
+        valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+    }
     
     int16_t PWM_VALVE_ID;
     PWM_VALVE_ID = ID_index_array[canindex] * 1000;
@@ -1217,16 +1254,22 @@
     valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]);
 
     int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + (int) DDV_CENTER;
+    int16_t temp_valve_pos = 0;
+    if(VALVE_POS_VALVE_ID>=DDV_CENTER) {
+        temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER));
+    } else {
+        temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER));
+    }
     
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 7;
+    temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE;
-    temp_msg.data[1] = (uint8_t) VALVE_POS_VALVE_ID;
-    temp_msg.data[2] = (uint8_t) (VALVE_POS_VALVE_ID >> 8);
-    temp_msg.data[3] = (uint8_t) valve_pos_vs_flowrate;
-    temp_msg.data[4] = (uint8_t) (valve_pos_vs_flowrate >> 8);
-    temp_msg.data[5] = (uint8_t) (valve_pos_vs_flowrate >> 16);
-    temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 24);
+    temp_msg.data[1] = (uint8_t) temp_valve_pos;
+    temp_msg.data[2] = (uint8_t) (temp_valve_pos >> 8);
+    temp_msg.data[5] = (uint8_t) valve_pos_vs_flowrate;
+    temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 8);
+    temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 16);
+    temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 24);
 
     can.write(temp_msg);
 }
@@ -1344,3 +1387,4 @@
 
 
 
+

--- a/CAN/function_CAN.h	Wed Jan 29 13:50:24 2020 +0000
+++ b/CAN/function_CAN.h	Wed Feb 05 08:35:58 2020 +0000
@@ -9,7 +9,8 @@
 // INIT_CID
 #define INIT_CID_RX_CMD                   100
 #define INIT_CID_RX_REF_POSITION          200
-#define INIT_CID_RX_REF_PWM            300
+#define INIT_CID_RX_REF_VALVE_POS       300
+#define INIT_CID_RX_REF_PWM            400
 
 #define INIT_CID_TX_INFO              1100
 #define INIT_CID_TX_POSITION          1200
@@ -52,6 +53,8 @@
 #define             CRX_SET_VELOCITY_COMP_GAIN      122
 #define             CRX_ASK_COMPLIANCE_GAIN         23
 #define             CRX_SET_COMPLIANCE_GAIN         123
+#define             CRX_ASK_FLOWFORCE_GAIN          24
+#define             CRX_SET_FLOWFORCE_GAIN          124
 #define             CRX_ASK_VALVE_FF                25
 #define             CRX_SET_VALVE_FF                125
 #define             CRX_ASK_BULK_MODULUS            26
@@ -115,7 +118,7 @@
 #define             CTX_SEND_VALVE_DEADZONE                     21
 #define             CTX_SEND_VELOCITY_COMP_GAIN                 22
 #define             CTX_SEND_COMPLIANCE_GAIN                    23
-#define             CTX_SEND_VALVE_CNETER                       24
+#define             CTX_SEND_FLOWFORCE_GAIN                     24
 #define             CTX_SEND_VALVE_FF                           25
 #define             CTX_SEND_BULK_MODULUS                       26
 #define             CTX_SEND_CHAMBER_VOLUME                     27
@@ -138,6 +141,7 @@
 #define             CTX_VALVE_POS_VS_FLOWRATE                   44
 #define             CTX_VALVE_POS_NUM                           45
 #define             CTX_VALVE_MAX_MIN_POS                       46
+
 // Sensor & State Transmission
 void CAN_TX_POSITION(int16_t t_pos, int16_t t_vel, int16_t t_torq);
 void CAN_TX_TORQUE(int16_t t_torque);
@@ -160,6 +164,7 @@
 void CAN_TX_VALVE_DEADZONE(void);
 void CAN_TX_VELOCITY_COMP_GAIN(void);
 void CAN_TX_COMPLIANCE_GAIN(void);
+void CAN_TX_FLOWFORCE_GAIN(void);
 void CAN_TX_VALVE_FF(void);
 void CAN_TX_BULK_MODULUS(void);
 void CAN_TX_CHAMBER_VOLUME(void);

--- a/SPI_EEP_ENC/SPI_EEP_ENC.h	Wed Jan 29 13:50:24 2020 +0000
+++ b/SPI_EEP_ENC/SPI_EEP_ENC.h	Wed Feb 05 08:35:58 2020 +0000
@@ -177,5 +177,6 @@
 
 #define             RID_K_SPRING                        57
 #define             RID_D_DAMPER                        58
+#define             RID_FLOWFORCE_GAIN                 59
 
 #endif //_SPI_H_
\ No newline at end of file

--- a/function_utilities/function_utilities.cpp	Wed Jan 29 13:50:24 2020 +0000
+++ b/function_utilities/function_utilities.cpp	Wed Feb 05 08:35:58 2020 +0000
@@ -24,8 +24,8 @@
 int16_t DIR_VALVE = 0;
 int16_t DIR_VALVE_ENC = 0;
 
-float SUPPLY_VOLTAGE = 12000.0f;
-float VALVE_VOLTAGE_LIMIT = 12000.0f;  //mv
+float SUPPLY_VOLTAGE = 12.0f;
+float VALVE_VOLTAGE_LIMIT = 12.0f;  //v
 
 float P_GAIN_VALVE_POSITION = 0.0f;
 float I_GAIN_VALVE_POSITION= 0.0f;
@@ -55,6 +55,7 @@
 
 int16_t VELOCITY_COMP_GAIN;
 int16_t COMPLIANCE_GAIN;
+int16_t FLOWFORCE_GAIN;
 
 int16_t VALVE_CENTER;
 
@@ -379,6 +380,7 @@
     writer.write(RID_VALVE_DEADZONE_MINUS,(int) (VALVE_DEADZONE_MINUS * 10.0f));
     writer.write(RID_VELOCITY_COMP_GAIN,(int) VELOCITY_COMP_GAIN);
     writer.write(RID_COMPLIANCE_GAIN,(int) COMPLIANCE_GAIN);
+    writer.write(RID_FLOWFORCE_GAIN, (int) FLOWFORCE_GAIN);
     writer.write(RID_VALVE_CNETER,(int) VALVE_CENTER);
     writer.write(RID_VALVE_FF,(int) VALVE_FF);
     writer.write(RID_BULK_MODULUS,(int) BNO);
@@ -441,9 +443,7 @@
     DIR_VALVE = flashReadInt(Rom_Sector, RID_VALVE_DIR);
     DIR_VALVE_ENC = flashReadInt(Rom_Sector, RID_VALVE_ENC_DIR);
     SUPPLY_VOLTAGE = (float) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1f;
-//    SUPPLY_VOLTAGE = 12000.0f;
     VALVE_VOLTAGE_LIMIT = (float) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1f;
-//    VALVE_VOLTAGE_LIMIT = 12000.0f;
     P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION);
     I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION);
     D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION);
@@ -457,6 +457,7 @@
     VALVE_DEADZONE_MINUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_MINUS)) * 0.1f;
     VELOCITY_COMP_GAIN = flashReadInt(Rom_Sector, RID_VELOCITY_COMP_GAIN);
     COMPLIANCE_GAIN = flashReadInt(Rom_Sector, RID_COMPLIANCE_GAIN);
+    FLOWFORCE_GAIN = flashReadInt(Rom_Sector, RID_FLOWFORCE_GAIN);
     VALVE_CENTER = flashReadInt(Rom_Sector, RID_VALVE_CNETER);
     VALVE_FF = flashReadInt(Rom_Sector, RID_VALVE_FF);
     BULK_MODULUS = flashReadInt(Rom_Sector, RID_BULK_MODULUS);

--- a/main.cpp	Wed Jan 29 13:50:24 2020 +0000
+++ b/main.cpp	Wed Feb 05 08:35:58 2020 +0000
@@ -75,6 +75,7 @@
 
 extern int CID_RX_CMD;
 extern int CID_RX_REF_POSITION;
+extern int CID_RX_REF_VALVE_POS;
 extern int CID_RX_REF_PWM;
 
 extern int CID_TX_INFO;
@@ -265,7 +266,6 @@
     }
 
     Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
-//    Ref_Valve_Pos_FF = 0.0;
     return Ref_Valve_Pos_FF;
     
 }
@@ -330,8 +330,10 @@
             break;
         }
     }
-
-    V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
+    if(vel.sen>=0.0f)
+        V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
+    else
+        V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; // - (float) FLOWFORCE_GAIN * 0.01f * vel.sen / 30.0f;
     //V_out = VALVE_PWM_RAW_FB;
 }
 
@@ -928,10 +930,10 @@
                 }
                 if (need_enc_init) {
                     if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_plus_end = pos.sen;
                     } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_minus_end = pos.sen;
                     } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
                     temp_time = TMR_FREQ_5k;
@@ -1104,16 +1106,16 @@
                 if (TMR3_COUNT_FLOWRATE == 0) {
                     if (pos_plus_end == pos_minus_end) need_enc_init = true;
                     else {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         temp_time = (int) (0.5f * (float) TMR_FREQ_5k);
                     }
                 }
                 if (need_enc_init) {
                     if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_plus_end = pos.sen;
                     } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_minus_end = pos.sen;
                     } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
                         need_enc_init = false;
@@ -1216,12 +1218,12 @@
 
             case MODE_PRESSURE_SENSOR_CALIB: {
                 if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
-                    V_out = -VALVE_VOLTAGE_LIMIT;
+                    V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                     if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
                         CUR_PRES_A_sum += CUR_PRES_A;
                     }
                 } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
-                    V_out = VALVE_VOLTAGE_LIMIT;
+                    V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                     if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
                         CUR_PRES_B_sum += CUR_PRES_B;
                     }
@@ -1250,8 +1252,8 @@
             case MODE_ROTARY_FRICTION_TUNING: {
                 if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f);
                 V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f);
-                if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT;
-                else V_out = -VALVE_VOLTAGE_LIMIT;
+                if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+                else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; 
                 TMR3_COUNT_ROTARY_FRIC_TUNE++;
                 if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
                     TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
@@ -1307,23 +1309,23 @@
                 VALVE_DZ_timer = VALVE_DZ_timer + 1;
                 if(first_check == 0) {
                     if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                     } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_plus_end = pos.sen;
                     } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                     } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_minus_end = pos.sen;
                     } else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) {
-                        V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                        V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                     } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
-                        V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                        V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         data_num = data_num + 1;
                         VALVE_POS_TMP = VALVE_POS_TMP + value;
                     } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
-                        V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                        V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         DDV_POS_AVG = VALVE_POS_TMP / data_num;
                         START_POS = pos.sen;
                         VALVE_POS_TMP = 0;
@@ -1353,6 +1355,8 @@
                             DZ_case = 0;
                         }
 
+                        CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
+                        
                         VEL_POINT = 0;
                         first_check = 1;
                         DZ_DIRECTION = 1;
@@ -1365,7 +1369,7 @@
                 } else {
                     if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) {
                         if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) {
-                            V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                            V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
@@ -1406,7 +1410,7 @@
                         }
                     } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) {
                         if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                            V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
@@ -1453,7 +1457,7 @@
                         }
                     } else if(DZ_case == 0 && DZ_NUM ==1) {
                         if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                            V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
@@ -1493,7 +1497,7 @@
                         }
                     } else {
                         if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                            V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
@@ -1543,17 +1547,17 @@
                 VALVE_FR_timer = VALVE_FR_timer + 1;
                 if(first_check == 0) {
                     if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         //CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
                     } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_plus_end = pos.sen;
                         //                    CAN_TX_PRES((int16_t) (V_out), (int16_t) (7));
                     } else if(VALVE_FR_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                     } else if(VALVE_FR_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
                         //                    CAN_TX_PRES((int16_t) (V_out), (int16_t) (8));
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_minus_end = pos.sen;
                         first_check = 1;
                         VALVE_FR_timer = 0;
@@ -1564,7 +1568,7 @@
                     }
                 } else {
                     if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                        V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                     } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                         data_num = 0;
                         valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
@@ -1607,6 +1611,7 @@
                         first_check = 0;
                         VALVE_FR_timer = 0;
                         CONTROL_MODE = MODE_NO_ACT;
+//                        CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
                     }
                 }
                 break;
@@ -1645,12 +1650,12 @@
         /*******************************************************
         ***     PWM
         ********************************************************/
-        if (V_out >= 12000.0f) {
-            V_out = 12000.0f;
-        } else if(V_out<=-12000.0f) {
-            V_out = -12000.0f;
+        if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) {
+            V_out = VALVE_VOLTAGE_LIMIT*1000.0f;
+        } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) {
+            V_out = -VALVE_VOLTAGE_LIMIT*1000.0f;
         }
-        PWM_out= V_out/SUPPLY_VOLTAGE; // Full duty : 12000.0mV
+        PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); // Full duty : 12000.0mV
 
         // Saturation of output voltage to 5.0V
 //        if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
@@ -1690,7 +1695,7 @@
         if (flag_data_request[0] == HIGH) {
             //position+velocity
             //CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
-            CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
+            CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (V_out/100.0f), (int16_t) (torq.sen*10.0f));
         }
 
         if (flag_data_request[1] == HIGH) {
@@ -1708,7 +1713,7 @@
 
         if (flag_data_request[2] == HIGH) {
             //pressure A and B
-            CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF * 1000.0)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+            CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (PRES_A_VREF * 1000.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
             //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
             //                        CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
             //            CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);

--- a/setting.h	Wed Jan 29 13:50:24 2020 +0000
+++ b/setting.h	Wed Feb 05 08:35:58 2020 +0000
@@ -148,6 +148,7 @@
 
 extern int16_t VELOCITY_COMP_GAIN;
 extern int16_t COMPLIANCE_GAIN;
+extern int16_t FLOWFORCE_GAIN;
 
 extern int16_t VALVE_CENTER;
 
@@ -411,3 +412,4 @@
 
 
 
+