Revision 225:9c0becc196ba, committed 2021-02-04

Comitter:

jeongyeseong

Date:

Thu Feb 04 05:43:47 2021 +0000

Parent:

224:985dba42f261

Commit message:

20210204

Changed in this revision

--- a/CAN/function_CAN.cpp	Mon Dec 28 14:27:11 2020 +0000
+++ b/CAN/function_CAN.cpp	Thu Feb 04 05:43:47 2021 +0000
@@ -17,6 +17,7 @@
 int CID_TX_PRES = 1400;
 int CID_TX_VOUT = 1500;
 int CID_TX_VALVE_POSITION = 1600;
+int CID_TX_SOMETHING = 1700;
 
 // variables
 uint8_t can_index = 0;
@@ -47,6 +48,7 @@
     CID_TX_PRES = (int) (BNO + INIT_CID_TX_PRES);
     CID_TX_VOUT = (int) (BNO + INIT_CID_TX_VOUT);
     CID_TX_VALVE_POSITION = (int) (BNO + INIT_CID_TX_VALVE_POSITION);
+    CID_TX_SOMETHING = (int) (BNO + INIT_CID_TX_SOMETHING);
 }
 
 void ReadCMD(int16_t CMD)
@@ -731,6 +733,69 @@
         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));
+                }
+            }
+        }
+    
+        // 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;
+//            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 (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
+        }
+
+    }
 
 
     } else if(address==CID_RX_REF_VALVE_POS) {
@@ -751,6 +816,8 @@
         int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
         Vout.ref = (double) temp_ref_pwm;
     }
+
+
 }
 
 /******************************************************************************
@@ -762,7 +829,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 7;
+    temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) CTX_SEND_INFO;
     temp_msg.data[1] = (uint8_t) BNO;
     temp_msg.data[2] = (uint8_t) CAN_FREQ;
@@ -779,7 +846,8 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 2;
+    //temp_msg.len = 2;
+    temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) CTX_SEND_BNO;
     temp_msg.data[1] = (uint8_t) BNO;
 
@@ -791,7 +859,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 5;
+    temp_msg.len = 8;
     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;
@@ -806,7 +874,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -819,7 +887,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 2;
+    temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE;
     temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE;
 
@@ -831,7 +899,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -844,7 +912,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -857,7 +925,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -872,7 +940,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -887,7 +955,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -942,7 +1010,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 7;
+    temp_msg.len = 8;
     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);
@@ -959,7 +1027,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -972,7 +1040,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -985,7 +1053,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -998,7 +1066,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -1011,7 +1079,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 5;
+    temp_msg.len = 8;
     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);
@@ -1026,7 +1094,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 5;
+    temp_msg.len = 8;
     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);
@@ -1041,7 +1109,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 5;
+    temp_msg.len = 8;
     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);
@@ -1056,7 +1124,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 5;
+    temp_msg.len = 8;
     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);
@@ -1071,7 +1139,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) CTX_SEND_STROKE;
     temp_msg.data[1] = (uint8_t) STROKE;
     temp_msg.data[2] = (uint8_t) (STROKE >> 8);
@@ -1084,7 +1152,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 5;
+    temp_msg.len = 8;
     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);
@@ -1099,7 +1167,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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;
@@ -1113,7 +1181,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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;
@@ -1127,7 +1195,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 5;
+    temp_msg.len = 8;
     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);
@@ -1144,7 +1212,7 @@
     send_friction = (int16_t) (FRICTION * 10.0f);
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -1157,7 +1225,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 6;
+    temp_msg.len = 8;
     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);
@@ -1180,7 +1248,7 @@
     temp_ddv_center = (double)VALVE_CENTER;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 7;
+    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);
@@ -1197,7 +1265,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 6;
+    temp_msg.len = 8;
     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);
@@ -1213,7 +1281,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 6;
+    temp_msg.len = 8;
     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);
@@ -1231,7 +1299,7 @@
     send_homepos_offset = (int16_t) (HOMEPOS_OFFSET);
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -1246,7 +1314,7 @@
     send_homepos_valve_opening = (int16_t) (HOMEPOS_VALVE_OPENING);
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -1269,7 +1337,7 @@
     PWM_VALVE_ID = ID_index_array[canindex] * 1000;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 5;
+    temp_msg.len = 8;
     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);
@@ -1298,10 +1366,10 @@
     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);
-    temp_msg.data[5] = (uint8_t) valve_pos_vs_flowrate;
-    temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 8);
-    temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 16);
-    temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 24);
+    temp_msg.data[3] = (uint8_t) valve_pos_vs_flowrate;
+    temp_msg.data[4] = (uint8_t) (valve_pos_vs_flowrate >> 8);
+    temp_msg.data[5] = (uint8_t) (valve_pos_vs_flowrate >> 16);
+    temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 24);
 
     can.write(temp_msg);
 }
@@ -1313,7 +1381,7 @@
     valve_pos_num = (int16_t) VALVE_POS_NUM;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 3;
+    temp_msg.len = 8;
     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);
@@ -1334,7 +1402,7 @@
     temp_ddv_center = VALVE_CENTER;
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 7;
+    temp_msg.len = 8;
     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);
@@ -1355,7 +1423,7 @@
     send_ref_array = (int16_t) (ref_array[t_cnt_buffer]);
 
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 5;
+    temp_msg.len = 8;
     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);
@@ -1375,7 +1443,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_POSITION;
-    temp_msg.len = 6;
+    temp_msg.len = 8;
     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;
@@ -1411,7 +1479,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_TORQUE;
-    temp_msg.len = 2;
+    temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) t_valve_pos;
     temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
 
@@ -1423,7 +1491,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_PRES;
-    temp_msg.len = 4;
+    temp_msg.len = 8;
     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;
@@ -1437,7 +1505,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_VOUT;
-    temp_msg.len = 2;
+    temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) t_pwm;
     temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
 
@@ -1461,4 +1529,23 @@
 
 
     can.write(temp_msg);
+}
+
+void CAN_TX_SOMETHING(int16_t t_a, int16_t t_b, int16_t t_c, int16_t t_d)
+{
+    CANMessage temp_msg;
+
+    temp_msg.id = CID_TX_SOMETHING;
+    temp_msg.len = 8;
+    temp_msg.data[0] = (uint8_t) t_a;
+    temp_msg.data[1] = (uint8_t) (t_a >> 8);
+    temp_msg.data[2] = (uint8_t) t_b;
+    temp_msg.data[3] = (uint8_t) (t_b >> 8);
+    temp_msg.data[4] = (uint8_t) t_c;
+    temp_msg.data[5] = (uint8_t) (t_c >> 8);
+    temp_msg.data[6] = (uint8_t) t_d;
+    temp_msg.data[7] = (uint8_t) (t_d >> 8);
+
+
+    can.write(temp_msg);
 }
\ No newline at end of file

--- a/CAN/function_CAN.h	Mon Dec 28 14:27:11 2020 +0000
+++ b/CAN/function_CAN.h	Thu Feb 04 05:43:47 2021 +0000
@@ -18,6 +18,7 @@
 #define INIT_CID_TX_PRES              1400
 #define INIT_CID_TX_VOUT              1500
 #define INIT_CID_TX_VALVE_POSITION    1600
+#define INIT_CID_TX_SOMETHING         1700
 
 
 // CID_RX_CMD - RX CMD type
@@ -154,6 +155,7 @@
 void CAN_TX_PRES(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);
 
 // Information Transmission
 void CAN_TX_INFO(void);

--- a/function_utilities/function_utilities.cpp	Mon Dec 28 14:27:11 2020 +0000
+++ b/function_utilities/function_utilities.cpp	Thu Feb 04 05:43:47 2021 +0000
@@ -399,7 +399,7 @@
 void ROM_CALL_DATA(void)
 {
     BNO = spi_eeprom_read(RID_BNO);
-    BNO = 1;
+    BNO = 2;
     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);

--- a/main.cpp	Mon Dec 28 14:27:11 2020 +0000
+++ b/main.cpp	Thu Feb 04 05:43:47 2021 +0000
@@ -88,6 +88,7 @@
 extern int CID_TX_PRES;
 extern int CID_TX_VOUT;
 extern int CID_TX_VALVE_POSITION;
+extern int CID_TX_SOMETHING;
 
 
 /*******************************************************************************
@@ -1687,48 +1688,52 @@
         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));
-                    }
-                }
-            }
+                // 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
             }
 
-            // ID:1400
+        // Reference Current, Current Current (ID:1700)
             if (flag_data_request[2] == HIGH) {
-                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
+                
+                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;
+    //            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.