Diff: CAN/function_CAN.cpp

Revision:

49:e7bcfc244d40

Parent:

48:889798ff9329

Child:

50:3c630b5eba9f

--- a/CAN/function_CAN.cpp	Wed Jan 29 13:50:24 2020 +0000
+++ b/CAN/function_CAN.cpp	Wed Feb 05 08:35:58 2020 +0000
@@ -8,7 +8,8 @@
 // CAN ID Setting Variables
 int CID_RX_CMD = 100;
 int CID_RX_REF_POSITION = 200;
-int CID_RX_REF_PWM = 300;
+int CID_RX_REF_VALVE_POS = 300;
+int CID_RX_REF_PWM = 400;
 
 int CID_TX_INFO = 1100;
 int CID_TX_POSITION = 1200;
@@ -29,6 +30,7 @@
 
     CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD);
     CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION);
+    CID_RX_REF_VALVE_POS = (int) (BNO + INIT_CID_RX_REF_VALVE_POS);
     CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM);
 
     CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO);
@@ -322,6 +324,20 @@
 
             break;
         }
+        
+        case CRX_ASK_FLOWFORCE_GAIN: {
+            CAN_TX_FLOWFORCE_GAIN();
+
+            break;
+        }
+
+        case CRX_SET_FLOWFORCE_GAIN: {
+            FLOWFORCE_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+            ROM_RESET_DATA();
+
+            break;
+        }
 
         case CRX_ASK_VALVE_FF: {
             CAN_TX_VALVE_FF();
@@ -716,13 +732,16 @@
         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_PWM) {
+    } 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;
         }
+    } else if(address==CID_RX_REF_PWM){
+        int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        Vout.ref = (double) temp_ref_pwm;
     }   
 }
 
@@ -828,7 +847,7 @@
 }
 
 void CAN_TX_VOLTAGE_SUPPLY(void) {
-    int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10);
+    int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10.0f);
 
     CANMessage temp_msg;
 
@@ -842,7 +861,7 @@
 }
 
 void CAN_TX_VOLTAGE_VALVE(void) {
-    int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10);
+    int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f);
 
     CANMessage temp_msg;
 
@@ -936,6 +955,18 @@
     can.write(temp_msg);
 }
 
+void CAN_TX_FLOWFORCE_GAIN(void) {
+    CANMessage temp_msg;
+    
+    temp_msg.id = CID_TX_INFO;
+    temp_msg.len = 3;
+    temp_msg.data[0] = (uint8_t) CTX_SEND_FLOWFORCE_GAIN;
+    temp_msg.data[1] = (uint8_t) FLOWFORCE_GAIN;
+    temp_msg.data[2] = (uint8_t) (FLOWFORCE_GAIN >> 8);
+
+    can.write(temp_msg);
+}
+
 void CAN_TX_VALVE_FF(void) {
     CANMessage temp_msg;
     
@@ -1195,7 +1226,13 @@
 void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex) {
     CANMessage temp_msg;
     int16_t valve_pos_vs_pwm;
-    valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
+//    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);
+    } else {
+        valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+    }
     
     int16_t PWM_VALVE_ID;
     PWM_VALVE_ID = ID_index_array[canindex] * 1000;
@@ -1217,16 +1254,22 @@
     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 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));
+    } else {
+        temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER));
+    }
     
     temp_msg.id = CID_TX_INFO;
-    temp_msg.len = 7;
+    temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE;
-    temp_msg.data[1] = (uint8_t) VALVE_POS_VALVE_ID;
-    temp_msg.data[2] = (uint8_t) (VALVE_POS_VALVE_ID >> 8);
-    temp_msg.data[3] = (uint8_t) valve_pos_vs_flowrate;
-    temp_msg.data[4] = (uint8_t) (valve_pos_vs_flowrate >> 8);
-    temp_msg.data[5] = (uint8_t) (valve_pos_vs_flowrate >> 16);
-    temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 24);
+    temp_msg.data[1] = (uint8_t) temp_valve_pos;
+    temp_msg.data[2] = (uint8_t) (temp_valve_pos >> 8);
+    temp_msg.data[5] = (uint8_t) valve_pos_vs_flowrate;
+    temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 8);
+    temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 16);
+    temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 24);
 
     can.write(temp_msg);
 }
@@ -1344,3 +1387,4 @@
 
 
 
+