Andy Rodriguez / Roboteq_CANOpen_USMA

Dependents:   JAGBot

RoboteqController.cpp@6:f9c4795448c1, 2018-06-13 (annotated)

Committer:
zephyrcare
Date:
Wed Jun 13 09:07:45 2018 +0000
Revision:
6:f9c4795448c1
Parent:
4:1ee0a235c997
Child:
7:33d64a3c48a2
une seule m?thode pour le calcul des commandes; r?cup?ration angle guidon; vitesse des moteurs selon la rotation du guidon;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zephyrcare 6:f9c4795448c1 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
zephyrcare 6:f9c4795448c1 8 RoboteqController::RoboteqController(ControllerCAN* controller, uint8_t node_id, bool TPDO_enabled) :
garivetm 1:57b8f6ed930b 9 PeripheralCAN(controller),
zephyrcare 6:f9c4795448c1 10 node_id(node_id),
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
zephyrcare 4:1ee0a235c997 35 switch(SDOmsg.idx){
garivetm 0:faf0960419c1 36 case qBS: // Brushless speed in rpm
zephyrcare 6:f9c4795448c1 37 if(SDOmsg.subindex == 1) speed1 = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
zephyrcare 6:f9c4795448c1 38 else if(SDOmsg.subindex == 2) speed2 = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
garivetm 0:faf0960419c1 39 break;
garivetm 0:faf0960419c1 40 //case
garivetm 0:faf0960419c1 41 //..
garivetm 0:faf0960419c1 42 }
garivetm 0:faf0960419c1 43 }
garivetm 3:a03c91082856 44 else if (Id == Id_TPDO1){
garivetm 3:a03c91082856 45 TPDO_receive(1, msg);
garivetm 3:a03c91082856 46 }
garivetm 3:a03c91082856 47 else if (Id == Id_TPDO2){
garivetm 3:a03c91082856 48 TPDO_receive(2, msg);
garivetm 3:a03c91082856 49 }
garivetm 3:a03c91082856 50 else if (Id == Id_TPDO3){
garivetm 3:a03c91082856 51 TPDO_receive(3, msg);
garivetm 3:a03c91082856 52 }
garivetm 3:a03c91082856 53 else if (Id == Id_TPDO4){
garivetm 3:a03c91082856 54 TPDO_receive(4, msg);
garivetm 3:a03c91082856 55 }
garivetm 0:faf0960419c1 56 }
garivetm 0:faf0960419c1 57
garivetm 0:faf0960419c1 58 void RoboteqController::SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx){
garivetm 0:faf0960419c1 59 char buffer[8];
garivetm 0:faf0960419c1 60
garivetm 0:faf0960419c1 61 buffer[0] = (CSS_QUERY << 4) + (n << 2);
garivetm 0:faf0960419c1 62 buffer[1] = idx;
garivetm 0:faf0960419c1 63 buffer[2] = (idx >> 8);
garivetm 0:faf0960419c1 64 buffer[3] = sub_idx;
garivetm 0:faf0960419c1 65
garivetm 0:faf0960419c1 66 // Clear bytes 4-7 because no data are uploaded (it's a query)
garivetm 0:faf0960419c1 67 for(int i = 4 ; i < 7 ; i++){
garivetm 0:faf0960419c1 68 buffer[i] = 0;
garivetm 0:faf0960419c1 69 }
garivetm 0:faf0960419c1 70
garivetm 0:faf0960419c1 71 writeOnCAN(Id_CSS_REQ, buffer, 8);
garivetm 0:faf0960419c1 72 }
garivetm 0:faf0960419c1 73
garivetm 0:faf0960419c1 74 void RoboteqController::SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data){
zephyrcare 4:1ee0a235c997 75 char msg[8];
garivetm 0:faf0960419c1 76
zephyrcare 4:1ee0a235c997 77 msg[0] = (CSS_CMD << 4) + (n << 2);
zephyrcare 4:1ee0a235c997 78 msg[1] = idx;
zephyrcare 4:1ee0a235c997 79 msg[2] = (idx >> 8);
zephyrcare 4:1ee0a235c997 80 msg[3] = sub_idx;
garivetm 0:faf0960419c1 81
garivetm 0:faf0960419c1 82 // Load bytes 4-7 from data array
garivetm 0:faf0960419c1 83 for(int i = 0 ; i < 4-n ; i++){
zephyrcare 4:1ee0a235c997 84 msg[4+i] = *(data+i);
garivetm 0:faf0960419c1 85 }
garivetm 0:faf0960419c1 86
zephyrcare 4:1ee0a235c997 87 writeOnCAN(Id_CSS_REQ, msg, 8);
garivetm 0:faf0960419c1 88 }
garivetm 0:faf0960419c1 89
garivetm 0:faf0960419c1 90 void RoboteqController::CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg){
garivetm 0:faf0960419c1 91 SDOmsg.css = (CANmsg.data[0] & 0xF0 >> 4);
garivetm 0:faf0960419c1 92 SDOmsg.n = (CANmsg.data[0] & 0x0F >> 2);
zephyrcare 4:1ee0a235c997 93 SDOmsg.idx = CANmsg.data[1] + (CANmsg.data[2] << 8);
garivetm 0:faf0960419c1 94 SDOmsg.subindex = CANmsg.data[3];
garivetm 0:faf0960419c1 95 SDOmsg.data[0] = CANmsg.data[4];
garivetm 0:faf0960419c1 96 SDOmsg.data[1] = CANmsg.data[5];
garivetm 0:faf0960419c1 97 SDOmsg.data[2] = CANmsg.data[6];
garivetm 0:faf0960419c1 98 SDOmsg.data[3] = CANmsg.data[7];
garivetm 2:d61b4a989cab 99 }
garivetm 3:a03c91082856 100
garivetm 3:a03c91082856 101 void RoboteqController::RPDO_send(uint8_t n, int32_t user_var1, int32_t user_var2){
garivetm 3:a03c91082856 102 char buffer[8];
garivetm 3:a03c91082856 103
garivetm 3:a03c91082856 104 buffer[0] = user_var1;
garivetm 3:a03c91082856 105 buffer[1] = (user_var1 >> 8);
garivetm 3:a03c91082856 106 buffer[2] = (user_var1 >> 16);
garivetm 3:a03c91082856 107 buffer[3] = (user_var1 >> 24);
garivetm 3:a03c91082856 108
garivetm 3:a03c91082856 109 buffer[4] = user_var2;
garivetm 3:a03c91082856 110 buffer[5] = (user_var2 >> 8);
garivetm 3:a03c91082856 111 buffer[6] = (user_var2 >> 16);
garivetm 3:a03c91082856 112 buffer[7] = (user_var2 >> 24);
garivetm 3:a03c91082856 113
garivetm 3:a03c91082856 114 writeOnCAN(RPDO_ID[n-1] + node_id, buffer, 8);
garivetm 3:a03c91082856 115 }
garivetm 3:a03c91082856 116
zephyrcare 4:1ee0a235c997 117 void RoboteqController::TPDO_parse(const CANMessage& CANmsg, int32_t* p_user_var1, int32_t* p_user_var2){
zephyrcare 4:1ee0a235c997 118 *p_user_var1 = CANmsg.data[0] + (CANmsg.data[1] << 8) + (CANmsg.data[2] << 16) + (CANmsg.data[3] << 24);
zephyrcare 4:1ee0a235c997 119 *p_user_var2 = CANmsg.data[4] + (CANmsg.data[5] << 8) + (CANmsg.data[6] << 16) + (CANmsg.data[7] << 24);
garivetm 3:a03c91082856 120 }
garivetm 3:a03c91082856 121
garivetm 3:a03c91082856 122 void RoboteqController::TPDO_receive(uint8_t n, const CANMessage& CANmsg){
garivetm 3:a03c91082856 123 /* Nothing to do, use the following structure in a redefinition :
garivetm 3:a03c91082856 124 if (n == 1){
garivetm 3:a03c91082856 125 // User defined section
garivetm 3:a03c91082856 126 }
garivetm 3:a03c91082856 127 else if (n == 2){
garivetm 3:a03c91082856 128 // User defined section
garivetm 3:a03c91082856 129 }
garivetm 3:a03c91082856 130 else if (n == 3){
garivetm 3:a03c91082856 131 // User defined section
garivetm 3:a03c91082856 132 }
garivetm 3:a03c91082856 133 else if (n == 4){
garivetm 3:a03c91082856 134 // User defined section
garivetm 3:a03c91082856 135 }
garivetm 3:a03c91082856 136 */
garivetm 3:a03c91082856 137 }
zephyrcare 4:1ee0a235c997 138
zephyrcare 4:1ee0a235c997 139
zephyrcare 6:f9c4795448c1 140 void RoboteqController::sendCommand(uint8_t mot_num, uint32_t cmd){
zephyrcare 4:1ee0a235c997 141 uint8_t data[4];
zephyrcare 4:1ee0a235c997 142
zephyrcare 4:1ee0a235c997 143 data[0] = cmd;
zephyrcare 4:1ee0a235c997 144 data[1] = (cmd>>8);
zephyrcare 4:1ee0a235c997 145 data[2] = (cmd>>16);
zephyrcare 4:1ee0a235c997 146 data[3] = (cmd>>24);
zephyrcare 4:1ee0a235c997 147
zephyrcare 4:1ee0a235c997 148 SDO_command(0, cCANGO, mot_num, data);
zephyrcare 4:1ee0a235c997 149 }