rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
Lightvalve
Date:
Tue Jul 13 01:40:56 2021 +0000
Parent:
237:c2cfe349f5c1
Child:
239:8ac5c6162bc1
Commit message:
210713

Changed in this revision

CAN/function_CAN.cpp Show annotated file Show diff for this revision Revisions of this file
CAN/function_CAN.h Show annotated file Show diff for this revision Revisions of this file
SPI_EEP_ENC/SPI_EEP_ENC.h Show annotated file Show diff for this revision Revisions of this file
function_utilities/function_utilities.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
setting.h Show annotated file Show diff for this revision Revisions of this file
--- a/CAN/function_CAN.cpp	Wed Jul 07 10:09:21 2021 +0000
+++ b/CAN/function_CAN.cpp	Tue Jul 13 01:40:56 2021 +0000
@@ -355,17 +355,17 @@
             break;
         }
 
-        case CRX_ASK_COMPLIANCE_GAIN: {
-            CAN_TX_COMPLIANCE_GAIN();
+        case CRX_ASK_VALVE_ELECTRIC_CENTER: {
+            CAN_TX_VALVE_ELECTRIC_CENTER();
 
             break;
         }
 
-        case CRX_SET_COMPLIANCE_GAIN: {
-            COMPLIANCE_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
+        case CRX_SET_VALVE_ELECTRIC_CENTER: {
+            VALVE_ELECTRIC_CENTER = (int16_t) (msg.data[1] | msg.data[2] << 8);
 
 
-            spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN);
+            spi_eeprom_write(RID_VALVE_ELECTRIC_CENTER, (int16_t) VALVE_ELECTRIC_CENTER);
 
             break;
         }
@@ -736,9 +736,11 @@
             valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~30000)
         } else { //SW Valve
             if(temp_ref_valve_pos >= 0) {
-                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+//                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                valve_pos.ref = (double)temp_ref_valve_pos; // -10000~10000
             } else {
-                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+//                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                valve_pos.ref = (double)temp_ref_valve_pos; // -10000~10000
             }
         }
     } else if(address==CID_RX_REF_PWM) {
@@ -974,15 +976,15 @@
     can.write(temp_msg);
 }
 
-void CAN_TX_COMPLIANCE_GAIN(void)
+void CAN_TX_VALVE_ELECTRIC_CENTER(void)
 {
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 3;
-    temp_msg.data[0] = (uint8_t) CTX_SEND_COMPLIANCE_GAIN;
-    temp_msg.data[1] = (uint8_t) COMPLIANCE_GAIN;
-    temp_msg.data[2] = (uint8_t) (COMPLIANCE_GAIN >> 8);
+    temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_ELECTRIC_CENTER;
+    temp_msg.data[1] = (uint8_t) VALVE_ELECTRIC_CENTER;
+    temp_msg.data[2] = (uint8_t) (VALVE_ELECTRIC_CENTER >> 8);
 
     can.write(temp_msg);
 }
--- a/CAN/function_CAN.h	Wed Jul 07 10:09:21 2021 +0000
+++ b/CAN/function_CAN.h	Tue Jul 13 01:40:56 2021 +0000
@@ -55,8 +55,8 @@
 #define             CRX_SET_VALVE_DEADZONE          121
 #define             CRX_ASK_VELOCITY_COMP_GAIN      22
 #define             CRX_SET_VELOCITY_COMP_GAIN      122
-#define             CRX_ASK_COMPLIANCE_GAIN         23
-#define             CRX_SET_COMPLIANCE_GAIN         123
+#define             CRX_ASK_VALVE_ELECTRIC_CENTER   23 
+#define             CRX_SET_VALVE_ELECTRIC_CENTER   123
 #define             CRX_ASK_VALVE_FF                25
 #define             CRX_SET_VALVE_FF                125
 #define             CRX_ASK_BULK_MODULUS            26
@@ -126,7 +126,7 @@
 #define             CTX_SEND_PID_GAIN                           20
 #define             CTX_SEND_VALVE_DEADZONE                     21
 #define             CTX_SEND_VELOCITY_COMP_GAIN                 22
-#define             CTX_SEND_COMPLIANCE_GAIN                    23
+#define             CTX_SEND_VALVE_ELECTRIC_CENTER              23
 #define             CTX_SEND_VALVE_FF                           25
 #define             CTX_SEND_BULK_MODULUS                       26
 #define             CTX_SEND_CHAMBER_VOLUME                     27
@@ -179,7 +179,7 @@
 void CAN_TX_PID_GAIN(int t_type);
 void CAN_TX_VALVE_DEADZONE(void);
 void CAN_TX_VELOCITY_COMP_GAIN(void);
-void CAN_TX_COMPLIANCE_GAIN(void);
+void CAN_TX_VALVE_ELECTRIC_CENTER(void);
 void CAN_TX_VALVE_FF(void);
 void CAN_TX_BULK_MODULUS(void);
 void CAN_TX_CHAMBER_VOLUME(void);
@@ -240,6 +240,7 @@
 extern State pres_B;
 extern State cur;
 extern State valve_pos;
+extern State valve_pos_raw;
 
 extern State INIT_Vout;
 extern State INIT_Valve_Pos;
--- a/SPI_EEP_ENC/SPI_EEP_ENC.h	Wed Jul 07 10:09:21 2021 +0000
+++ b/SPI_EEP_ENC/SPI_EEP_ENC.h	Tue Jul 13 01:40:56 2021 +0000
@@ -46,7 +46,7 @@
 #define             RID_VALVE_DEADZONE_MINUS            18
 
 #define             RID_VELOCITY_COMP_GAIN              19
-#define             RID_COMPLIANCE_GAIN                 20
+//#define             RID_COMPLIANCE_GAIN                 20
 
 #define             RID_VALVE_CNETER                    21
 
--- a/function_utilities/function_utilities.cpp	Wed Jul 07 10:09:21 2021 +0000
+++ b/function_utilities/function_utilities.cpp	Tue Jul 13 01:40:56 2021 +0000
@@ -60,7 +60,7 @@
 int16_t VALVE_DEADZONE_MINUS;
 
 int16_t VELOCITY_COMP_GAIN;
-int16_t COMPLIANCE_GAIN;
+//int16_t COMPLIANCE_GAIN;
 
 int16_t VALVE_CENTER;
 
@@ -421,7 +421,7 @@
     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);
+//    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);
--- a/main.cpp	Wed Jul 07 10:09:21 2021 +0000
+++ b/main.cpp	Tue Jul 13 01:40:56 2021 +0000
@@ -73,6 +73,7 @@
 State pres_B;
 State cur;          // unit : mA
 State valve_pos;
+State valve_pos_raw;
 
 State INIT_Vout;
 State INIT_Valve_Pos;
@@ -240,8 +241,8 @@
     make_delay();
 
     ////// bno rom
-//    spi_eeprom_write(RID_BNO, (int16_t) 1);
-//    make_delay();
+    spi_eeprom_write(RID_BNO, (int16_t) 1);
+    make_delay();
     ////////
 
     // rom
@@ -280,13 +281,13 @@
         dac_1 = FORCE_VREF / 3.3f;
         dac_2 = 0.0f;
     } else if (SENSING_MODE == 1) {
-        if (DIR_VALVE_ENC > 0) {
+//        if (DIR_VALVE_ENC > 0) {
             dac_1 = PRES_A_VREF / 3.3f;
             dac_2 = PRES_B_VREF / 3.3f;
-        } else {
-            dac_1 = PRES_B_VREF / 3.3f;
-            dac_2 = PRES_A_VREF / 3.3f;
-        }
+//        } else {
+//            dac_1 = PRES_B_VREF / 3.3f;
+//            dac_2 = PRES_A_VREF / 3.3f;
+//        }
     }
     make_delay();
 
@@ -383,6 +384,43 @@
     Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
 }
 
+// Valve feedforward for SW valve
+void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS_10000)
+{
+    int i = 0;
+    float valve_pos_ref_dz = 0.0f;
+    if(REF_VALVE_POS_10000 > 10000.0f) {
+        REF_VALVE_POS_10000 = 10000.0f;
+    } else if(REF_VALVE_POS_10000 < -10000.0f) {
+        REF_VALVE_POS_10000 = -10000.0f;
+    }
+    if (REF_VALVE_POS_10000 >= 0) {
+        valve_pos_ref_dz = REF_VALVE_POS_10000/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS;
+    } else {
+        valve_pos_ref_dz = -REF_VALVE_POS_10000/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS;
+    }
+    valve_pos_err = (float) (valve_pos_ref_dz - value);
+    valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
+    valve_pos_err_old = valve_pos_err;
+    valve_pos_err_sum += valve_pos_err;
+    if (valve_pos_err_sum > 1000.0f) valve_pos_err_sum = 1000.0f;
+    if (valve_pos_err_sum<-1000.0f) valve_pos_err_sum = -1000.0f;
+
+    VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff;
+
+    for(i=0; i<24; i++) {
+        if(valve_pos_ref_dz >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && (float) valve_pos_ref_dz <=  max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) {
+            if(i==0) {
+                VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) valve_pos_ref_dz - VALVE_POS_VS_PWM[i]);
+            } else {
+                VALVE_PWM_RAW_FF = (float) 1000.0f* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) valve_pos_ref_dz - VALVE_POS_VS_PWM[i-1]) + 1000.0f * (float) ID_index_array[i-1];
+            }
+            break;
+        }
+    }
+    Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
+}
+
 // PWM duty vs. voltage output of L6205 in STM board
 #define LT_MAX_IDX  57
 float LT_PWM_duty[LT_MAX_IDX] = {-100.0f, -80.0f, -60.0f, -50.0f, -40.0f, -35.0f, -30.0f, -25.0f, -20.0f,
@@ -457,20 +495,20 @@
             force.UpdateSen((((float)ADC1->DR) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR4, 100.0f); // unit : N
         } else if (SENSING_MODE == 1) { // Pressure sensing
             float pres_A_new, pres_B_new;
-            if (DIR_VALVE_ENC > 0) {
+//            if (DIR_VALVE_ENC > 0) {
                 pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
                 pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
-            } else {
-                pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
-                pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
-            }
+//            } else {
+//                pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
+//                pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
+//            }
             pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f);
             pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f);
 
-            if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
+            if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
                 float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
                 torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f);  // unit : Nm
-            } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
+            } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
                 float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
                 force.UpdateSen(force_new,FREQ_TMR4,1000.0f);  // unit : N
             }
@@ -786,21 +824,21 @@
                         data_num = 0;
 
                     } else if(VALVE_DZ_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
-                        valve_pos.ref = DDV_POS_AVG;
-                        VALVE_POS_CONTROL(valve_pos.ref);
+                        valve_pos_raw.ref = DDV_POS_AVG;
+                        VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                     } else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) {
-                        valve_pos.ref = DDV_POS_AVG;
-                        VALVE_POS_CONTROL(valve_pos.ref);
+                        valve_pos_raw.ref = DDV_POS_AVG;
+                        VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                     } else if(VALVE_DZ_timer == (int) (6.0f * (float) TMR_FREQ_5k)) {
-                        valve_pos.ref = DDV_POS_AVG;
-                        VALVE_POS_CONTROL(valve_pos.ref);
+                        valve_pos_raw.ref = DDV_POS_AVG;
+                        VALVE_POS_CONTROL(valve_pos_raw.ref);
                         FINAL_POS = pos.sen;
 
-                        if((FINAL_POS - START_POS)>10) {
+                        if((FINAL_POS - START_POS)>1) {
                             DZ_case = 1;
-                        } else if((FINAL_POS - START_POS)<-10) {
+                        } else if((FINAL_POS - START_POS)<-1) {
                             DZ_case = -1;
                         } else {
                             DZ_case = 0;
@@ -821,21 +859,21 @@
                         } 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)) {
-                            valve_pos.ref = Ref_Valve_Pos_Old  - DZ_case * DZ_DIRECTION * 64 / DZ_index;
-                            if(valve_pos.ref <= VALVE_MIN_POS) {
-                                valve_pos.ref = VALVE_MIN_POS;
-                            } else if(valve_pos.ref >= VALVE_MAX_POS) {
-                                valve_pos.ref = VALVE_MAX_POS;
+                            valve_pos_raw.ref = Ref_Valve_Pos_Old  - DZ_case * DZ_DIRECTION * 64 / DZ_index;
+                            if(valve_pos_raw.ref <= VALVE_MIN_POS) {
+                                valve_pos_raw.ref = VALVE_MIN_POS;
+                            } else if(valve_pos_raw.ref >= VALVE_MAX_POS) {
+                                valve_pos_raw.ref = VALVE_MAX_POS;
                             }
-                            VALVE_POS_CONTROL(valve_pos.ref);
+                            VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            Ref_Valve_Pos_Old = valve_pos_raw.ref;
                             FINAL_POS = pos.sen;
 
-                            if((FINAL_POS - START_POS)>2) {
+                            if((FINAL_POS - START_POS)>1) {
                                 DZ_DIRECTION = 1 * DZ_case;
-                            } else if((FINAL_POS - START_POS)<-2) {
+                            } else if((FINAL_POS - START_POS)<-1) {
                                 DZ_DIRECTION = -1 * DZ_case;
                             } else {
                                 DZ_DIRECTION = 1 * DZ_case;
@@ -844,7 +882,7 @@
                             VALVE_DZ_timer = 0;
                             DZ_index= DZ_index *2;
                             if(DZ_index >= 128) {
-                                FIRST_DZ = valve_pos.ref;
+                                FIRST_DZ = valve_pos_raw.ref;
                                 DZ_NUM = 2;
                                 Ref_Valve_Pos_Old = FIRST_DZ;
                                 DZ_index = 1;
@@ -857,23 +895,23 @@
                         } 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)) {
-                            valve_pos.ref = Ref_Valve_Pos_Old  - DZ_case * DZ_DIRECTION * 64 / DZ_index;
-                            if(valve_pos.ref <= VALVE_MIN_POS) {
-                                valve_pos.ref = VALVE_MIN_POS;
-                            } else if(valve_pos.ref >= VALVE_MAX_POS) {
-                                valve_pos.ref = VALVE_MAX_POS;
+                            valve_pos_raw.ref = Ref_Valve_Pos_Old  - DZ_case * DZ_DIRECTION * 64 / DZ_index;
+                            if(valve_pos_raw.ref <= VALVE_MIN_POS) {
+                                valve_pos_raw.ref = VALVE_MIN_POS;
+                            } else if(valve_pos_raw.ref >= VALVE_MAX_POS) {
+                                valve_pos_raw.ref = VALVE_MAX_POS;
                             }
-                            VALVE_POS_CONTROL(valve_pos.ref);
+                            VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
                             Vout.ref = 0.0f;
                         } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            Ref_Valve_Pos_Old = valve_pos_raw.ref;
                             FINAL_POS = pos.sen;
 
-                            if((FINAL_POS - START_POS)>2) {
+                            if((FINAL_POS - START_POS)>1) {
                                 DZ_DIRECTION = 1 * DZ_case;
-                            } else if((FINAL_POS - START_POS)<-2) {
+                            } else if((FINAL_POS - START_POS)<-1) {
                                 DZ_DIRECTION = -1 * DZ_case;
                             } else {
                                 DZ_DIRECTION = -1 * DZ_case;
@@ -882,7 +920,7 @@
                             VALVE_DZ_timer = 0;
                             DZ_index= DZ_index * 2;
                             if(DZ_index >= 128) {
-                                SECOND_DZ = valve_pos.ref;
+                                SECOND_DZ = valve_pos_raw.ref;
                                 VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
                                 first_check = 0;
                                 VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
@@ -902,21 +940,21 @@
                         } 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)) {
-                            valve_pos.ref = Ref_Valve_Pos_Old  - DZ_DIRECTION * 64 / DZ_index;
-                            if(valve_pos.ref <= VALVE_MIN_POS) {
-                                valve_pos.ref = VALVE_MIN_POS;
-                            } else if(valve_pos.ref >= VALVE_MAX_POS) {
-                                valve_pos.ref = VALVE_MAX_POS;
+                            valve_pos_raw.ref = Ref_Valve_Pos_Old  - DZ_DIRECTION * 64 / DZ_index;
+                            if(valve_pos_raw.ref <= VALVE_MIN_POS) {
+                                valve_pos_raw.ref = VALVE_MIN_POS;
+                            } else if(valve_pos_raw.ref >= VALVE_MAX_POS) {
+                                valve_pos_raw.ref = VALVE_MAX_POS;
                             }
-                            VALVE_POS_CONTROL(valve_pos.ref);
+                            VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            Ref_Valve_Pos_Old = valve_pos_raw.ref;
                             FINAL_POS = pos.sen;
 
-                            if((FINAL_POS - START_POS)>2) {
+                            if((FINAL_POS - START_POS)>1) {
                                 DZ_DIRECTION = 1;
-                            } else if((FINAL_POS - START_POS)<-2) {
+                            } else if((FINAL_POS - START_POS)<-1) {
                                 DZ_DIRECTION = -1;
                             } else {
                                 DZ_DIRECTION = 1;
@@ -924,7 +962,7 @@
                             VALVE_DZ_timer = 0;
                             DZ_index= DZ_index *2;
                             if(DZ_index >= 128) {
-                                FIRST_DZ = valve_pos.ref;
+                                FIRST_DZ = valve_pos_raw.ref;
                                 DZ_NUM = 2;
                                 Ref_Valve_Pos_Old = FIRST_DZ;
                                 DZ_index = 1;
@@ -937,23 +975,23 @@
                         } 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)) {
-                            valve_pos.ref = Ref_Valve_Pos_Old  + DZ_DIRECTION * 64 / DZ_index;
-                            if(valve_pos.ref <= VALVE_MIN_POS) {
-                                valve_pos.ref = VALVE_MIN_POS;
-                            } else if(valve_pos.ref > VALVE_MAX_POS) {
-                                valve_pos.ref = VALVE_MAX_POS;
+                            valve_pos_raw.ref = Ref_Valve_Pos_Old  + DZ_DIRECTION * 64 / DZ_index;
+                            if(valve_pos_raw.ref <= VALVE_MIN_POS) {
+                                valve_pos_raw.ref = VALVE_MIN_POS;
+                            } else if(valve_pos_raw.ref > VALVE_MAX_POS) {
+                                valve_pos_raw.ref = VALVE_MAX_POS;
                             }
-                            VALVE_POS_CONTROL(valve_pos.ref);
+                            VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
                             Vout.ref = 0.0f;
                         } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            Ref_Valve_Pos_Old = valve_pos_raw.ref;
                             FINAL_POS = pos.sen;
 
-                            if((FINAL_POS - START_POS)>2) {
+                            if((FINAL_POS - START_POS)>1) {
                                 DZ_DIRECTION = -1;
-                            } else if((FINAL_POS - START_POS)<-2) {
+                            } else if((FINAL_POS - START_POS)<-1) {
                                 DZ_DIRECTION = 1;
                             } else {
                                 DZ_DIRECTION = 1;
@@ -962,7 +1000,7 @@
                             VALVE_DZ_timer = 0;
                             DZ_index= DZ_index *2;
                             if(DZ_index >= 128) {
-                                SECOND_DZ = valve_pos.ref;
+                                SECOND_DZ = valve_pos_raw.ref;
                                 VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
                                 first_check = 0;
                                 VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
@@ -980,7 +1018,7 @@
                 }
                 break;
             }
-            
+
             case MODE_STEP_TEST: {
                 float valve_pos_ref = 0.0f;
                 if (cnt_step_test < (int) (1.0f * (float) TMR_FREQ_5k)) {
@@ -989,10 +1027,13 @@
                     valve_pos_ref = 10000.0f;
                 }
                 if(valve_pos_ref >= 0) {
-                    valve_pos.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
                 } else {
-                    valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
                 }
+                
+                VALVE_POS_CONTROL(valve_pos_raw.ref);
+                
                 ref_array[cnt_step_test] = valve_pos_ref;
                 if(value>=(float) VALVE_ELECTRIC_CENTER) {
                     pos_array[cnt_step_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER);
@@ -1000,7 +1041,7 @@
                     pos_array[cnt_step_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER);
                 }
 
-                CONTROL_MODE = MODE_VALVE_POSITION_CONTROL;
+                CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
                 cnt_step_test++;
                 if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k)) {
                     buffer_data_size = cnt_step_test;
@@ -1024,14 +1065,17 @@
                 CONTROL_MODE = MODE_NO_ACT;
                 break;
             }
-            
+
             case MODE_FREQ_TEST: {
                 float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3);
                 if(valve_pos_ref >= 0) {
-                    valve_pos.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
                 } else {
-                    valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
                 }
+                
+                VALVE_POS_CONTROL(valve_pos_raw.ref);
+                
                 ref_array[cnt_freq_test] = valve_pos_ref;
                 if(value>=(float) VALVE_ELECTRIC_CENTER) {
                     pos_array[cnt_freq_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER);
@@ -1039,7 +1083,7 @@
                     pos_array[cnt_freq_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER);
                 }
 
-                CONTROL_MODE = MODE_VALVE_POSITION_CONTROL;
+                CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
                 cnt_freq_test++;
                 if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3 > 2) {
                     buffer_data_size = cnt_freq_test;
@@ -1071,7 +1115,7 @@
 
             case MODE_VALVE_POSITION_CONTROL: {
                 if (OPERATING_MODE == 5) { //SW Valve
-                    VALVE_POS_CONTROL(valve_pos.ref);
+                    VALVE_POS_CONTROL_DZ(valve_pos.ref);
                     V_out = Vout.ref;
                 } else if (CURRENT_CONTROL_MODE == 0) { //PWM
                     V_out = valve_pos.ref;
@@ -1100,7 +1144,7 @@
                 vel.err = vel.ref - vel.sen; // Unit : mm/s or deg/s
 
                 // position control command ===============================================================================================================================================
-                if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode
                     temp_vel_pos = 0.1f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err) * PI / 180.0f; // rad/s
                     //                            L when P-gain = 100, f_cut = 10Hz
                 } else {
@@ -1110,24 +1154,25 @@
 
                 // torque control command ===============================================================================================================================================
                 float alpha_SpringDamper = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*30.0f));
-                K_LPF = alpha_SpringDamper * K_LPF + (1.0f-alpha_SpringDamper) * K_SPRING;
-                D_LPF = alpha_SpringDamper * D_LPF + (1.0f-alpha_SpringDamper) * D_DAMPER;
+                K_LPF = (1.0f-alpha_SpringDamper) * K_LPF + alpha_SpringDamper * K_SPRING;
+                D_LPF = (1.0f-alpha_SpringDamper) * D_LPF + alpha_SpringDamper * D_DAMPER;
 
-                if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
-                    float torq_ref_act = torq.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : Nm
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode
+                    float torq_ref_act = torq.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : Nm
                     torq.err = torq_ref_act - torq.sen;
                     torq.err_int += torq.err/((float)TMR_FREQ_5k);
                     temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
                 } else {
-                    float force_ref_act = force.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : N
+                    float force_ref_act = force.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : N
                     force.err = force_ref_act - force.sen;
+//                    force.err = 0.0f - force.sen;
                     force.err_int += force.err/((float)TMR_FREQ_5k);
                     temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
                 }
 
 
                 // velocity feedforward command ========================================================================================================================================
-                if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode
                     temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref * PI / 180.0f; // rad/s
                 } else {
                     temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref; // mm/s
@@ -1142,6 +1187,7 @@
                     Qact = temp_vel * ((float)PISTON_AREA_A * 0.00006f); // mm^3/sec >> LPM
                     if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
                         I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))))/C_d;
+//                        I_REF = Qact*2.0f;
                     } else {    // SW valve
                         valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)));
                     }
@@ -1149,6 +1195,7 @@
                     Qact = temp_vel * ((float)PISTON_AREA_B * 0.00006f); // mm^3/sec >> LPM
                     if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
                         I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d;
+//                        I_REF = Qact*2.0f;
                     } else {    // SW valve
                         valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)));
                     }
@@ -1206,12 +1253,12 @@
                             valve_pos_pulse = -10000.0f;
                         }
                     }
-                    if (valve_pos_pulse >= 0) {
-                        valve_pos.ref = valve_pos_pulse/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS;
-                    } else {
-                        valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS;
-                    }
-                    VALVE_POS_CONTROL(valve_pos.ref);
+//                    if (valve_pos_pulse >= 0) {
+//                        valve_pos.ref = valve_pos_pulse/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS;
+//                    } else {
+//                        valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS;
+//                    }
+                    VALVE_POS_CONTROL_DZ(valve_pos_pulse);
                     V_out = Vout.ref;
                 }
                 break;
@@ -1340,7 +1387,7 @@
                 I_REF_fil_old = I_REF_fil_DZ;
 //                VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV
                 VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV
-                double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
+                double V_MAX = VALVE_VOLTAGE_LIMIT*1000.0f; // Maximum Voltage : 12V = 12000mV
 
                 double Ka = 3.0f / KP_I;
                 if (VALVE_PWM_RAW > V_MAX) {
@@ -1431,7 +1478,8 @@
 
             // Valve Position (ID:1300)
             if (flag_data_request[1] == HIGH) {
-                CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
+//                CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
+                CAN_TX_PWM((int16_t)(alpha3));
             }
 
             // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
@@ -1448,6 +1496,9 @@
                 } else {
                     valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
                 }
+
+                valve_pos_ref_can = (float)valve_pos.ref;
+
                 CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_ref_can);
             }
 
--- a/setting.h	Wed Jul 07 10:09:21 2021 +0000
+++ b/setting.h	Tue Jul 13 01:40:56 2021 +0000
@@ -155,7 +155,7 @@
 extern int16_t VALVE_DEADZONE_MINUS;
 
 extern int16_t VELOCITY_COMP_GAIN;
-extern int16_t COMPLIANCE_GAIN;
+//extern int16_t COMPLIANCE_GAIN;
 
 extern int16_t VALVE_CENTER;