Diff: RoboteqController.cpp

Revision:

0:faf0960419c1

Child:

1:57b8f6ed930b

diff -r 000000000000 -r faf0960419c1 RoboteqController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboteqController.cpp	Sat May 05 15:41:25 2018 +0000
@@ -0,0 +1,73 @@
+#include "RoboteqController.h"
+
+RoboteqController::RoboteqController() : node_id(1), mot_num(0), Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id){
+}
+
+RoboteqController::RoboteqController(uint8_t node_id, uint8_t mot_num) : node_id(node_id), mot_num(mot_num),
+                                                                        Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id) {
+    PeripheralCAN::addIdRead(&Id_CSS_REQ);
+    PeripheralCAN::addIdRead(&Id_CSS_ANS);
+}
+
+RoboteqController::~RoboteqController(){}
+    
+
+void RoboteqController::update(const unsigned short& Id, const CANMessage& msg){
+    SDOMessage SDOmsg;
+    
+    /* Treatment of the controller answer */
+    if(Id == Id_CSS_ANS){
+        CANMsg2SDO(msg, SDOmsg);
+        
+        switch(SDOmsg.index){
+            case qBS: // Brushless speed in rpm
+                rotor_speed = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
+            break;
+            //case 
+            //..
+        }
+    }
+}
+
+void RoboteqController::SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx){
+    char buffer[8];
+    
+    buffer[0] = (CSS_QUERY << 4) + (n << 2);
+    buffer[1] = idx;
+    buffer[2] = (idx >> 8);
+    buffer[3] = sub_idx;
+    
+    // Clear bytes 4-7 because no data are  uploaded (it's a query)
+    for(int i = 4 ; i < 7 ; i++){
+        buffer[i] = 0;
+    }
+    
+    writeOnCAN(Id_CSS_REQ, buffer, 8);
+}
+
+void RoboteqController::SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data){
+    char buffer[8];
+    
+    buffer[0] = (CSS_CMD << 4) + (n << 2);
+    buffer[1] = idx;
+    buffer[2] = (idx >> 8);
+    buffer[3] = sub_idx;
+    
+    // Load bytes 4-7 from data array
+    for(int i = 0 ; i < 4-n ; i++){
+        buffer[4+i] = *(data+i);
+    }
+    
+    writeOnCAN(Id_CSS_REQ, buffer, 8);
+}
+
+void RoboteqController::CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg){
+    SDOmsg.css = (CANmsg.data[0] & 0xF0 >> 4);
+    SDOmsg.n = (CANmsg.data[0] & 0x0F >> 2);
+    SDOmsg.index = CANmsg.data[1] + (CANmsg.data[2] << 8);
+    SDOmsg.subindex = CANmsg.data[3];
+    SDOmsg.data[0] = CANmsg.data[4];
+    SDOmsg.data[1] = CANmsg.data[5];
+    SDOmsg.data[2] = CANmsg.data[6];
+    SDOmsg.data[3] = CANmsg.data[7];
+}
\ No newline at end of file