rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
Lightvalve
Date:
Wed Jul 07 10:09:21 2021 +0000
Parent:
236:fd7961dfa78a
Child:
238:4e660aa77eb7
Commit message:
210707

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	Fri Jun 25 12:21:04 2021 +0000
+++ b/CAN/function_CAN.cpp	Wed Jul 07 10:09:21 2021 +0000
@@ -649,6 +649,22 @@
         case CRX_SET_ROM: {
             break;
         }
+        
+        case CRX_ASK_BUFFER: {
+            cnt_buffer = (int16_t) (msg.data[1] | msg.data[2] << 8);
+            CAN_TX_BUFFER(cnt_buffer);
+            break;
+        }
+        case CRX_SET_STEP_TEST: {
+            cnt_step_test = 0;
+            CONTROL_UTILITY_MODE = 37;
+            break;
+        }
+        case CRX_SET_FREQ_TEST: {
+            cnt_freq_test = 0;
+            CONTROL_UTILITY_MODE = 34;
+            break;
+        }
 
         default:
             break;
@@ -720,9 +736,9 @@
             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_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_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;
             } else {
-                valve_pos.ref = (double)VALVE_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_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;
             }
         }
     } else if(address==CID_RX_REF_PWM) {
@@ -1228,10 +1244,10 @@
     CANMessage temp_msg;
     int16_t valve_pos_vs_pwm;
 
-    if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_CENTER) {
-        valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
+    if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_ELECTRIC_CENTER) {
+        valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER);
     } else {
-        valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
+        valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER);
     }
 
     int16_t PWM_VALVE_ID;
@@ -1388,6 +1404,18 @@
     can.write(temp_msg);
 }
 
+void CAN_TX_TORQUE(int16_t t_valve_pos)
+{
+    CANMessage temp_msg;
+
+    temp_msg.id = CID_TX_PWM;
+    temp_msg.len = 2;
+    temp_msg.data[0] = (uint8_t) t_valve_pos;
+    temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
+
+    can.write(temp_msg);
+}
+
 void CAN_TX_PWM(int16_t t_pwm)
 {
     CANMessage temp_msg;
@@ -1436,4 +1464,16 @@
 
 
     can.write(temp_msg);
+}
+
+void CAN_TX_VOUT(int16_t t_vout)
+{
+    CANMessage temp_msg;
+
+    temp_msg.id = CID_TX_VOUT;
+    temp_msg.len = 2;
+    temp_msg.data[0] = (uint8_t) t_vout;
+    temp_msg.data[1] = (uint8_t) (t_vout >> 8);
+
+    can.write(temp_msg);
 }
\ No newline at end of file
--- a/CAN/function_CAN.h	Fri Jun 25 12:21:04 2021 +0000
+++ b/CAN/function_CAN.h	Wed Jul 07 10:09:21 2021 +0000
@@ -155,11 +155,13 @@
 void CAN_TX_POSITION_FT(int16_t t_pos, int16_t t_vel, int16_t t_torq);
 void CAN_TX_POSITION_PRESSURE(int16_t t_pos, int16_t t_vel, int16_t t_pa, int16_t t_pb);
 //void CAN_TX_PWM(int16_t t_valve_pos, int16_t t_vout);
-void CAN_TX_PWM(int16_t t_valve_pos);
+//void CAN_TX_PWM(int16_t t_valve_pos);
+void CAN_TX_TORQUE(int16_t t_valve_pos);
 void CAN_TX_CURRENT(int16_t t_pres_a, int16_t t_pres_b);
 void CAN_TX_PWM(int16_t t_pwm);
 void CAN_TX_VALVE_POSITION(int16_t t_valve_pos_1, int16_t t_valve_pos_2, int16_t t_ref_valve_pos, int16_t t_pwm);
 void CAN_TX_SOMETHING (int16_t t_a, int16_t t_b, int16_t t_c, int16_t t_d);
+void CAN_TX_VOUT(int16_t t_vout);
 
 // Information Transmission
 void CAN_TX_INFO(void);
--- a/SPI_EEP_ENC/SPI_EEP_ENC.h	Fri Jun 25 12:21:04 2021 +0000
+++ b/SPI_EEP_ENC/SPI_EEP_ENC.h	Wed Jul 07 10:09:21 2021 +0000
@@ -91,6 +91,7 @@
 #define             RID_VALVE_POS_NUM                   54
 //#define             RID_DDV_CENTER                      55
 #define             RID_VALVE_CENTER_OFFSET             56
+#define             RID_VALVE_ELECTRIC_CENTER           57
 
 #define             RID_VALVE_GAIN_PLUS_1              60
 #define             RID_VALVE_GAIN_MINUS_1             61
@@ -180,6 +181,7 @@
 #define             RID_CURRENT_CONTROL_MODE            222
 #define             RID_FLAG_VALVE_DEADZONE             223
 
+
 #define             RID_K_SPRING                        57
 #define             RID_D_DAMPER                        58
 
--- a/function_utilities/function_utilities.cpp	Fri Jun 25 12:21:04 2021 +0000
+++ b/function_utilities/function_utilities.cpp	Wed Jul 07 10:09:21 2021 +0000
@@ -171,6 +171,7 @@
 
 int VALVE_MAX_POS;
 int VALVE_MIN_POS;
+int VALVE_ELECTRIC_CENTER;
 int VALVE_POS_NUM;
 float VALVE_CENTER_OFFSET;
 float VALVE_DZ_MINUS_OFFSET;
@@ -464,6 +465,7 @@
     VALVE_MAX_POS = spi_eeprom_read(RID_VALVE_MAX_POS);
     VALVE_MIN_POS = spi_eeprom_read(RID_VALVE_MIN_POS);
     VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM);
+    VALVE_ELECTRIC_CENTER = spi_eeprom_read(RID_VALVE_ELECTRIC_CENTER);
 
 //    K_SPRING = spi_eeprom_read(RID_K_SPRING);
 //    D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
--- a/main.cpp	Fri Jun 25 12:21:04 2021 +0000
+++ b/main.cpp	Wed Jul 07 10:09:21 2021 +0000
@@ -240,7 +240,7 @@
     make_delay();
 
     ////// bno rom
-//    spi_eeprom_write(RID_BNO, (int16_t) 2);
+//    spi_eeprom_write(RID_BNO, (int16_t) 1);
 //    make_delay();
     ////////
 
@@ -541,17 +541,19 @@
                 break;
         }
 
-        if (((OPERATING_MODE&0b010)>>1) == 0) {
+        if (((OPERATING_MODE&0b110)>>1) == 0) {
             K_v = 1.03f; // Q = K_v*sqrt(deltaP)*tanh(C_d*Xv);
             C_d = 0.16f;
             mV_PER_mA = 500.0f; // 5000mV/10mA
             mV_PER_pulse = 0.5f; // 5000mV/10000pulse
             mA_PER_pulse = 0.001f; // 10mA/10000pulse
-        } else if (((OPERATING_MODE&0b010)>>1) == 1) {
+        } else if (((OPERATING_MODE&0b110)>>1) == 1) {
             K_v = 0.5f; // KNR (LPM >> mA) , 100bar
             mV_PER_mA = 166.6666f; // 5000mV/30mA
             mV_PER_pulse = 0.5f; // 5000mV/10000pulse
             mA_PER_pulse = 0.003f; // 30mA/10000pulse
+        } else if (((OPERATING_MODE&0b110)>>1) == 2) {
+            C_d = 0.0000845f; // Q = C_d * Valve_pos * sqrt(deltaP*alpha/(1+alpha)) : Valve_pos = 10000, deltaP = 70, alpha = 1 -> Q = 5
         }
 
         // =====================================================================
@@ -706,6 +708,356 @@
                 }
                 break;
             }
+
+            case MODE_DDV_POS_VS_PWM_ID: {
+                CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
+                VALVE_ID_timer = VALVE_ID_timer + 1;
+
+                if(VALVE_ID_timer < TMR_FREQ_5k*1) {
+                    Vout.ref = 3000.0f * sin(2.0f*3.14159f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f);
+                } else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
+                    Vout.ref = 1000.0f*(ID_index_array[ID_index]);
+                } else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
+                    VALVE_POS_TMP = 0;
+                    data_num = 0;
+                } else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
+                    data_num = data_num + 1;
+                    VALVE_POS_TMP = VALVE_POS_TMP + value;
+                } else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
+                    Vout.ref = 0.0f;
+                } else {
+                    VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
+                    VALVE_ID_timer = 0;
+                    ID_index= ID_index +1;
+                }
+
+                if(ID_index>=25) {
+                    int i;
+                    VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
+                    for(i=0; i<25; i++) {
+                        VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
+                        if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) {
+                            VALVE_MAX_POS = VALVE_POS_AVG[i];
+                            VALVE_POS_AVG_OLD = VALVE_MAX_POS;
+                        } else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) {
+                            VALVE_MIN_POS = VALVE_POS_AVG[i];
+                            VALVE_POS_AVG_OLD = VALVE_MIN_POS;
+                        }
+                    }
+                    VALVE_ELECTRIC_CENTER = VALVE_POS_VS_PWM[0];
+                    spi_eeprom_write(RID_VALVE_ELECTRIC_CENTER, (int16_t) VALVE_ELECTRIC_CENTER);
+                    spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
+                    spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
+                    for(int i=0; i<25; i++) {
+                        spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) VALVE_POS_VS_PWM[i]);
+                    }
+                    ID_index = 0;
+                    CONTROL_UTILITY_MODE = MODE_NO_ACT;
+                }
+
+
+                break;
+            }
+            case MODE_DDV_DEADZONE_AND_CENTER: {
+                CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
+                VALVE_DZ_timer = VALVE_DZ_timer + 1;
+                if(first_check == 0) {
+                    if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
+                        Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
+                    } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
+                        Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
+                        pos_plus_end = pos.sen;
+                    } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
+                        Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+                    } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
+                        Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+                        pos_minus_end = pos.sen;
+                    } else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) {
+                        Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
+                    } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
+                        Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
+                        data_num = data_num + 1;
+                        VALVE_POS_TMP = VALVE_POS_TMP + value;
+                    } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
+                        Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
+                        DDV_POS_AVG = VALVE_POS_TMP / data_num;
+                        START_POS = pos.sen;
+                        VALVE_POS_TMP = 0;
+                        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);
+
+                    } else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) {
+                        valve_pos.ref = DDV_POS_AVG;
+                        VALVE_POS_CONTROL(valve_pos.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);
+                        FINAL_POS = pos.sen;
+
+                        if((FINAL_POS - START_POS)>10) {
+                            DZ_case = 1;
+                        } else if((FINAL_POS - START_POS)<-10) {
+                            DZ_case = -1;
+                        } else {
+                            DZ_case = 0;
+                        }
+
+                        first_check = 1;
+                        DZ_DIRECTION = 1;
+                        VALVE_DZ_timer = 0;
+                        Ref_Valve_Pos_Old = DDV_POS_AVG;
+                        DZ_NUM = 1;
+                        DZ_index = 1;
+
+                    }
+                } 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)) {
+                            Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
+                        } 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_CONTROL(valve_pos.ref);
+
+                        } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
+                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            FINAL_POS = pos.sen;
+
+                            if((FINAL_POS - START_POS)>2) {
+                                DZ_DIRECTION = 1 * DZ_case;
+                            } else if((FINAL_POS - START_POS)<-2) {
+                                DZ_DIRECTION = -1 * DZ_case;
+                            } else {
+                                DZ_DIRECTION = 1 * DZ_case;
+                            }
+
+                            VALVE_DZ_timer = 0;
+                            DZ_index= DZ_index *2;
+                            if(DZ_index >= 128) {
+                                FIRST_DZ = valve_pos.ref;
+                                DZ_NUM = 2;
+                                Ref_Valve_Pos_Old = FIRST_DZ;
+                                DZ_index = 1;
+                                DZ_DIRECTION = 1;
+                            }
+                        }
+                    } 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)) {
+                            Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
+                        } 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_CONTROL(valve_pos.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;
+                            FINAL_POS = pos.sen;
+
+                            if((FINAL_POS - START_POS)>2) {
+                                DZ_DIRECTION = 1 * DZ_case;
+                            } else if((FINAL_POS - START_POS)<-2) {
+                                DZ_DIRECTION = -1 * DZ_case;
+                            } else {
+                                DZ_DIRECTION = -1 * DZ_case;
+                            }
+
+                            VALVE_DZ_timer = 0;
+                            DZ_index= DZ_index * 2;
+                            if(DZ_index >= 128) {
+                                SECOND_DZ = valve_pos.ref;
+                                VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
+                                first_check = 0;
+                                VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
+                                VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
+
+                                spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
+                                spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) VALVE_DEADZONE_PLUS);
+                                spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) VALVE_DEADZONE_MINUS);
+
+                                CONTROL_UTILITY_MODE = MODE_NO_ACT;
+                                DZ_index = 1;
+                            }
+                        }
+                    } else if(DZ_case == 0 && DZ_NUM ==1) {
+                        if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
+                            Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
+                        } 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_CONTROL(valve_pos.ref);
+
+                        } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
+                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            FINAL_POS = pos.sen;
+
+                            if((FINAL_POS - START_POS)>2) {
+                                DZ_DIRECTION = 1;
+                            } else if((FINAL_POS - START_POS)<-2) {
+                                DZ_DIRECTION = -1;
+                            } else {
+                                DZ_DIRECTION = 1;
+                            }
+                            VALVE_DZ_timer = 0;
+                            DZ_index= DZ_index *2;
+                            if(DZ_index >= 128) {
+                                FIRST_DZ = valve_pos.ref;
+                                DZ_NUM = 2;
+                                Ref_Valve_Pos_Old = FIRST_DZ;
+                                DZ_index = 1;
+                                DZ_DIRECTION = 1;
+                            }
+                        }
+                    } else {
+                        if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
+                            Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
+                        } 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_CONTROL(valve_pos.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;
+                            FINAL_POS = pos.sen;
+
+                            if((FINAL_POS - START_POS)>2) {
+                                DZ_DIRECTION = -1;
+                            } else if((FINAL_POS - START_POS)<-2) {
+                                DZ_DIRECTION = 1;
+                            } else {
+                                DZ_DIRECTION = 1;
+                            }
+
+                            VALVE_DZ_timer = 0;
+                            DZ_index= DZ_index *2;
+                            if(DZ_index >= 128) {
+                                SECOND_DZ = valve_pos.ref;
+                                VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
+                                first_check = 0;
+                                VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
+                                VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
+
+                                spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
+                                spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) VALVE_DEADZONE_PLUS);
+                                spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) VALVE_DEADZONE_MINUS);
+
+                                CONTROL_UTILITY_MODE = MODE_NO_ACT;
+                                DZ_index = 1;
+                            }
+                        }
+                    }
+                }
+                break;
+            }
+            
+            case MODE_STEP_TEST: {
+                float valve_pos_ref = 0.0f;
+                if (cnt_step_test < (int) (1.0f * (float) TMR_FREQ_5k)) {
+                    valve_pos_ref = 0.0f;
+                } else {
+                    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;
+                } else {
+                    valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                }
+                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);
+                } else {
+                    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;
+                cnt_step_test++;
+                if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k)) {
+                    buffer_data_size = cnt_step_test;
+                    cnt_step_test = 0;
+                    cnt_send_buffer = 0;
+                    CONTROL_UTILITY_MODE = MODE_SEND_OVER;
+                    CONTROL_MODE = MODE_NO_ACT;
+                }
+//                if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k))
+//                {
+//                    CONTROL_UTILITY_MODE = MODE_NO_ACT;
+//                    CONTROL_MODE = MODE_NO_ACT;
+//                    CAN_TX_PWM((int16_t) (1)); //1300
+//                }
+
+                break;
+            }
+            case MODE_SEND_OVER: {
+                CAN_TX_TORQUE((int16_t) (buffer_data_size)); //1300
+                CONTROL_UTILITY_MODE = MODE_NO_ACT;
+                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;
+                } else {
+                    valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                }
+                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);
+                } else {
+                    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;
+                cnt_freq_test++;
+                if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3 > 2) {
+                    buffer_data_size = cnt_freq_test;
+                    cnt_freq_test = 0;
+                    cnt_send_buffer = 0;
+                    freq_test_valve_ref = freq_test_valve_ref * 1.05f;
+                    if (freq_test_valve_ref >= 400) {
+                        CONTROL_UTILITY_MODE = MODE_NO_ACT;
+                        CONTROL_MODE = MODE_NO_ACT;
+                        CAN_TX_VOUT((int16_t) (1)); //1300
+                    }
+                    CONTROL_MODE = MODE_NO_ACT;
+                    CONTROL_UTILITY_MODE = MODE_SEND_OVER;
+
+                }
+                break;
+            }
+
             default:
                 break;
         }
@@ -783,39 +1135,84 @@
 
                 // command integration =================================================================================================================================================
                 temp_vel = (1.0f - alpha_trans) * temp_vel_pos  + alpha_trans * temp_vel_FT + temp_vel_ff; // Position Control + Torque Control + Velocity Feedforward
-                
+
                 float Qact = 0.0f; // required flow rate
+                float valve_pos_pulse = 0.0f;
                 if( temp_vel > 0.0f ) {
                     Qact = temp_vel * ((float)PISTON_AREA_A * 0.00006f); // mm^3/sec >> LPM
-                    I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))))/C_d;
+                    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;
+                    } else {    // SW valve
+                        valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)));
+                    }
                 } else {
                     Qact = temp_vel * ((float)PISTON_AREA_B * 0.00006f); // mm^3/sec >> LPM
-                    I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d;
+                    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;
+                    } else {    // SW valve
+                        valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)));
+                    }
                 }
-                
-                float I_MAX = 10.0f; // Maximum Current : 10mA
-                
-                // Anti-windup for FT
-//                if (I_GAIN_JOINT_TORQUE != 0.0f) {
-                if (I_GAIN_JOINT_TORQUE > 0.001f) {
-                    float Ka = 2.0f;
-                    if (I_REF > I_MAX) {
-                        float I_rem = I_REF - I_MAX;
-                        I_REF = I_MAX;
-                        float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
-                        torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
-                    } else if (I_REF < -I_MAX) {
-                        double I_rem = I_REF - (-I_MAX);
-                        I_REF = -I_MAX;
-                        float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
-                        torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
+
+
+
+
+                if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
+
+                    float I_MAX = 10.0f; // Maximum Current : 10mA
+                    // Anti-windup for FT
+                    //                if (I_GAIN_JOINT_TORQUE != 0.0f) {
+                    if (I_GAIN_JOINT_TORQUE > 0.001f) {
+                        float Ka = 2.0f;
+                        if (I_REF > I_MAX) {
+                            float I_rem = I_REF - I_MAX;
+                            I_REF = I_MAX;
+                            float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
+                            torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
+                        } else if (I_REF < -I_MAX) {
+                            double I_rem = I_REF - (-I_MAX);
+                            I_REF = -I_MAX;
+                            float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
+                            torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
+                        }
+                    } else {
+                        if(I_REF > I_MAX) {
+                            I_REF = I_MAX;
+                        } else if (I_REF < -I_MAX) {
+                            I_REF = -I_MAX;
+                        }
                     }
-                } else {
-                    if(I_REF > I_MAX) {
-                        I_REF = I_MAX;
-                    } else if (I_REF < -I_MAX) {
-                        I_REF = -I_MAX;  
+                } else {    //SW valve
+                    float Valve_pos_MAX = 10000.0f; // Maximum Valve Pos : 10000
+                    // Anti-windup for FT
+                    //                if (I_GAIN_JOINT_TORQUE != 0.0f) {
+                    if (I_GAIN_JOINT_TORQUE > 0.001f) {
+                        float Ka = 2.0f;
+                        if (valve_pos_pulse > Valve_pos_MAX) {
+                            float valve_pos_rem = valve_pos_pulse - Valve_pos_MAX;
+                            valve_pos_pulse = Valve_pos_MAX;
+                            float temp_vel_rem = C_d * valve_pos_rem * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
+                            torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
+                        } else if (valve_pos_pulse < -Valve_pos_MAX) {
+                            double valve_pos_rem = valve_pos_pulse - (-Valve_pos_MAX);
+                            valve_pos_pulse = -Valve_pos_MAX;
+                            float temp_vel_rem = C_d * valve_pos_rem * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
+                            torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
+                        }
+                    } else {
+                        if(valve_pos_pulse > 10000.0f) {
+                            valve_pos_pulse = 10000.0f;
+                        } else if(valve_pos_pulse < -10000.0f) {
+                            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);
+                    V_out = Vout.ref;
                 }
                 break;
             }
@@ -1021,14 +1418,14 @@
 
             // Position, Velocity, and Torque (ID:1200)
             if (flag_data_request[0] == HIGH) {
+
                 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
                     CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
 //                    CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
 
                 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
                     CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-//                    CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-//                    CAN_TX_POSITION_FT((int16_t) (logging*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+//                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (valve_pos_can*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
                 }
             }
 
@@ -1039,7 +1436,19 @@
 
             // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
             if (flag_data_request[2] == HIGH) {
-                CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (PRES_SUPPLY), (int16_t) (PRES_SUPPLY_NOM));
+                float valve_pos_can = 0.0f;
+                if(value >= VALVE_ELECTRIC_CENTER) {
+                    valve_pos_can = 10000.0f*((float)value-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
+                } else {
+                    valve_pos_can = -10000.0f*((float)value -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
+                }
+                float valve_pos_ref_can = 0.0f;
+                if(valve_pos.ref >= VALVE_ELECTRIC_CENTER) {
+                    valve_pos_ref_can = 10000.0f*((float)valve_pos.ref-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
+                } else {
+                    valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
+                }
+                CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_ref_can);
             }
 
             TMR2_COUNT_CAN_TX = 0;
--- a/setting.h	Fri Jun 25 12:21:04 2021 +0000
+++ b/setting.h	Wed Jul 07 10:09:21 2021 +0000
@@ -266,6 +266,7 @@
 
 extern int VALVE_MAX_POS;
 extern int VALVE_MIN_POS;
+extern int VALVE_ELECTRIC_CENTER;
 extern int VALVE_POS_NUM;
 extern float VALVE_CENTER_OFFSET;
 extern float VALVE_DZ_MINUS_OFFSET;