rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
hyhoon
Date:
Wed Mar 03 10:56:25 2021 +0000
Parent:
225:9c0becc196ba
Child:
227:699c3e572283
Commit message:
20210305;

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.cpp 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	Thu Feb 04 05:43:47 2021 +0000
+++ b/CAN/function_CAN.cpp	Wed Mar 03 10:56:25 2021 +0000
@@ -126,6 +126,7 @@
             CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]);
             if (CONTROL_MODE == 22) {    //MODE_FIND_HOME
                 FLAG_FIND_HOME = true;
+                FINDHOME_STAGE = FINDHOME_INIT;
             }
             break;
         }
@@ -290,12 +291,12 @@
 
         case CRX_SET_VALVE_DEADZONE: {
             VALVE_CENTER = (int16_t) (msg.data[1] | msg.data[2] << 8);
-            VALVE_DEADZONE_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
-            VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8);
+            VALVE_DEADZONE_PLUS = (int16_t)(msg.data[3] | msg.data[4] << 8);
+            VALVE_DEADZONE_MINUS = (int16_t)(msg.data[5] | msg.data[6] << 8);
 
-            spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
-            spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f));
-            spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f));
+            spi_eeprom_write(RID_VALVE_CNETER, VALVE_CENTER);
+            spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
+            spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
 
             break;
         }
@@ -545,10 +546,6 @@
 
             break;
         }
-        case CRX_ASK_DDV_VALVE_DEADZONE: {
-            CAN_TX_DDV_VALVE_DEADZONE();
-            break;
-        }
 
         case CRX_LOW_REF: {
             REFERENCE_MODE = msg.data[1];
@@ -586,7 +583,7 @@
             break;
         }
         case CRX_SET_HOMEPOS_OFFSET: {
-            HOMEPOS_OFFSET = (int16_t) (msg.data[1] | msg.data[2] << 8);
+            HOMEPOS_OFFSET = (int16_t)(msg.data[1] | msg.data[2] << 8);
             spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
             break;
         }
@@ -619,86 +616,6 @@
         case CRX_SET_ROM: {
             break;
         }
-        case CRX_SET_VALVE_CENTER_OFFSET: {
-            VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
-            VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET;
-            spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
-
-            break;
-        }
-        case CRX_SET_VALVE_DZ_MINUS_OFFSET: {
-            VALVE_DZ_MINUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
-            VALVE_DEADZONE_MINUS = VALVE_DEADZONE_MINUS + VALVE_DZ_MINUS_OFFSET;
-            spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f));
-
-            break;
-        }
-        case CRX_SET_VALVE_DZ_PLUS_OFFSET: {
-            VALVE_DZ_PLUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
-            VALVE_DEADZONE_PLUS = VALVE_DEADZONE_PLUS + VALVE_DZ_PLUS_OFFSET;
-            spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f));
-            break;
-        }
-        case CRX_SET_PID_GAIN_OPP: {
-            if (msg.data[1] == 0) {
-                P_GAIN_VALVE_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
-                I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
-                D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
-                //spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
-                //spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
-                //spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
-            } else if (msg.data[1] == 1) {
-                P_GAIN_JOINT_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
-                I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
-                D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
-                //spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
-                //spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
-                //spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
-            } else if (msg.data[1] == 2) {
-                P_GAIN_JOINT_TORQUE = (int16_t) (msg.data[2] | msg.data[3] << 8);
-                I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8);
-                D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8);
-                //spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
-                //spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
-                //spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
-            }
-            break;
-        }
-        case CRX_ASK_VALVE_MAX_MIN_POS: {
-            CAN_TX_DDV_VALVE_MAX_MIN_POS();
-            break;
-        }
-        case CRX_DELAY_TEST: {
-            flag_delay_test = 1;
-            break;
-        }
-        case CRX_SET_NN_CONTROL_FLAG: {
-            NN_Control_Flag = (int16_t) msg.data[1];
-            CONTROL_UTILITY_MODE = 1;
-            break;
-        }
-        case CRX_SET_FREQ_TEST: {
-            cnt_freq_test = 0;
-            CONTROL_UTILITY_MODE = 34;
-            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_CHANGE_EVERY_REFERNCE: {
-            flag_every_reference = 1;
-            break;
-        }
 
         default:
             break;
@@ -721,8 +638,8 @@
         int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
 
         if((OPERATING_MODE&0b001)==0) { // Rotary Actuator
-            pos.ref = (double)temp_pos * 1.0f;
-            vel.ref = (double)temp_vel * 10.0f;
+            pos.ref = (double)temp_pos * 10.0f;
+            vel.ref = (double)temp_vel * 256.0f;
         } else { //Linear Actuator
             pos.ref = (double)temp_pos * 10.0f;
             vel.ref = (double)temp_vel * 256.0f;
@@ -733,35 +650,35 @@
         torq_dot.sen = torq.sen-torq_sen_past;
         torq.ref_old = torq.ref;
         torq_sen_past = torq.sen;
-        
-            if(CAN_FREQ == -1) {
-        // Position, Velocity, and Torque (ID:1200)
-        if (flag_data_request[0] == HIGH) {
-            if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
-                if (SENSING_MODE == 0) {
-                    CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
-                } else if (SENSING_MODE == 1) {
-                    CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
-                }
-            } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
-                if (SENSING_MODE == 0) {
-                    CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
-                } else if (SENSING_MODE == 1) {
-                    CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
+
+        if(CAN_FREQ == -1) {
+            // Position, Velocity, and Torque (ID:1200)
+            if (flag_data_request[0] == HIGH) {
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+                    if (SENSING_MODE == 0) {
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen*10.0f));
+                    } else if (SENSING_MODE == 1) {
+                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*10.0f), (int16_t) ((pres_B.sen)*10.0f));
+                    }
+                } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+                    if (SENSING_MODE == 0) {
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
+                    } else if (SENSING_MODE == 1) {
+                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*10.0f), (int16_t) ((pres_B.sen)*10.0f));
+                    }
                 }
             }
-        }
-    
-        // ID:1300
-        if (flag_data_request[1] == HIGH) {
-            CAN_TX_TORQUE((int16_t) 7); //1300
-        }
-    
-        // Reference Current, Current Current (ID:1700)
-        if (flag_data_request[2] == HIGH) {
-            
-            CAN_TX_SOMETHING((int16_t) (I_REF_fil / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (0), (int16_t) (0));
-            //double t_value = 0.0f;
+
+            // ID:1300
+            if (flag_data_request[1] == HIGH) {
+                CAN_TX_TORQUE((int16_t) (pos.ref/10.0f)); //1300
+            }
+
+            // Reference Current, Current Current (ID:1700)
+            if (flag_data_request[2] == HIGH) {
+
+                CAN_TX_SOMETHING((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t)HOMEPOS_OFFSET, (int16_t) (I_ERR / mA_PER_pulse));
+                //double t_value = 0.0f;
 //            if(value>=(float) VALVE_CENTER) {
 //                t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
 //            } else {
@@ -773,29 +690,29 @@
 //            } else {
 //                t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
 //            }
-//    
-//    
+//
+//
 //            CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
-        }
-    
-        //If it doesn't rest, below can can not work.
-        for (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) {
-            ;
+            }
+
+            //If it doesn't rest, below can can not work.
+            for (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) {
+                ;
+            }
+
+            // ID:1500
+            if (flag_data_request[3] == HIGH) {
+                //PWM
+                CAN_TX_PWM((int16_t) (torq.ref)); //1500
+            }
+
+            // ID:1600
+            if (flag_data_request[4] == HIGH) {
+                //valve position
+                CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
+            }
+
         }
-    
-        // ID:1500
-        if (flag_data_request[3] == HIGH) {
-            //PWM
-            CAN_TX_PWM((int16_t) (torq.ref)); //1500
-        }
-    
-        // ID:1600
-        if (flag_data_request[4] == HIGH) {
-            //valve position
-            CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
-        }
-
-    }
 
 
     } else if(address==CID_RX_REF_VALVE_POS) {
@@ -829,7 +746,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 7;
     temp_msg.data[0] = (uint8_t) CTX_SEND_INFO;
     temp_msg.data[1] = (uint8_t) BNO;
     temp_msg.data[2] = (uint8_t) CAN_FREQ;
@@ -847,7 +764,7 @@
 
     temp_msg.id = CID_TX_INFO;
     //temp_msg.len = 2;
-    temp_msg.len = 8;
+    temp_msg.len = 2;
     temp_msg.data[0] = (uint8_t) CTX_SEND_BNO;
     temp_msg.data[1] = (uint8_t) BNO;
 
@@ -859,7 +776,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_SEND_OPERATING_MODE;
     temp_msg.data[1] = (uint8_t) OPERATING_MODE;
     temp_msg.data[2] = (uint8_t) SENSING_MODE;
@@ -874,7 +791,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_CAN_FREQ;
     temp_msg.data[1] = (uint8_t) CAN_FREQ;
     temp_msg.data[2] = (uint8_t) (CAN_FREQ >> 8);
@@ -887,7 +804,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 2;
     temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE;
     temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE;
 
@@ -899,7 +816,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_JOINT_ENC_DIR;
     temp_msg.data[1] = (uint8_t) DIR_JOINT_ENC;
     temp_msg.data[2] = (uint8_t) (DIR_JOINT_ENC >> 8);
@@ -912,7 +829,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DIR;
     temp_msg.data[1] = (uint8_t) DIR_VALVE;
     temp_msg.data[2] = (uint8_t) (DIR_VALVE >> 8);
@@ -925,7 +842,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_ENC_DIR;
     temp_msg.data[1] = (uint8_t) DIR_VALVE_ENC;
     temp_msg.data[2] = (uint8_t) (DIR_VALVE_ENC >> 8);
@@ -940,7 +857,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_SUPPLY;
     temp_msg.data[1] = (uint8_t) (send_voltage_supply);
     temp_msg.data[2] = (uint8_t) (send_voltage_supply >> 8);
@@ -955,7 +872,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_VALVE;
     temp_msg.data[1] = (uint8_t) send_voltage_valve;
     temp_msg.data[2] = (uint8_t) (send_voltage_valve >> 8);
@@ -1010,14 +927,14 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 7;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE;
     temp_msg.data[1] = (uint8_t) VALVE_CENTER;
     temp_msg.data[2] = (uint8_t) (VALVE_CENTER >> 8);
-    temp_msg.data[3] = (uint8_t) (int) (VALVE_DEADZONE_PLUS);
-    temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8);
-    temp_msg.data[5] = (uint8_t) (int) (VALVE_DEADZONE_MINUS);
-    temp_msg.data[6] = (uint8_t) ((int) (VALVE_DEADZONE_MINUS) >> 8);
+    temp_msg.data[3] = (uint8_t) VALVE_DEADZONE_PLUS;
+    temp_msg.data[4] = (uint8_t) (VALVE_DEADZONE_PLUS >> 8);
+    temp_msg.data[5] = (uint8_t) VALVE_DEADZONE_MINUS;
+    temp_msg.data[6] = (uint8_t) (VALVE_DEADZONE_MINUS >> 8);
 
     can.write(temp_msg);
 }
@@ -1027,7 +944,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VELOCITY_COMP_GAIN;
     temp_msg.data[1] = (uint8_t) VELOCITY_COMP_GAIN;
     temp_msg.data[2] = (uint8_t) (VELOCITY_COMP_GAIN >> 8);
@@ -1040,7 +957,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    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);
@@ -1053,7 +970,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_FF;
     temp_msg.data[1] = (uint8_t) VALVE_FF;
     temp_msg.data[2] = (uint8_t) (VALVE_FF >> 8);
@@ -1066,7 +983,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_BULK_MODULUS;
     temp_msg.data[1] = (uint8_t) BULK_MODULUS;
     temp_msg.data[2] = (uint8_t) (BULK_MODULUS >> 8);
@@ -1079,7 +996,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_SEND_CHAMBER_VOLUME;
     temp_msg.data[1] = (uint8_t) CHAMBER_VOLUME_A;
     temp_msg.data[2] = (uint8_t) (CHAMBER_VOLUME_A >> 8);
@@ -1094,7 +1011,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_SEND_PISTON_AREA;
     temp_msg.data[1] = (uint8_t) PISTON_AREA_A;
     temp_msg.data[2] = (uint8_t) (PISTON_AREA_A >> 8);
@@ -1109,7 +1026,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_SEND_PRES;
     temp_msg.data[1] = (uint8_t) PRES_SUPPLY;
     temp_msg.data[2] = (uint8_t) (PRES_SUPPLY >> 8);
@@ -1124,7 +1041,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_LIMIT;
     temp_msg.data[1] = (uint8_t) ENC_LIMIT_MINUS;
     temp_msg.data[2] = (uint8_t) (ENC_LIMIT_MINUS >> 8);
@@ -1139,7 +1056,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_STROKE;
     temp_msg.data[1] = (uint8_t) STROKE;
     temp_msg.data[2] = (uint8_t) (STROKE >> 8);
@@ -1152,7 +1069,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT;
     temp_msg.data[1] = (uint8_t) VALVE_MIN_POS;
     temp_msg.data[2] = (uint8_t) (VALVE_MIN_POS >> 8);
@@ -1167,7 +1084,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION;
     int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION);
     temp_msg.data[1] = (uint8_t) temp_enc_pulse_per_position;
@@ -1181,7 +1098,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
     int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f);
     temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque;
@@ -1195,7 +1112,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR;
     temp_msg.data[1] = (uint8_t) (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f);
     temp_msg.data[2] = (uint8_t) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f) >> 8);
@@ -1212,7 +1129,7 @@
     send_friction = (int16_t) (FRICTION * 10.0f);
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_FRICTION;
     temp_msg.data[1] = (uint8_t) send_friction;
     temp_msg.data[2] = (uint8_t) (send_friction >> 8);
@@ -1225,7 +1142,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 6;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_PLUS;
     temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[0] * 50.0f);
     temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[2] * 50.0f);
@@ -1236,36 +1153,13 @@
     can.write(temp_msg);
 }
 
-void CAN_TX_DDV_VALVE_DEADZONE(void)
-{
-    CANMessage temp_msg;
-    float temp_valve_deadzone_minus = 0.0f;
-    float temp_valve_deadzone_plus = 0.0f;
-    float temp_ddv_center = 0.0f;
-
-    temp_valve_deadzone_plus = (double)VALVE_DEADZONE_PLUS;
-    temp_valve_deadzone_minus = (double)VALVE_DEADZONE_MINUS;
-    temp_ddv_center = (double)VALVE_CENTER;
-
-    temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
-    temp_msg.data[0] = (uint8_t) CTX_SEND_DDV_VALVE_DEADZONE;
-    temp_msg.data[1] = (uint8_t) temp_valve_deadzone_minus;
-    temp_msg.data[2] = (uint8_t) ((int) (temp_valve_deadzone_minus) >> 8);
-    temp_msg.data[3] = (uint8_t) (temp_valve_deadzone_plus);
-    temp_msg.data[4] = (uint8_t) ((int) (temp_valve_deadzone_plus) >> 8);
-    temp_msg.data[5] = (uint8_t) (temp_ddv_center);
-    temp_msg.data[6] = (uint8_t) ((int) (temp_ddv_center) >> 8);
-
-    can.write(temp_msg);
-}
 
 void CAN_TX_VALVE_GAIN_MINUS(void)
 {
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 6;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_MINUS;
     temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[1] * 50.0f);
     temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[3] * 50.0f);
@@ -1281,7 +1175,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 6;
     temp_msg.data[0] = (uint8_t) CTX_SEND_REFENCE_MODE;
     temp_msg.data[1] = (uint8_t) REFERENCE_MODE;
     temp_msg.data[2] = (uint8_t) (int) (REFERENCE_FREQ * 100.0f);
@@ -1296,10 +1190,10 @@
 {
     CANMessage temp_msg;
     int16_t send_homepos_offset;
-    send_homepos_offset = (int16_t) (HOMEPOS_OFFSET);
+    send_homepos_offset = (int16_t)HOMEPOS_OFFSET;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_OFFSET;
     temp_msg.data[1] = (uint8_t) send_homepos_offset;
     temp_msg.data[2] = (uint8_t) (send_homepos_offset >> 8);
@@ -1314,7 +1208,7 @@
     send_homepos_valve_opening = (int16_t) (HOMEPOS_VALVE_OPENING);
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_VALVE_OPENING;
     temp_msg.data[1] = (uint8_t) send_homepos_valve_opening;
     temp_msg.data[2] = (uint8_t) (send_homepos_valve_opening >> 8);
@@ -1337,7 +1231,7 @@
     PWM_VALVE_ID = ID_index_array[canindex] * 1000;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_VALVE_PWM_VS_VALVE_POS;
     temp_msg.data[1] = (uint8_t) PWM_VALVE_ID;
     temp_msg.data[2] = (uint8_t) (PWM_VALVE_ID >> 8);
@@ -1362,7 +1256,7 @@
     }
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 7;
     temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE;
     temp_msg.data[1] = (uint8_t) temp_valve_pos;
     temp_msg.data[2] = (uint8_t) (temp_valve_pos >> 8);
@@ -1381,7 +1275,7 @@
     valve_pos_num = (int16_t) VALVE_POS_NUM;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_NUM;
     temp_msg.data[1] = (uint8_t) valve_pos_num;
     temp_msg.data[2] = (uint8_t) (valve_pos_num >> 8);
@@ -1402,7 +1296,7 @@
     temp_ddv_center = VALVE_CENTER;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 7;
     temp_msg.data[0] = (uint8_t) CTX_VALVE_MAX_MIN_POS;
     temp_msg.data[1] = (uint8_t) temp_valve_max_pos;
     temp_msg.data[2] = (uint8_t) ((int) (temp_valve_max_pos) >> 8);
@@ -1423,7 +1317,7 @@
     send_ref_array = (int16_t) (ref_array[t_cnt_buffer]);
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 8;
+    temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_SEND_BUFFER;
     temp_msg.data[1] = (uint8_t) send_pos_array;
     temp_msg.data[2] = (uint8_t) (send_pos_array >> 8);
@@ -1443,7 +1337,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_POSITION;
-    temp_msg.len = 8;
+    temp_msg.len = 6;
     temp_msg.data[0] = (uint8_t) t_pos;
     temp_msg.data[1] = (uint8_t) (t_pos >> 8);
     temp_msg.data[2] = (uint8_t) t_vel;
@@ -1474,14 +1368,14 @@
 }
 
 
-void CAN_TX_TORQUE(int16_t t_valve_pos)
+void CAN_TX_TORQUE(int16_t t_torque)
 {
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_TORQUE;
-    temp_msg.len = 8;
-    temp_msg.data[0] = (uint8_t) t_valve_pos;
-    temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
+    temp_msg.len = 2;
+    temp_msg.data[0] = (uint8_t) t_torque;
+    temp_msg.data[1] = (uint8_t) (t_torque >> 8);
 
     can.write(temp_msg);
 }
@@ -1491,7 +1385,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_PRES;
-    temp_msg.len = 8;
+    temp_msg.len = 4;
     temp_msg.data[0] = (uint8_t) t_pres_a;
     temp_msg.data[1] = (uint8_t) (t_pres_a >> 8);
     temp_msg.data[2] = (uint8_t) t_pres_b;
@@ -1505,7 +1399,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_VOUT;
-    temp_msg.len = 8;
+    temp_msg.len = 2;
     temp_msg.data[0] = (uint8_t) t_pwm;
     temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
 
--- a/CAN/function_CAN.h	Thu Feb 04 05:43:47 2021 +0000
+++ b/CAN/function_CAN.h	Wed Mar 03 10:56:25 2021 +0000
@@ -190,7 +190,6 @@
 void CAN_TX_HOMEPOS_OFFSET(void);
 void CAN_TX_HOMPOS_VALVE_OPENING(void);
 void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex);
-void CAN_TX_DDV_VALVE_DEADZONE(void);
 void CAN_TX_VALVE_POS_VS_FLOWRATE(int8_t canindex);
 void CAN_TX_VALVE_POS_NUM(void);
 void CAN_TX_DDV_VALVE_MAX_MIN_POS(void);
--- a/SPI_EEP_ENC/SPI_EEP_ENC.cpp	Thu Feb 04 05:43:47 2021 +0000
+++ b/SPI_EEP_ENC/SPI_EEP_ENC.cpp	Wed Mar 03 10:56:25 2021 +0000
@@ -68,7 +68,7 @@
     enc_cs = 1;
 
     // write MDR1 -> 0b10 -> 2-byte counter mode
-    for(i=0;i<100;i++);
+    for(i=0;i<10000;i++);
     enc_cs = 0;
     temp = enc.write(0b10010000);     // WR + MDR1
     //temp = enc.write(0b00000010);     // 2 byte mode
--- a/function_utilities/function_utilities.cpp	Thu Feb 04 05:43:47 2021 +0000
+++ b/function_utilities/function_utilities.cpp	Wed Mar 03 10:56:25 2021 +0000
@@ -54,8 +54,8 @@
 //float I_GAIN_JOINT_TORQUE_OPP = 0.0;
 //float D_GAIN_JOINT_TORQUE_OPP = 0.0;
 
-float VALVE_DEADZONE_PLUS;
-float VALVE_DEADZONE_MINUS;
+int16_t VALVE_DEADZONE_PLUS;
+int16_t VALVE_DEADZONE_MINUS;
 
 int16_t VELOCITY_COMP_GAIN;
 int16_t COMPLIANCE_GAIN;
@@ -101,7 +101,7 @@
 float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
 float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
 
-int HOMEPOS_OFFSET;
+int16_t HOMEPOS_OFFSET;
 int HOMEPOS_VALVE_OPENING;
 
 float FRICTION;
@@ -124,7 +124,7 @@
 int REF_MOVE_TIME_5k;
 int INIT_REF_PWM;
 int INIT_REF_VALVE_POS;
-int INIT_REF_POS;
+int32_t INIT_REF_POS;
 int INIT_REF_VEL;
 int INIT_REF_TORQUE;
 int INIT_REF_PRES_DIFF;
@@ -301,6 +301,7 @@
 // Current Control Variables
 double I_REF = 0.0f;
 double I_REF_fil = 0.0f;
+double I_REF_fil_DZ = 0.0f;
 double I_ERR = 0.0f;
 double I_ERR_INT = 0.0f;
 double I_REF_fil_old = 0.0f;
@@ -399,7 +400,7 @@
 void ROM_CALL_DATA(void)
 {
     BNO = spi_eeprom_read(RID_BNO);
-    BNO = 2;
+    BNO = 11;
     OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
     SENSING_MODE = spi_eeprom_read(RID_SENSING_MODE);
     CURRENT_CONTROL_MODE = spi_eeprom_read(RID_CURRENT_CONTROL_MODE);
@@ -419,8 +420,8 @@
     P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
     I_GAIN_JOINT_TORQUE = spi_eeprom_read( RID_I_GAIN_JOINT_TORQUE);
     D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
-    VALVE_DEADZONE_PLUS = (float) (spi_eeprom_read( RID_VALVE_DEADZONE_PLUS)) * 0.1f;
-    VALVE_DEADZONE_MINUS = (float) (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS)) * 0.1f;
+    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);
     VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
--- a/main.cpp	Thu Feb 04 05:43:47 2021 +0000
+++ b/main.cpp	Wed Mar 03 10:56:25 2021 +0000
@@ -90,6 +90,8 @@
 extern int CID_TX_VALVE_POSITION;
 extern int CID_TX_SOMETHING;
 
+float pres_A_new = 0.0f;
+float pres_B_new = 0.0f;
 
 /*******************************************************************************
  *  REFERENCE MODE
@@ -196,27 +198,39 @@
     /*********************************
     ***     Initialization
     *********************************/
-    
+
     HAL_Init();
     SystemClock_Config();
-    
+
     LED = 0;
     pc.baud(9600);
 
     // i2c init
-    i2c.frequency(400 * 1000);          // 0.4 mHz
-    wait_ms(2);                         // Power Up wait
-    look_for_hardware_i2c();            // Hardware present
-    init_as5510(i2c_slave_addr1);
-    make_delay();
+//    i2c.frequency(400 * 1000);          // 0.4 mHz
+//    wait_ms(2);                         // Power Up wait
+//    look_for_hardware_i2c();            // Hardware present
+//    init_as5510(i2c_slave_addr1);
+//    make_delay();
 
     // spi init
+    enc_cs = 1;
     eeprom.format(8,3);
     eeprom.frequency(5000000); //5M
     enc.format(8,0);
     enc.frequency(5000000); //5M
+    enc_cs = 0;
     make_delay();
 
+
+    // spi _ enc
+    spi_enc_set_init();
+    make_delay();
+
+    ////// bno rom
+//    spi_eeprom_write(RID_BNO, (int16_t)1);
+//    make_delay();
+    ////////
+
     // rom
     ROM_CALL_DATA();
     make_delay();
@@ -230,34 +244,41 @@
     TIM4->CR1 ^= TIM_CR1_UDIS;
     make_delay();
 
-    // TMR3 init
-    Init_TMR3();
-    TIM3->CR1 ^= TIM_CR1_UDIS;
-    make_delay();
-
     // CAN
     can.attach(&CAN_RX_HANDLER);
     CAN_ID_INIT();
     make_delay();
 
+    //can.reset();
+    can.filter(msg.id, 0xFFFFF000, CANStandard);
+
+    // TMR3 init
+    Init_TMR3();
+    TIM3->CR1 ^= TIM_CR1_UDIS;
+    make_delay();
+
+
+
     //Timer priority
     NVIC_SetPriority(TIM3_IRQn, 2);
     NVIC_SetPriority(TIM4_IRQn, 3);
 
-    //can.reset();
-    can.filter(msg.id, 0xFFFFF000, CANStandard);
+
 
-    // spi _ enc
-    spi_enc_set_init();
-    make_delay();
+
 
     //DAC init
     if (SENSING_MODE == 0) {
         dac_1 = TORQUE_VREF / 3.3f;
         dac_2 = 0.0f;
     } else if (SENSING_MODE == 1) {
-        dac_1 = PRES_A_VREF / 3.3f;
-        dac_2 = PRES_B_VREF / 3.3f;
+        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;
+        }
     }
     make_delay();
 
@@ -281,7 +302,7 @@
 //        timer_while ++;
 
         //i2c for SW valve
-        if(OPERATING_MODE == 5){
+        if(OPERATING_MODE == 5) {
             read_field(i2c_slave_addr1);
             if(DIR_VALVE_ENC < 0) value = 1023 - value;
         }
@@ -408,7 +429,6 @@
 extern "C" void TIM4_IRQHandler(void)
 {
     if (TIM4->SR & TIM_SR_UIF ) {
-
         /*******************************************************
         ***     Sensor Read & Data Handling
         ********************************************************/
@@ -426,10 +446,16 @@
             pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new;
             torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
 
-        // Pressure Sensing (0~210)bar =============================================
+            // Pressure Sensing (0~210)bar =============================================
         } else if (SENSING_MODE == 1) {
-            float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
-            float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
+
+            if (DIR_VALVE_ENC > 0) {
+                pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
+                pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
+            } else {
+                pres_A_new = (((float)ADC2->DR) - PRES_A_NULL);
+                pres_B_new = (((float)ADC1->DR) - PRES_B_NULL);
+            }
             double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 200Hz
             pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new;
             pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new;
@@ -466,6 +492,7 @@
 {
     if (TIM3->SR & TIM_SR_UIF ) {
 
+
         if (((OPERATING_MODE&0b110)>>1) == 0) {
             K_v = 0.4f; // Moog (LPM >> mA) , 100bar
             mV_PER_mA = 500.0f; // 5000mV/10mA
@@ -548,83 +575,6 @@
                 break;
             }
 
-//            case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
-//                if (TMR3_COUNT_DEADZONE == 0) {
-//                    if (pos_plus_end == pos_minus_end) need_enc_init = true;
-//                    else temp_time = 0;
-//                }
-//                if (need_enc_init) {
-//                    if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
-//                        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 * 1000.0f;
-//                        pos_minus_end = pos.sen;
-//                    } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
-//                    temp_time = TMR_FREQ_5k;
-//                }
-//
-//                if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
-//                    V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
-//                    VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
-//
-//                } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) {
-//                    V_out = 0;
-//                    CUR_VELOCITY_sum += CUR_VELOCITY;
-//                } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) {
-//                    if (CUR_VELOCITY_sum == 0) DZ_dir = 1;
-//                    else if (CUR_VELOCITY_sum > 0) DZ_dir = 1;
-//                    else if (CUR_VELOCITY_sum < 0) DZ_dir = -1;
-//                    else DZ_temp_cnt2 = DZ_end;
-//                    CUR_VELOCITY_sum = 0;
-//                } else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
-//                    if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end;
-//
-//                    // Position of Dead Zone
-//                    //  (CUR_VELOCITY < 0)  (CUR_VELOCITY == 0)  (CUR_VELOCITY > 0)
-//                    //     |        /                 |    /                      |/
-//                    //     | ______/               ___|___/                ______/|
-//                    //     |/                     /   |                   /       |
-//                    //    /|                     /    |                  /        |
-//                    //     0V                         0V                          0V
-//
-//                    if (DZ_temp_cnt2 < DZ_end) {
-//                        if (TMR3_COUNT_DEADZONE % 20 != 0) {
-//                            CUR_VELOCITY_sum += CUR_VELOCITY;
-//                        } else {
-//                            V_out -= DZ_dir;
-//                            if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++;
-//                            CUR_VELOCITY_sum = 0;
-//                        }
-//                        if (DZ_temp_cnt == 5) {
-//                            if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out;
-//                            else VALVE_DEADZONE_PLUS = (int16_t) V_out;
-//                            DZ_dir = -DZ_dir;
-//                            DZ_temp_cnt = 0;
-//                            DZ_temp_cnt2++;
-//                        }
-//                    } else {
-//                        TMR3_COUNT_DEADZONE = -1;
-//                        VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2;
-//                        if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) {
-//                            VALVE_DEADZONE_PLUS = VALVE_CENTER;
-//                            VALVE_DEADZONE_MINUS = VALVE_CENTER;
-//                        }
-//                        V_out = 0;
-//
-//                      
-//
-//                        //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
-//                        //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
-//
-//                        CONTROL_MODE = MODE_NO_ACT;
-//                        DZ_temp_cnt2 = 0;
-//                    }
-//                }
-//                TMR3_COUNT_DEADZONE++;
-//                break;
-//            }
-
             case MODE_FIND_HOME: {
                 if (FINDHOME_STAGE == FINDHOME_INIT) {
                     cnt_findhome = 0;
@@ -648,15 +598,19 @@
                     }
 
                     if ((cnt_vel_findhome < 3*TMR_FREQ_5k) &&  cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
-                        if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f;
-                        else pos.ref = pos.ref - 12.0f;
+                        if (HOMEPOS_OFFSET > 0) {
+                            pos.ref = pos.ref + 12.0f;
+                        } else {
+                            pos.ref = pos.ref - 12.0f;
+                        }
+
 
                         CONTROL_MODE = MODE_JOINT_CONTROL;
                         alpha_trans = 0.0f;
 
                     } else {
-                        ENC_SET(HOMEPOS_OFFSET);
-                        INIT_REF_POS = HOMEPOS_OFFSET;
+                        ENC_SET((int32_t)((int32_t)HOMEPOS_OFFSET*10));
+                        INIT_REF_POS = (int32_t)((int32_t)HOMEPOS_OFFSET*10);
                         REF_POSITION = 0;
                         REF_VELOCITY = 0;
                         FINDHOME_POSITION = 0;
@@ -683,6 +637,7 @@
                     pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
                     vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
                     pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
+                    CONTROL_MODE = MODE_JOINT_CONTROL;
 
                     if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
 
@@ -785,7 +740,7 @@
 //                    }
 //                    if (fl_temp_cnt2 == 100) {
 //
-//                       
+//
 //
 //                        //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
 //                        cur_vel_sum = 0;
@@ -829,8 +784,13 @@
                         if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
                         if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
 
-                        dac_1 = PRES_A_VREF / 3.3f;
-                        dac_2 = PRES_B_VREF / 3.3f;
+                        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 {
                     CONTROL_UTILITY_MODE = MODE_NO_ACT;
@@ -843,8 +803,13 @@
                     spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0f));
                     spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0f));
 
-                    dac_1 = PRES_A_VREF / 3.3f;
-                    dac_2 = PRES_B_VREF / 3.3f;
+                    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;
+                    }
                 }
                 TMR3_COUNT_PRES_NULL++;
                 break;
@@ -874,7 +839,7 @@
 //                    CUR_PRES_A_mean = 0;
 //                    CUR_PRES_B_mean = 0;
 //
-//                   
+//
 //
 //                    //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
 //                    //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
@@ -1156,7 +1121,7 @@
                                 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_MAX_POS, (int16_t) VALVE_MAX_POS);
                                 spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
@@ -1359,7 +1324,7 @@
                 } else if (CURRENT_CONTROL_MODE == 0) { //PWM
                     V_out = valve_pos.ref;
                 } else {
-                    I_REF = valve_pos.ref * 0.001f;
+                    I_REF = (double)(valve_pos.ref * (float)mA_PER_pulse);
                 }
                 break;
             }
@@ -1573,7 +1538,14 @@
                 double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
                 I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
 
-                I_ERR = I_REF_fil - cur.sen;
+                if (FLAG_VALVE_DEADZONE) {
+                    if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA
+                    else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA
+                    else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA
+                } else {
+                    I_REF_fil_DZ = I_REF_fil;
+                }
+                I_ERR = I_REF_fil_DZ - (double)cur.sen;
                 I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
 
 
@@ -1596,10 +1568,10 @@
                 double FF_gain = 1.0f;
 
                 VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT;
-                I_REF_fil_diff = I_REF_fil - I_REF_fil_old;
-                I_REF_fil_old = I_REF_fil;
+                I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old;
+                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); // 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 Ka = 3.0f / KP_I;
@@ -1623,19 +1595,9 @@
             ////////////////////////////////////////////////////////////////////////////
             /////////////////  Dead Zone Cancellation & Linearization //////////////////
             ////////////////////////////////////////////////////////////////////////////
-            // Dead Zone Cancellation (Mechanical Valve dead-zone)
-            if (FLAG_VALVE_DEADZONE) {
-                if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
-                else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
-
-                VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
-
-            } else {
-                VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
-            }
 
             // Output Voltage Linearization
-            double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV
+            double CUR_PWM_nonlin = (double)VALVE_PWM_RAW; // Unit : mV
             double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin);  // -8000~8000
 
             // Dead Zone Cancellation (Electrical dead-zone)
@@ -1688,65 +1650,64 @@
         TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
         TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
 
-       
-        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
 
-                // Position, Velocity, and Torque (ID:1200)
-                if (flag_data_request[0] == HIGH) {
-                    if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
-                        if (SENSING_MODE == 0) {
-                            CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
-                        } else if (SENSING_MODE == 1) {
-                            CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
-                        }
-                    } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
-                        if (SENSING_MODE == 0) {
-                            CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
-                        } else if (SENSING_MODE == 1) {
-                            CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
-                        }
-                    }        
-                
-                 }
+        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+            // Position, Velocity, and Torque (ID:1200)
+            if (flag_data_request[0] == HIGH) {
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+                    if (SENSING_MODE == 0) {
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
+                    } else if (SENSING_MODE == 1) {
+                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
+                    }
+                } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+                    if (SENSING_MODE == 0) {
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
+                    } else if (SENSING_MODE == 1) {
+                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
+                    }
+                }
 
-            
+            }
+
+
             // ID:1300
             if (flag_data_request[1] == HIGH) {
                 CAN_TX_TORQUE((int16_t) 7); //1300
             }
 
-        // Reference Current, Current Current (ID:1700)
+            // Reference Current, Current Current (ID:1700)
             if (flag_data_request[2] == HIGH) {
-                
-                CAN_TX_SOMETHING((int16_t) (I_REF_fil / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (0), (int16_t) (0));
+
+                CAN_TX_SOMETHING((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (HOMEPOS_OFFSET), (int16_t) (0));
                 //double t_value = 0.0f;
-    //            if(value>=(float) VALVE_CENTER) {
-    //                t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-    //            } else {
-    //                t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-    //            }
-    //            double t_value_ref = 0.0f;
-    //            if(valve_pos.ref>=(float) VALVE_CENTER) {
-    //                t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-    //            } else {
-    //                t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-    //            }
-    //    
-    //    
-    //            CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
+                //            if(value>=(float) VALVE_CENTER) {
+                //                t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+                //            } else {
+                //                t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+                //            }
+                //            double t_value_ref = 0.0f;
+                //            if(valve_pos.ref>=(float) VALVE_CENTER) {
+                //                t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+                //            } else {
+                //                t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+                //            }
+                //
+                //
+                //            CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
             }
 
             //If it doesn't rest, below can can not work.
             for (can_rest = 0; can_rest < 10000; can_rest++) {
                 ;
             }
-            
+
             // ID:1500
             if (flag_data_request[3] == HIGH) {
                 //PWM
                 CAN_TX_PWM((int16_t) (torq.ref)); //1500
             }
-            
+
             // ID:1600
             if (flag_data_request[4] == HIGH) {
                 //valve position
--- a/setting.h	Thu Feb 04 05:43:47 2021 +0000
+++ b/setting.h	Wed Mar 03 10:56:25 2021 +0000
@@ -148,8 +148,8 @@
 //extern float I_GAIN_JOINT_TORQUE_OPP;
 //extern float D_GAIN_JOINT_TORQUE_OPP;
 
-extern float VALVE_DEADZONE_PLUS;
-extern float VALVE_DEADZONE_MINUS;
+extern int16_t VALVE_DEADZONE_PLUS;
+extern int16_t VALVE_DEADZONE_MINUS;
 
 extern int16_t VELOCITY_COMP_GAIN;
 extern int16_t COMPLIANCE_GAIN;
@@ -196,7 +196,7 @@
 extern float PRES_SENSOR_A_PULSE_PER_BAR;
 extern float PRES_SENSOR_B_PULSE_PER_BAR;
 
-extern int HOMEPOS_OFFSET;
+extern int16_t HOMEPOS_OFFSET;
 extern int HOMEPOS_VALVE_OPENING;
 
 extern float FRICTION;
@@ -219,7 +219,7 @@
 extern int REF_MOVE_TIME_5k;
 extern int INIT_REF_PWM;
 extern int INIT_REF_VALVE_POS;
-extern int INIT_REF_POS;
+extern int32_t INIT_REF_POS;
 extern int INIT_REF_VEL;
 extern int INIT_REF_TORQUE;
 extern int INIT_REF_PRES_DIFF;
@@ -276,8 +276,6 @@
 extern float VALVE_CENTER_OFFSET;
 extern float VALVE_DZ_MINUS_OFFSET;
 extern float VALVE_DZ_PLUS_OFFSET;
-extern float VALVE_DEADZONE_MINUS;
-extern float VALVE_DEADZONE_PLUS;
 
 extern int TMR3_COUNT_FINDHOME;
 extern int TMR3_COUNT_FLOWRATE;
@@ -392,6 +390,7 @@
 // Current Control Variables
 extern double I_REF;
 extern double I_REF_fil;
+extern double I_REF_fil_DZ;
 extern double I_ERR;
 extern double I_ERR_INT;
 extern double I_REF_fil_old;