Diff: CAN/function_CAN.cpp

Revision:

54:647072f5307a

Parent:

52:8ea76864368a

Child:

55:b25725257569

--- a/CAN/function_CAN.cpp	Wed Feb 19 05:48:57 2020 +0000
+++ b/CAN/function_CAN.cpp	Tue Feb 25 12:56:39 2020 +0000
@@ -102,10 +102,7 @@
 
         case CRX_SET_CAN_FREQ: {
             CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8);
-            
             ROM_RESET_DATA();
-            
-            //spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
             break;
         }
 
@@ -116,7 +113,8 @@
         }
 
         case CRX_SET_CONTROL_MODE: {
-            CONTROL_MODE = (int16_t) (msg.data[1]);
+            //CONTROL_MODE = (int16_t) (msg.data[1]);
+            CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]);
             if (CONTROL_MODE == 22) {    //MODE_FIND_HOME
                 FLAG_FIND_HOME = true;
             }
@@ -230,8 +228,8 @@
         }
 
         case CRX_SET_HOMEPOS: {
-            CONTROL_MODE = 22;
-            SETTING_SWITCH = (int16_t) (msg.data[1]);
+            //CONTROL_MODE = 22;
+            CONTROL_UTILITY_MODE = 22;
             break;
         }
 
@@ -694,6 +692,10 @@
             CAN_TX_DDV_VALVE_MAX_MIN_POS();
             break;
         }
+        case CRX_DELAY_TEST:{
+            flag_delay_test = 1;
+            break;
+        }
 
     default:
         break;
@@ -708,22 +710,37 @@
     if(address==CID_RX_CMD){
         unsigned int CMD = msg.data[0];
         ReadCMD(CMD);
+        
     } else if(address==CID_RX_REF_POSITION) {
-        //int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
+        
         int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
-        //int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
         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);
         
-        int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
-        pos.ref = (double)temp_pos * 4.0f;
-        vel.ref = (double)temp_vel * 100.0f;
+        if((OPERATING_MODE&0b001)==0) { // Rotary Actuator
+            pos.ref = (double)temp_pos * 1.0f;
+            vel.ref = (double)temp_vel * 10.0f;
+        }
+        else { //Linear Actuator
+            pos.ref = (double)temp_pos * 4.0f;
+            vel.ref = (double)temp_vel * 100.0f;
+        }
+        
         torq.ref = (double)temp_torq * 0.1f;
+        
     } else if(address==CID_RX_REF_VALVE_POS) {
         int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
-        if(temp_ref_valve_pos >= 0){
-            valve_pos.ref = (double)DDV_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)DDV_CENTER)/10000.0f  ;
-        }else{
-            valve_pos.ref = (double)DDV_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)DDV_CENTER)/10000.0f;
+        
+        if ((OPERATING_MODE&0b110) == 0) { //Moog Valve
+            valve_pos.ref = (double) temp_ref_valve_pos;
+        } else if ((OPERATING_MODE&0b110) == 1) { //KNR Valve
+            valve_pos.ref = (double) temp_ref_valve_pos;
+        } else { //SW Valve
+            if(temp_ref_valve_pos >= 0) {
+                valve_pos.ref = (double)DDV_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)DDV_CENTER)/10000.0f;
+            } else {
+                valve_pos.ref = (double)DDV_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)DDV_CENTER)/10000.0f;
+            }
         }
     } else if(address==CID_RX_REF_PWM){
         int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
@@ -745,7 +762,7 @@
     temp_msg.data[2] = (uint8_t) CAN_FREQ;
     temp_msg.data[3] = (uint8_t) (CAN_FREQ >> 8);
     temp_msg.data[4] = (uint8_t) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]);
-    temp_msg.data[5] = (uint8_t) CONTROL_MODE;
+    temp_msg.data[5] = (uint8_t) CONTROL_UTILITY_MODE;
     temp_msg.data[6] = (uint8_t) OPERATING_MODE;
 
     can.write(temp_msg);
@@ -794,7 +811,7 @@
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 2;
     temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE;
-    temp_msg.data[1] = (uint8_t) CONTROL_MODE;
+    temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE;
 
     can.write(temp_msg);
 }
@@ -1056,7 +1073,7 @@
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION;
-    int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION * 10.0f);
+    int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION);
     temp_msg.data[1] = (uint8_t) temp_enc_pulse_per_position;
     temp_msg.data[2] = (uint8_t) (temp_enc_pulse_per_position >> 8);
 
@@ -1384,3 +1401,4 @@
 
 
 
+