Diff: RoboteqController.cpp

Revision:

6:f9c4795448c1

Parent:

4:1ee0a235c997

Child:

7:33d64a3c48a2

--- a/RoboteqController.cpp	Tue May 29 15:51:46 2018 +0000
+++ b/RoboteqController.cpp	Wed Jun 13 09:07:45 2018 +0000
@@ -1,13 +1,13 @@
-#include "RoboteqController.h"
+ #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, bool TPDO_enabled) : 
+RoboteqController::RoboteqController(ControllerCAN* controller, uint8_t node_id, bool TPDO_enabled) : 
                     PeripheralCAN(controller),
-                    node_id(node_id), mot_num(mot_num),
+                    node_id(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);
@@ -34,7 +34,8 @@
         
         switch(SDOmsg.idx){
             case qBS: // Brushless speed in rpm
-                speed = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
+                if(SDOmsg.subindex == 1) speed1 = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
+                else if(SDOmsg.subindex == 2) speed2 = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
             break;
             //case 
             //..
@@ -136,7 +137,7 @@
 }
 
 
-void RoboteqController::sendCommand(uint32_t cmd){
+void RoboteqController::sendCommand(uint8_t mot_num, uint32_t cmd){
     uint8_t data[4];
     
     data[0] = cmd;