Diff: CAN/function_CAN.cpp

Revision:

67:c2812cf26c38

Parent:

66:a8e6799dbce3

Child:

68:328e1be06f5d

--- a/CAN/function_CAN.cpp	Tue May 12 06:23:46 2020 +0000
+++ b/CAN/function_CAN.cpp	Tue May 12 07:43:44 2020 +0000
@@ -20,13 +20,9 @@
 
 // variables
 uint8_t can_index = 0;
+
 extern DigitalOut LED;
 
-//extern int num_input;
-//extern int num_input_array;
-extern float input[];
-extern float x_future[];
-
 /*******************************************************************************
  * CAN functions
  ******************************************************************************/
@@ -266,10 +262,10 @@
                 ROM_RESET_DATA();
                 
             } else if (msg.data[1] == 3) {
-                K_SPRING = (int16_t) (msg.data[2] | msg.data[3] << 8);
-                D_DAMPER = (int16_t) (msg.data[4] | msg.data[5] << 8);
+                K_SPRING = (float) (((float) ((int16_t) (msg.data[2] | msg.data[3] << 8))) * 0.1f);
+                D_DAMPER = (float) (((float) ((int16_t) (msg.data[4] | msg.data[5] << 8))) * 0.01f);
                 
-                ROM_RESET_DATA();
+//                ROM_RESET_DATA();     //For Real-time changing
             }
 
             break;
@@ -694,11 +690,6 @@
             flag_delay_test = 1;
             break;
         }
-        case CRX_SET_NN_CONTROL_FLAG: {
-            NN_Control_Flag = (int16_t) msg.data[1];
-            CONTROL_UTILITY_MODE = 1;
-            break;
-        }
 
     default:
         break;
@@ -725,22 +716,11 @@
             vel.ref = (double)temp_vel * 10.0f;
         }
         else { //Linear Actuator
-            pos.ref = (double)temp_pos * 4.0f;
-            vel.ref = (double)temp_vel * 100.0f;
+            pos.ref = (double)temp_pos * 10.0f;
+            vel.ref = (double)temp_vel * 256.0f;
         }
         
-        torq.ref = (double)temp_torq * 0.1f;
-        
-        x_future[0] = pos.sen/(float)(ENC_PULSE_PER_POSITION);  //mm
-        for(int i=1;i<num_input_array-1;i++){
-            x_future[i] = x_future[i+1];
-        }
-        x_future[num_input_array-1] = pos.ref/(float)(ENC_PULSE_PER_POSITION);  //mm
-        
-        for(int i=0;i<num_input;i++){
-            input[i] = (x_future[2*i+2] - x_future[0])*0.2f+0.5f;
-        }
-
+        torq.ref = (double)temp_torq * 0.1f / (float)(TORQUE_SENSOR_PULSE_PER_TORQUE);
         
     } else if(address==CID_RX_REF_VALVE_POS) {
         int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
@@ -914,8 +894,8 @@
         sendIgain = (int16_t) (I_GAIN_JOINT_TORQUE);
         sendDgain = (int16_t) (D_GAIN_JOINT_TORQUE);
     } else if (t_type == 3) {
-        sendPgain = (int16_t) (K_SPRING);
-        sendIgain = (int16_t) (D_DAMPER);
+        sendPgain = (int16_t) (K_SPRING * 10.0f);
+        sendIgain = (int16_t) (D_DAMPER * 100.0f);
     }
 
     CANMessage temp_msg;
@@ -1359,15 +1339,26 @@
     can.write(temp_msg);
 }
 
-void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout) {
+//void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout) {
+//    CANMessage temp_msg;
+//
+//    temp_msg.id = CID_TX_TORQUE;
+//    temp_msg.len = 4;
+//    temp_msg.data[0] = (uint8_t) t_valve_pos;
+//    temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
+//    temp_msg.data[2] = (uint8_t) t_vout;
+//    temp_msg.data[3] = (uint8_t) (t_vout >> 8);
+//
+//    can.write(temp_msg);
+//}
+
+void CAN_TX_TORQUE(int16_t t_valve_pos) {
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_TORQUE;
-    temp_msg.len = 4;
+    temp_msg.len = 2;
     temp_msg.data[0] = (uint8_t) t_valve_pos;
     temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
-    temp_msg.data[2] = (uint8_t) t_vout;
-    temp_msg.data[3] = (uint8_t) (t_vout >> 8);
 
     can.write(temp_msg);
 }
@@ -1411,14 +1402,4 @@
     
 
     can.write(temp_msg);
-}
-
-
-
-
-
-
-
-
-
-
+}
\ No newline at end of file