Diff: main.cpp

Revision:

225:9c0becc196ba

Parent:

224:985dba42f261

Child:

226:82a3ca333004

--- 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.