Andy Rodriguez / Roboteq_CANOpen_USMA

Dependents:   JAGBot

RoboteqController.cpp@3:a03c91082856, 2018-05-18 (annotated)

Committer:
garivetm
Date:
Fri May 18 09:32:45 2018 +0000
Revision:
3:a03c91082856
Parent:
2:d61b4a989cab
Child:
4:1ee0a235c997
Enable RPDO message sending and TPDO message reception.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garivetm 0:faf0960419c1 1 #include "RoboteqController.h"
garivetm 0:faf0960419c1 2
garivetm 3:a03c91082856 3 const uint16_t RoboteqController::RPDO_ID[4] = {0x200, 0x300, 0x400, 0x500}; // RPDO msg IDs to send data to the controller
garivetm 3:a03c91082856 4
garivetm 1:57b8f6ed930b 5 RoboteqController::RoboteqController(){
garivetm 0:faf0960419c1 6 }
garivetm 0:faf0960419c1 7
garivetm 3:a03c91082856 8 RoboteqController::RoboteqController(ControllerCAN* controller, uint8_t node_id, uint8_t mot_num, bool TPDO_enabled) :
garivetm 1:57b8f6ed930b 9 PeripheralCAN(controller),
garivetm 1:57b8f6ed930b 10 node_id(node_id), mot_num(mot_num),
garivetm 3:a03c91082856 11 Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id),
garivetm 3:a03c91082856 12 Id_TPDO1(ID_TPDO1+node_id),Id_TPDO2(ID_TPDO2+node_id), Id_TPDO3(ID_TPDO3+node_id), Id_TPDO4(ID_TPDO4+node_id) {
garivetm 0:faf0960419c1 13 PeripheralCAN::addIdRead(&Id_CSS_REQ);
garivetm 0:faf0960419c1 14 PeripheralCAN::addIdRead(&Id_CSS_ANS);
garivetm 3:a03c91082856 15
garivetm 3:a03c91082856 16 // If TPDO is enabled, make TPDO message readable
garivetm 3:a03c91082856 17 if(TPDO_enabled){
garivetm 3:a03c91082856 18 PeripheralCAN::addIdRead(&Id_TPDO1);
garivetm 3:a03c91082856 19 PeripheralCAN::addIdRead(&Id_TPDO2);
garivetm 3:a03c91082856 20 PeripheralCAN::addIdRead(&Id_TPDO3);
garivetm 3:a03c91082856 21 PeripheralCAN::addIdRead(&Id_TPDO4);
garivetm 3:a03c91082856 22 }
garivetm 0:faf0960419c1 23 }
garivetm 0:faf0960419c1 24
garivetm 0:faf0960419c1 25 RoboteqController::~RoboteqController(){}
garivetm 0:faf0960419c1 26
garivetm 0:faf0960419c1 27
garivetm 0:faf0960419c1 28 void RoboteqController::update(const unsigned short& Id, const CANMessage& msg){
garivetm 0:faf0960419c1 29 SDOMessage SDOmsg;
garivetm 0:faf0960419c1 30
garivetm 0:faf0960419c1 31 /* Treatment of the controller answer */
garivetm 0:faf0960419c1 32 if(Id == Id_CSS_ANS){
garivetm 0:faf0960419c1 33 CANMsg2SDO(msg, SDOmsg);
garivetm 0:faf0960419c1 34
garivetm 0:faf0960419c1 35 switch(SDOmsg.index){
garivetm 0:faf0960419c1 36 case qBS: // Brushless speed in rpm
garivetm 3:a03c91082856 37 speed = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
garivetm 0:faf0960419c1 38 break;
garivetm 0:faf0960419c1 39 //case
garivetm 0:faf0960419c1 40 //..
garivetm 0:faf0960419c1 41 }
garivetm 0:faf0960419c1 42 }
garivetm 3:a03c91082856 43 else if (Id == Id_TPDO1){
garivetm 3:a03c91082856 44 TPDO_receive(1, msg);
garivetm 3:a03c91082856 45 }
garivetm 3:a03c91082856 46 else if (Id == Id_TPDO2){
garivetm 3:a03c91082856 47 TPDO_receive(2, msg);
garivetm 3:a03c91082856 48 }
garivetm 3:a03c91082856 49 else if (Id == Id_TPDO3){
garivetm 3:a03c91082856 50 TPDO_receive(3, msg);
garivetm 3:a03c91082856 51 }
garivetm 3:a03c91082856 52 else if (Id == Id_TPDO4){
garivetm 3:a03c91082856 53 TPDO_receive(4, msg);
garivetm 3:a03c91082856 54 }
garivetm 0:faf0960419c1 55 }
garivetm 0:faf0960419c1 56
garivetm 0:faf0960419c1 57 void RoboteqController::SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx){
garivetm 0:faf0960419c1 58 char buffer[8];
garivetm 0:faf0960419c1 59
garivetm 0:faf0960419c1 60 buffer[0] = (CSS_QUERY << 4) + (n << 2);
garivetm 0:faf0960419c1 61 buffer[1] = idx;
garivetm 0:faf0960419c1 62 buffer[2] = (idx >> 8);
garivetm 0:faf0960419c1 63 buffer[3] = sub_idx;
garivetm 0:faf0960419c1 64
garivetm 0:faf0960419c1 65 // Clear bytes 4-7 because no data are uploaded (it's a query)
garivetm 0:faf0960419c1 66 for(int i = 4 ; i < 7 ; i++){
garivetm 0:faf0960419c1 67 buffer[i] = 0;
garivetm 0:faf0960419c1 68 }
garivetm 0:faf0960419c1 69
garivetm 0:faf0960419c1 70 writeOnCAN(Id_CSS_REQ, buffer, 8);
garivetm 0:faf0960419c1 71 }
garivetm 0:faf0960419c1 72
garivetm 0:faf0960419c1 73 void RoboteqController::SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data){
garivetm 0:faf0960419c1 74 char buffer[8];
garivetm 0:faf0960419c1 75
garivetm 0:faf0960419c1 76 buffer[0] = (CSS_CMD << 4) + (n << 2);
garivetm 0:faf0960419c1 77 buffer[1] = idx;
garivetm 0:faf0960419c1 78 buffer[2] = (idx >> 8);
garivetm 0:faf0960419c1 79 buffer[3] = sub_idx;
garivetm 0:faf0960419c1 80
garivetm 0:faf0960419c1 81 // Load bytes 4-7 from data array
garivetm 0:faf0960419c1 82 for(int i = 0 ; i < 4-n ; i++){
garivetm 0:faf0960419c1 83 buffer[4+i] = *(data+i);
garivetm 0:faf0960419c1 84 }
garivetm 0:faf0960419c1 85
garivetm 0:faf0960419c1 86 writeOnCAN(Id_CSS_REQ, buffer, 8);
garivetm 0:faf0960419c1 87 }
garivetm 0:faf0960419c1 88
garivetm 0:faf0960419c1 89 void RoboteqController::CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg){
garivetm 0:faf0960419c1 90 SDOmsg.css = (CANmsg.data[0] & 0xF0 >> 4);
garivetm 0:faf0960419c1 91 SDOmsg.n = (CANmsg.data[0] & 0x0F >> 2);
garivetm 0:faf0960419c1 92 SDOmsg.index = CANmsg.data[1] + (CANmsg.data[2] << 8);
garivetm 0:faf0960419c1 93 SDOmsg.subindex = CANmsg.data[3];
garivetm 0:faf0960419c1 94 SDOmsg.data[0] = CANmsg.data[4];
garivetm 0:faf0960419c1 95 SDOmsg.data[1] = CANmsg.data[5];
garivetm 0:faf0960419c1 96 SDOmsg.data[2] = CANmsg.data[6];
garivetm 0:faf0960419c1 97 SDOmsg.data[3] = CANmsg.data[7];
garivetm 2:d61b4a989cab 98 }
garivetm 3:a03c91082856 99
garivetm 3:a03c91082856 100 void RoboteqController::RPDO_send(uint8_t n, int32_t user_var1, int32_t user_var2){
garivetm 3:a03c91082856 101 char buffer[8];
garivetm 3:a03c91082856 102
garivetm 3:a03c91082856 103 buffer[0] = user_var1;
garivetm 3:a03c91082856 104 buffer[1] = (user_var1 >> 8);
garivetm 3:a03c91082856 105 buffer[2] = (user_var1 >> 16);
garivetm 3:a03c91082856 106 buffer[3] = (user_var1 >> 24);
garivetm 3:a03c91082856 107
garivetm 3:a03c91082856 108 buffer[4] = user_var2;
garivetm 3:a03c91082856 109 buffer[5] = (user_var2 >> 8);
garivetm 3:a03c91082856 110 buffer[6] = (user_var2 >> 16);
garivetm 3:a03c91082856 111 buffer[7] = (user_var2 >> 24);
garivetm 3:a03c91082856 112
garivetm 3:a03c91082856 113 writeOnCAN(RPDO_ID[n-1] + node_id, buffer, 8);
garivetm 3:a03c91082856 114 }
garivetm 3:a03c91082856 115
garivetm 3:a03c91082856 116 void RoboteqController::TPDO_parse(const CANMessage& CANmsg, int32_t &user_var1, int32_t &user_var2){
garivetm 3:a03c91082856 117 user_var1 = CANmsg.data[0] + (CANmsg.data[1] << 8) + (CANmsg.data[2] << 16) + (CANmsg.data[3] << 24);
garivetm 3:a03c91082856 118 user_var2 = CANmsg.data[4] + (CANmsg.data[5] << 8) + (CANmsg.data[6] << 16) + (CANmsg.data[7] << 24);
garivetm 3:a03c91082856 119 }
garivetm 3:a03c91082856 120
garivetm 3:a03c91082856 121 void RoboteqController::TPDO_receive(uint8_t n, const CANMessage& CANmsg){
garivetm 3:a03c91082856 122 /* Nothing to do, use the following structure in a redefinition :
garivetm 3:a03c91082856 123 if (n == 1){
garivetm 3:a03c91082856 124 // User defined section
garivetm 3:a03c91082856 125 }
garivetm 3:a03c91082856 126 else if (n == 2){
garivetm 3:a03c91082856 127 // User defined section
garivetm 3:a03c91082856 128 }
garivetm 3:a03c91082856 129 else if (n == 3){
garivetm 3:a03c91082856 130 // User defined section
garivetm 3:a03c91082856 131 }
garivetm 3:a03c91082856 132 else if (n == 4){
garivetm 3:a03c91082856 133 // User defined section
garivetm 3:a03c91082856 134 }
garivetm 3:a03c91082856 135 */
garivetm 3:a03c91082856 136 }