Diff: CAN/function_CAN.cpp

Revision:

230:2c3e5ecbe7e1

Parent:

227:699c3e572283

--- a/CAN/function_CAN.cpp	Tue Apr 20 00:42:45 2021 +0000
+++ b/CAN/function_CAN.cpp	Wed Apr 21 04:20:39 2021 +0000
@@ -14,10 +14,11 @@
 int CID_TX_INFO = 1100;
 int CID_TX_POS_VEL_TORQ = 1200;
 int CID_TX_PWM = 1300;
+int CID_TX_SOMETHING = 1400;
+
 int CID_TX_CURRENT = 1400;
 int CID_TX_VOUT = 1500;
 int CID_TX_VALVE_POSITION = 1600;
-int CID_TX_SOMETHING = 1700;
 
 // variables
 uint8_t can_index = 0;
@@ -32,6 +33,31 @@
 extern float input_NN[];
 
 /*******************************************************************************
+ * State Class functions
+ ******************************************************************************/
+void State::UpdateSen(float sen_new, float Freq_update, float f_cut) {
+    if(f_cut<=0.0f) f_cut=0.001f;
+    
+    this->sen_diff = (sen_new-this->sen)*Freq_update;
+    float alpha_update = 1.0f / (1.0f + Freq_update / (2.0f * 3.14f * f_cut)); // f_cutoff : 100Hz
+    this->sen = (1.0f - alpha_update) * this->sen + alpha_update * sen_new;
+}
+
+void State::UpdateRef(float ref_new) {
+    this->ref = ref_new;
+}
+
+void State::Reset() {
+    this->sen = 0.0f;
+    this->sen_diff = 0.0f;
+    this->ref = 0.0f;
+    this->err = 0.0f;
+    this->err_int = 0.0f;
+    this->err_old = 0.0f;
+    this->err_diff = 0.0f;
+}
+
+/*******************************************************************************
  * CAN functions
  ******************************************************************************/
 void CAN_ID_INIT(void)
@@ -232,6 +258,18 @@
             CONTROL_UTILITY_MODE = 22;
             break;
         }
+        
+        case CRX_ASK_VARIABLE_SUPPLY:
+        {
+            CAN_TX_VARIABLE_SUPPLY_ONOFF();
+            break;
+        }                
+        
+        case CRX_SET_VARIABLE_SUPPLY:
+        {
+            SUPPLY_PRESSURE_UPDATE = msg.data[1];
+            break;
+        }
 
         case CRX_ASK_PID_GAIN: {
             CAN_TX_PID_GAIN(msg.data[1]);
@@ -392,22 +430,16 @@
             break;
         }
 
-        case CRX_ASK_PRES: {
-            CAN_TX_PRES_A_AND_B();
-//            SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
-            //dac_1 = PRES_A_VREF;
-            //dac_2 = PRES_B_VREF;
-
+        case CRX_ASK_SUP_PRES: {
+            CAN_TX_SUP_PRES();
             break;
         }
 
-        case CRX_SET_PRES: {
-            PRES_SUPPLY = (int16_t) (msg.data[1] | msg.data[2] << 8);
-            PRES_RETURN = (int16_t) (msg.data[3] | msg.data[4] << 8);
-            spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
-            spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
-
-
+        case CRX_SET_SUP_PRES: {
+            int16_t temp = (int16_t) (msg.data[1] | msg.data[2] << 8);
+            spi_eeprom_write(RID_PRES_SUPPLY, temp);
+            PRES_SUPPLY_NOM = (float)temp;
+            PRES_SUPPLY = PRES_SUPPLY_NOM;
             break;
         }
 
@@ -634,65 +666,58 @@
 
     } else if(address==CID_RX_REF_POSITION) {
 
-        int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); 
         int16_t temp_vel = (int16_t) (msg.data[2] | msg.data[3] << 8);
         int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
 
         if((OPERATING_MODE&0b001)==0) { // Rotary Actuator
-            REF_POSITION = (double)temp_pos * 10.0f;
-            REF_VELOCITY = (double)temp_vel * 256.0f;
+            REF_POSITION = (float)temp_pos * 1.0f / ENC_PULSE_PER_POSITION; // pulse >> deg 
+            REF_VELOCITY = (float)temp_vel * 10.0f / ENC_PULSE_PER_POSITION; // pulse/s >> deg/s 
+            REF_TORQUE = (float)temp_torq * 0.01f / TORQUE_SENSOR_PULSE_PER_TORQUE;  // pulse >> Nm
         } else { //Linear Actuator
-            REF_POSITION = (double)temp_pos * 10.0f;
-            REF_VELOCITY = (double)temp_vel * 256.0f;
+            REF_POSITION = (float)temp_pos * 4.0f / ENC_PULSE_PER_POSITION; // pulse >> mm
+            REF_VELOCITY = (float)temp_vel * 100.0f / ENC_PULSE_PER_POSITION; // pulse/s >> mm/s 
+            REF_FORCE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;  // pulse >> N
         }
 
-        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(SUPPLY_PRESSURE_UPDATE == 1) {
+            int16_t temp_REF_Ps = (int16_t) (msg.data[6] | msg.data[7] << 8);
+            PRES_SUPPLY = ((float)temp_REF_Ps) / 100.0; 
+            if(PRES_SUPPLY<35.0f) PRES_SUPPLY = 35.0f;
+            else if(PRES_SUPPLY>210.0f) PRES_SUPPLY = 210.0f;
+        } else {
+            PRES_SUPPLY = PRES_SUPPLY_NOM;
+        }
 
         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));
-                    }
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.1f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.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));
-                    }
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION*0.25f), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.01f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
                 }
             }
 
-            // PWM, ID:1300
+            // Valve Position (ID:1300)
             if (flag_data_request[1] == HIGH) {
-                CAN_TX_PWM((int16_t) (V_out)); //1300
+                CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); 
             }
 
-            // Reference Current, Current Current (ID:1400)
+            // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
             if (flag_data_request[2] == HIGH) {
-                CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400
+                CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (0), (int16_t) (0)); 
             }
-            
-            // Reference Current, Current Current (ID:1700)
-            if (flag_data_request[3] == HIGH) {
-                CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700
-            }
-
         }
 
-
     } 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
-            valve_pos.ref = (double) temp_ref_valve_pos;
+            valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~10000)
         } else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve
-            valve_pos.ref = (double) temp_ref_valve_pos;
+            valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~30000)
         } else { //SW Valve
             if(temp_ref_valve_pos >= 0) {
                 valve_pos.ref = (double)VALVE_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f;
@@ -851,6 +876,16 @@
     can.write(temp_msg);
 }
 
+void CAN_TX_VARIABLE_SUPPLY_ONOFF(void)
+{
+    CANMessage temp_msg;
+    temp_msg.id = CID_TX_INFO;
+    temp_msg.len = 2;
+    temp_msg.data[0] = (uint8_t) CTX_SEND_VARIABLE_SUPPLY;
+    temp_msg.data[1] = (uint8_t) SUPPLY_PRESSURE_UPDATE;
+
+    can.write(temp_msg);
+}
 
 void CAN_TX_PID_GAIN(int t_type)
 {
@@ -992,17 +1027,18 @@
     can.write(temp_msg);
 }
 
-void CAN_TX_PRES_A_AND_B(void)
+void CAN_TX_SUP_PRES(void)
 {
     CANMessage temp_msg;
 
+    int16_t temp_PRES_SUPPLY = (int16_t) (PRES_SUPPLY);
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 5;
-    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);
-    temp_msg.data[3] = (uint8_t) PRES_RETURN;
-    temp_msg.data[4] = (uint8_t) (PRES_RETURN >> 8);
+    temp_msg.data[0] = (uint8_t) CTX_SEND_SUP_PRES;
+    temp_msg.data[1] = (uint8_t) temp_PRES_SUPPLY;
+    temp_msg.data[2] = (uint8_t) (temp_PRES_SUPPLY >> 8);
+    temp_msg.data[3] = 0;
+    temp_msg.data[4] = 0;
 
     can.write(temp_msg);
 }