Diff: CAN/function_CAN.cpp

Revision:

7:e9086c72bb22

Parent:

4:58c8081de776

Child:

11:82d8768d7351

--- a/CAN/function_CAN.cpp	Tue Aug 20 10:40:27 2019 +0000
+++ b/CAN/function_CAN.cpp	Tue Aug 20 12:27:19 2019 +0000
@@ -1,4 +1,3 @@
-#include "mbed.h"
 #include "function_CAN.h"
 
 // CAN ID Setting Variables
@@ -6,18 +5,26 @@
 int CID_RX_REF_POSITION = 200;
 int CID_RX_REF_TORQUE = 300;
 int CID_RX_REF_PRES_DIFF = 400;
-int CID_RX_REF_PWM = 500;
+int CID_RX_REF_VOUT = 500;
 int CID_RX_REF_VALVE_POSITION = 600;
 
 int CID_TX_INFO = 1100;
 int CID_TX_POSITION = 1200;
 int CID_TX_TORQUE = 1300;
 int CID_TX_PRES = 1400;
-int CID_TX_PWM = 1500;
+int CID_TX_VOUT = 1500;
 int CID_TX_VALVE_POSITION = 1600;
 
-extern CAN can;
-extern CANMessage msg;
+// Board Information 
+int BNO = 0;
+int CONTROL_MODE = 0;
+double P_GAIN_JOINT_POSITION = 0.0;
+double I_GAIN_JOINT_POSITION = 0.0;
+double D_GAIN_JOINT_POSITION = 0.0;
+double P_GAIN_JOINT_TORQUE = 0.0;
+double I_GAIN_JOINT_TORQUE = 0.0;
+double D_GAIN_JOINT_TORQUE = 0.0;
+
 
 void ReadCMD(char CMD)
 {
@@ -38,10 +45,28 @@
     can.read(msg);
     unsigned int address = msg.id;
     
-    if(address==100){
+    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);
+        int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
+        pos.ref = (double)temp_pos;
+        vel.ref = (double)temp_vel;
+    } else if(address==CID_RX_REF_TORQUE) {
+        int16_t temp_torq = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        torq.ref = (double)temp_torq;
+    } else if(address==CID_RX_REF_PRES_DIFF) {
+//        int16_t temp_presdiff = (int16_t) (msg.data[0] | msg.data[1] << 8);
+//        torq.ref = (double)temp_presdiff;   
+    } else if(address==CID_RX_REF_VOUT) {
+        int16_t temp_PWM = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        Vout.ref = (double)temp_PWM;   
+    } else if(address==CID_RX_REF_VALVE_POSITION) {
+        int16_t temp_cur = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        cur.ref = (double)temp_cur; 
     }
+        
 }
 
 /******************************************************************************
@@ -98,7 +123,7 @@
 
     can.write(temp_msg);
 }
-
+*/
 inline void CAN_TX_CONTROL_MODE(void) {
     CANMessage temp_msg;
 
@@ -109,7 +134,7 @@
 
     can.write(temp_msg);
 }
-
+/*
 inline void CAN_TX_JOINT_ENC_DIR(void) {
     CANMessage temp_msg;
 
@@ -173,17 +198,18 @@
 
     can.write(temp_msg);
 }
+*/
 
 inline void CAN_TX_PID_GAIN(int t_type) {
     // t_type = 0 : valve position control gain
     // t_type = 1 : joint position control gain
     // t_type = 2 : joint torque control gain
 
-    long sendPgain, sendIgain, sendDgain;
+    long sendPgain=0, sendIgain=0, sendDgain=0;
     if (t_type == 0) {
-        sendPgain = (long) (P_GAIN_VALVE_POSITION);
-        sendIgain = (long) (I_GAIN_VALVE_POSITION);
-        sendDgain = (long) (D_GAIN_VALVE_POSITION);
+//        sendPgain = (long) (P_GAIN_VALVE_POSITION);
+//        sendIgain = (long) (I_GAIN_VALVE_POSITION);
+//        sendDgain = (long) (D_GAIN_VALVE_POSITION);
     } else if (t_type == 1) {
         sendPgain = (long) (P_GAIN_JOINT_POSITION);
         sendIgain = (long) (I_GAIN_JOINT_POSITION);
@@ -210,6 +236,7 @@
     can.write(temp_msg);
 }
 
+/*
 inline void CAN_TX_VALVE_DEADZONE(void) {
     CANMessage temp_msg;