Diff: CAN/function_CAN.cpp

Revision:

57:f4819de54e7a

Parent:

56:6f50d9d3bfee

Child:

60:64181f1d3e60

diff -r 6f50d9d3bfee -r f4819de54e7a CAN/function_CAN.cpp
--- a/CAN/function_CAN.cpp	Tue Mar 03 12:27:53 2020 +0000
+++ b/CAN/function_CAN.cpp	Fri Mar 06 12:14:38 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;
         }
 
@@ -285,10 +283,6 @@
             VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8);
             
             ROM_RESET_DATA();
-            
-            //spi_eeprom_write(RID_VALVE_CNETER, (int16_t) (VALVE_CENTER));
-            //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
-            //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
 
             break;
         }
@@ -385,6 +379,7 @@
             PISTON_AREA_A = (int16_t) (msg.data[1] | msg.data[2] << 8);
             PISTON_AREA_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
             PISTON_AREA_alpha = (double)PISTON_AREA_B/(double)PISTON_AREA_A;
+            alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
             
             ROM_RESET_DATA();
             
@@ -449,11 +444,9 @@
         }
 
         case CRX_SET_VALVE_LIMIT: {
-            VALVE_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
-            VALVE_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
+            VALVE_MIN_POS = (int16_t) (msg.data[1] | msg.data[2] << 8);
+            VALVE_MAX_POS = (int16_t) (msg.data[3] | msg.data[4] << 8);
             ROM_RESET_DATA();
-            //spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int16_t) VALVE_LIMIT_MINUS);
-            //spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int16_t) VALVE_LIMIT_PLUS);
 
             break;
         }
@@ -635,9 +628,8 @@
             break;
         }
         case CRX_SET_VALVE_CENTER_OFFSET: {
-            VALVE_CENTER_OFFSET_times10 = (int16_t) (msg.data[1] | msg.data[2] << 8);
-            VALVE_CENTER_OFFSET = ((float) VALVE_CENTER_OFFSET_times10) * 0.1f;
-            DDV_CENTER = DDV_CENTER + VALVE_CENTER_OFFSET;
+            VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
+            VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET;
             ROM_RESET_DATA();
             break;
         }
@@ -690,7 +682,7 @@
             break;
         }
         case CRX_ASK_VALVE_MAX_MIN_POS: {
-            CAN_TX_DDV_VALVE_DEADZONE();
+            //CAN_TX_DDV_VALVE_DEADZONE();
             CAN_TX_DDV_VALVE_MAX_MIN_POS();
             break;
         }
@@ -712,22 +704,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)>>1) == 0) { //Moog Valve
+            valve_pos.ref = (double) temp_ref_valve_pos;
+        } else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve
+            valve_pos.ref = (double) temp_ref_valve_pos;
+        } 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;
+            } else {
+                valve_pos.ref = (double)VALVE_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
+            }
         }
     } else if(address==CID_RX_REF_PWM){
         int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
@@ -749,7 +756,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);
@@ -798,7 +805,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);
 }
@@ -914,8 +921,8 @@
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 7;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE;
-    temp_msg.data[1] = (uint8_t) (int) (VALVE_CENTER);
-    temp_msg.data[2] = (uint8_t) ((int) (VALVE_CENTER) >> 8);
+    temp_msg.data[1] = (uint8_t) VALVE_CENTER;
+    temp_msg.data[2] = (uint8_t) (VALVE_CENTER >> 8);
     temp_msg.data[3] = (uint8_t) (int) (VALVE_DEADZONE_PLUS);
     temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8);
     temp_msg.data[5] = (uint8_t) (int) (VALVE_DEADZONE_MINUS);
@@ -1046,10 +1053,10 @@
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 5;
     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT;
-    temp_msg.data[1] = (uint8_t) VALVE_LIMIT_MINUS;
-    temp_msg.data[2] = (uint8_t) (VALVE_LIMIT_MINUS >> 8);
-    temp_msg.data[3] = (uint8_t) VALVE_LIMIT_PLUS;
-    temp_msg.data[4] = (uint8_t) (VALVE_LIMIT_PLUS >> 8);
+    temp_msg.data[1] = (uint8_t) VALVE_MIN_POS;
+    temp_msg.data[2] = (uint8_t) (VALVE_MIN_POS >> 8);
+    temp_msg.data[3] = (uint8_t) VALVE_MAX_POS;
+    temp_msg.data[4] = (uint8_t) (VALVE_MAX_POS >> 8);
 
     can.write(temp_msg);
 }
@@ -1129,9 +1136,11 @@
     float temp_valve_deadzone_plus = 0.0f;
     float temp_ddv_center = 0.0f;
     
-    temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
-    temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
-    temp_ddv_center = 0.0f;
+    //temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
+    //temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
+    temp_valve_deadzone_plus = (double)VALVE_DEADZONE_PLUS;
+    temp_valve_deadzone_minus = (double)VALVE_DEADZONE_MINUS;
+    temp_ddv_center = (double)VALVE_CENTER;
     
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 7;
@@ -1209,10 +1218,10 @@
     int16_t valve_pos_vs_pwm;
 //    valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
     
-    if(VALVE_POS_VS_PWM[canindex]>=DDV_CENTER) {
-        valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+    if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_CENTER) {
+        valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
     } else {
-        valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+        valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
     }
     
     int16_t PWM_VALVE_ID;
@@ -1234,12 +1243,12 @@
     int32_t valve_pos_vs_flowrate;
     valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]);
 
-    int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + (int) DDV_CENTER;
+    int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + VALVE_CENTER;
     int16_t temp_valve_pos = 0;
-    if(VALVE_POS_VALVE_ID>=DDV_CENTER) {
-        temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER));
+    if(VALVE_POS_VALVE_ID>=VALVE_CENTER) {
+        temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER));
     } else {
-        temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER));
+        temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER));
     }
     
     temp_msg.id = CID_TX_INFO;
@@ -1276,9 +1285,9 @@
     float temp_valve_min_pos = 0.0f;
     float temp_ddv_center = 0.0f;
     
-    temp_valve_max_pos = 10000.0f;
-    temp_valve_min_pos = -10000.0f;
-    temp_ddv_center = 0.0f;
+    temp_valve_max_pos = VALVE_MAX_POS;
+    temp_valve_min_pos = VALVE_MIN_POS;
+    temp_ddv_center = VALVE_CENTER;
     
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 7;
@@ -1345,7 +1354,7 @@
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_PRES;
-    temp_msg.len = 8;
+    temp_msg.len = 4;
     temp_msg.data[0] = (uint8_t) t_pres_a;
     temp_msg.data[1] = (uint8_t) (t_pres_a >> 8);
     temp_msg.data[2] = (uint8_t) t_pres_b;
@@ -1357,8 +1366,8 @@
 void CAN_TX_PWM(int16_t t_pwm) {
     CANMessage temp_msg;
 
-    temp_msg.id = CID_TX_POSITION;
-    temp_msg.len = 8;
+    temp_msg.id = CID_TX_VOUT;
+    temp_msg.len = 2;
     temp_msg.data[0] = (uint8_t) t_pwm;
     temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
 
@@ -1389,3 +1398,5 @@
 
 
 
+
+