Diff: RoboteqController.cpp

Revision:

3:a03c91082856

Parent:

2:d61b4a989cab

Child:

4:1ee0a235c997

--- a/RoboteqController.cpp	Thu May 17 14:26:20 2018 +0000
+++ b/RoboteqController.cpp	Fri May 18 09:32:45 2018 +0000
@@ -1,14 +1,25 @@
 #include "RoboteqController.h"
 
+const uint16_t RoboteqController::RPDO_ID[4] = {0x200, 0x300, 0x400, 0x500};   // RPDO msg IDs to send data to the controller
+
 RoboteqController::RoboteqController(){
 }
 
-RoboteqController::RoboteqController(ControllerCAN* controller, uint8_t node_id, uint8_t mot_num) : 
+RoboteqController::RoboteqController(ControllerCAN* controller, uint8_t node_id, uint8_t mot_num, bool TPDO_enabled) : 
                     PeripheralCAN(controller),
                     node_id(node_id), mot_num(mot_num),
-                    Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id) {
+                    Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id),
+                    Id_TPDO1(ID_TPDO1+node_id),Id_TPDO2(ID_TPDO2+node_id), Id_TPDO3(ID_TPDO3+node_id), Id_TPDO4(ID_TPDO4+node_id) {
     PeripheralCAN::addIdRead(&Id_CSS_REQ);
     PeripheralCAN::addIdRead(&Id_CSS_ANS);
+    
+    // If TPDO is enabled, make TPDO message readable
+    if(TPDO_enabled){
+        PeripheralCAN::addIdRead(&Id_TPDO1);
+        PeripheralCAN::addIdRead(&Id_TPDO2);
+        PeripheralCAN::addIdRead(&Id_TPDO3);
+        PeripheralCAN::addIdRead(&Id_TPDO4);
+    }
 }
 
 RoboteqController::~RoboteqController(){}
@@ -23,12 +34,24 @@
         
         switch(SDOmsg.index){
             case qBS: // Brushless speed in rpm
-                rotor_speed = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
+                speed = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
             break;
             //case 
             //..
         }
     }
+    else if (Id == Id_TPDO1){
+        TPDO_receive(1, msg);
+    }
+    else if (Id == Id_TPDO2){
+        TPDO_receive(2, msg);
+    }
+    else if (Id == Id_TPDO3){
+        TPDO_receive(3, msg);
+    }
+    else if (Id == Id_TPDO4){
+        TPDO_receive(4, msg);
+    }
 }
 
 void RoboteqController::SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx){
@@ -73,3 +96,41 @@
     SDOmsg.data[2] = CANmsg.data[6];
     SDOmsg.data[3] = CANmsg.data[7];
 }
+
+void RoboteqController::RPDO_send(uint8_t n, int32_t user_var1, int32_t user_var2){
+    char buffer[8];
+    
+    buffer[0] = user_var1;
+    buffer[1] = (user_var1 >> 8);
+    buffer[2] = (user_var1 >> 16);
+    buffer[3] = (user_var1 >> 24);
+    
+    buffer[4] = user_var2;
+    buffer[5] = (user_var2 >> 8);
+    buffer[6] = (user_var2 >> 16);
+    buffer[7] = (user_var2 >> 24);
+    
+    writeOnCAN(RPDO_ID[n-1] + node_id, buffer, 8);
+}
+
+void RoboteqController::TPDO_parse(const CANMessage& CANmsg, int32_t &user_var1, int32_t &user_var2){
+    user_var1 = CANmsg.data[0] + (CANmsg.data[1] << 8) + (CANmsg.data[2] << 16) + (CANmsg.data[3] << 24);
+    user_var2 = CANmsg.data[4] + (CANmsg.data[5] << 8) + (CANmsg.data[6] << 16) + (CANmsg.data[7] << 24);
+}
+
+void RoboteqController::TPDO_receive(uint8_t n, const CANMessage& CANmsg){
+    /* Nothing to do, use the following structure in a redefinition :
+    if (n == 1){
+        // User defined section
+    }
+    else if (n == 2){
+        // User defined section
+    }
+    else if (n == 3){
+        // User defined section
+    }
+    else if (n == 4){
+        // User defined section
+    }
+    */
+}