Diff: CAN/function_CAN.cpp

Revision:

215:699c3e572283

Parent:

214:82a3ca333004

Child:

218:2c3e5ecbe7e1

diff -r 82a3ca333004 -r 699c3e572283 CAN/function_CAN.cpp
--- a/CAN/function_CAN.cpp	Wed Mar 03 10:56:25 2021 +0000
+++ b/CAN/function_CAN.cpp	Thu Mar 11 07:27:23 2021 +0000
@@ -8,13 +8,13 @@
 // CAN ID Setting Variables
 int CID_RX_CMD = 100;
 int CID_RX_REF_POSITION = 200;
-int CID_RX_REF_VALVE_POS = 300;
+int CID_RX_REF_OPENLOOP = 300;
 int CID_RX_REF_PWM = 400;
 
 int CID_TX_INFO = 1100;
-int CID_TX_POSITION = 1200;
-int CID_TX_TORQUE = 1300;
-int CID_TX_PRES = 1400;
+int CID_TX_POS_VEL_TORQ = 1200;
+int CID_TX_PWM = 1300;
+int CID_TX_CURRENT = 1400;
 int CID_TX_VOUT = 1500;
 int CID_TX_VALVE_POSITION = 1600;
 int CID_TX_SOMETHING = 1700;
@@ -39,13 +39,13 @@
 
     CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD);
     CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION);
-    CID_RX_REF_VALVE_POS = (int) (BNO + INIT_CID_RX_REF_VALVE_POS);
+    CID_RX_REF_OPENLOOP = (int) (BNO + INIT_CID_RX_REF_OPENLOOP);
     CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM);
 
     CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO);
-    CID_TX_POSITION = (int) (BNO + INIT_CID_TX_POSITION);
-    CID_TX_TORQUE = (int) (BNO + INIT_CID_TX_TORQUE);
-    CID_TX_PRES = (int) (BNO + INIT_CID_TX_PRES);
+    CID_TX_POS_VEL_TORQ = (int) (BNO + INIT_CID_TX_POS_VEL_TORQ);
+    CID_TX_PWM = (int) (BNO + INIT_CID_TX_PWM);
+    CID_TX_CURRENT = (int) (BNO + INIT_CID_TX_CURRENT);
     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);
@@ -140,6 +140,7 @@
 //            if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
             //if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3;
 
+            
             break;
         }
 
@@ -638,84 +639,54 @@
         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 * 10.0f;
-            vel.ref = (double)temp_vel * 256.0f;
+            REF_POSITION = (double)temp_pos * 10.0f;
+            REF_VELOCITY = (double)temp_vel * 256.0f;
         } else { //Linear Actuator
-            pos.ref = (double)temp_pos * 10.0f;
-            vel.ref = (double)temp_vel * 256.0f;
+            REF_POSITION = (double)temp_pos * 10.0f;
+            REF_VELOCITY = (double)temp_vel * 256.0f;
         }
 
-        torq.ref = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;   //N
-        torq.ref_diff = torq.ref - torq.ref_old;
-        torq_dot.sen = torq.sen-torq_sen_past;
-        torq.ref_old = torq.ref;
-        torq_sen_past = torq.sen;
+        REF_TORQUE = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;   //N
+        torq.ref_diff = REF_TORQUE - REF_TORQUE_OLD;
+        REF_TORQUE_OLD = REF_TORQUE;
 
         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));
+                        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/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*10.0f), (int16_t) ((pres_B.sen)*10.0f));
+                        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)*10.0f), (int16_t) ((pres_B.sen)*10.0f));
+                        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
+            // PWM, ID:1300
             if (flag_data_request[1] == HIGH) {
-                CAN_TX_TORQUE((int16_t) (pos.ref/10.0f)); //1300
+                CAN_TX_PWM((int16_t) (V_out)); //1300
             }
 
-            // Reference Current, Current Current (ID:1700)
+            // Reference Current, Current Current (ID:1400)
             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 {
-//                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_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400
             }
-
-            //If it doesn't rest, below can can not work.
-            for (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) {
-                ;
-            }
-
-            // ID:1500
+            
+            // Reference Current, Current Current (ID:1700)
             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
+                CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700
             }
 
         }
 
 
-    } else if(address==CID_RX_REF_VALVE_POS) {
+    } else if(address==CID_RX_REF_OPENLOOP) {
         int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
 
         if (((OPERATING_MODE&0b110)>>1) == 0) { //Moog Valve
@@ -1336,7 +1307,7 @@
 {
     CANMessage temp_msg;
 
-    temp_msg.id = CID_TX_POSITION;
+    temp_msg.id = CID_TX_POS_VEL_TORQ;
     temp_msg.len = 6;
     temp_msg.data[0] = (uint8_t) t_pos;
     temp_msg.data[1] = (uint8_t) (t_pos >> 8);
@@ -1353,7 +1324,7 @@
 
     CANMessage temp_msg;
 
-    temp_msg.id = CID_TX_POSITION;
+    temp_msg.id = CID_TX_POS_VEL_TORQ;
     temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) t_pos;
     temp_msg.data[1] = (uint8_t) (t_pos >> 8);
@@ -1367,24 +1338,11 @@
     can.write(temp_msg);
 }
 
-
-void CAN_TX_TORQUE(int16_t t_torque)
+void CAN_TX_CURRENT(int16_t t_pres_a, int16_t t_pres_b)
 {
     CANMessage temp_msg;
 
-    temp_msg.id = CID_TX_TORQUE;
-    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);
-}
-
-void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b)
-{
-    CANMessage temp_msg;
-
-    temp_msg.id = CID_TX_PRES;
+    temp_msg.id = CID_TX_CURRENT;
     temp_msg.len = 4;
     temp_msg.data[0] = (uint8_t) t_pres_a;
     temp_msg.data[1] = (uint8_t) (t_pres_a >> 8);
@@ -1398,7 +1356,7 @@
 {
     CANMessage temp_msg;
 
-    temp_msg.id = CID_TX_VOUT;
+    temp_msg.id = CID_TX_PWM;
     temp_msg.len = 2;
     temp_msg.data[0] = (uint8_t) t_pwm;
     temp_msg.data[1] = (uint8_t) (t_pwm >> 8);