Diff: CAN/function_CAN.cpp

Revision:

68:328e1be06f5d

Parent:

67:c2812cf26c38

Child:

69:3995ffeaa786

--- a/CAN/function_CAN.cpp	Tue May 12 07:43:44 2020 +0000
+++ b/CAN/function_CAN.cpp	Tue May 12 11:25:00 2020 +0000
@@ -23,6 +23,11 @@
 
 extern DigitalOut LED;
 
+extern float x_past[];
+extern float f_past[];
+extern float f_future[];
+extern float input[];
+
 /*******************************************************************************
  * CAN functions
  ******************************************************************************/
@@ -690,6 +695,11 @@
             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;
@@ -720,7 +730,46 @@
             vel.ref = (double)temp_vel * 256.0f;
         }
         
-        torq.ref = (double)temp_torq * 0.1f / (float)(TORQUE_SENSOR_PULSE_PER_TORQUE);
+        torq.ref = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;   //N
+        
+        
+        ///////////////Make Data///////////////////
+        for(int i=0; i<num_array_x_past-1;i++){
+            x_past[i] = x_past[i+1];
+        }
+        x_past[num_array_x_past-1] = pos.sen / ENC_PULSE_PER_POSITION;   //mm
+        
+        for(int i=0; i<num_array_f_past-1;i++){
+            f_past[i] = f_past[i+1];
+        }
+        f_past[num_array_f_past-1] = torq.sen;  //N
+        
+        f_future[0] = torq.sen;  //N
+        for(int i=1;i<num_array_f_future-1;i++){
+            f_future[i] = f_future[i+1];
+        }
+        f_future[num_array_f_future-1] = torq.ref;  //N
+
+        int ind = 0;
+        for(int i=0;i<numpast_x;i++){
+            input[ind] = x_past[2*i] / 60.0f;
+            ind = ind + 1;
+        }
+        input[ind] = pos.sen / ENC_PULSE_PER_POSITION / 60.0f;
+        ind = ind + 1;
+        for(int i=0;i<numpast_f;i++){
+            input[ind] = f_past[2*i] / 10000.0f + 0.5f;
+            ind = ind + 1;
+        }
+        input[ind] = torq.sen / 10000.0f + 0.5f;
+        ind = ind + 1;
+        for(int i=0;i<numfuture_f;i++){
+            input[ind] = (f_future[2*i+2] - f_future[0])/10000.0f+0.5f;
+            ind = ind + 1;
+        }
+
+
+
         
     } else if(address==CID_RX_REF_VALVE_POS) {
         int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);