RoboteqController.cpp

00001  #include "RoboteqController.h"
00002 
00003 const uint16_t RoboteqController::RPDO_ID[4] = {0x200, 0x300, 0x400, 0x500};   // RPDO msg IDs to send data to the controller
00004 
00005 RoboteqController::RoboteqController(){
00006 }
00007 
00008 RoboteqController::RoboteqController(ControllerCAN* controller, uint8_t node_id, bool TPDO_enabled) : 
00009                     PeripheralCAN(controller),
00010                     node_id(node_id),
00011                     Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id),
00012                     Id_TPDO1(ID_TPDO1+node_id),Id_TPDO2(ID_TPDO2+node_id), Id_TPDO3(ID_TPDO3+node_id), Id_TPDO4(ID_TPDO4+node_id) {
00013     PeripheralCAN::addIdRead(&Id_CSS_REQ);
00014     PeripheralCAN::addIdRead(&Id_CSS_ANS);
00015     
00016     // If TPDO is enabled, make TPDO message readable
00017     if(TPDO_enabled){
00018         PeripheralCAN::addIdRead(&Id_TPDO1);
00019         PeripheralCAN::addIdRead(&Id_TPDO2);
00020         PeripheralCAN::addIdRead(&Id_TPDO3);
00021         PeripheralCAN::addIdRead(&Id_TPDO4);
00022     }
00023 }
00024 
00025 RoboteqController::~RoboteqController(){}
00026     
00027 
00028 void RoboteqController::update(const unsigned short& Id, const CANMessage& msg){
00029     SDOMessage SDOmsg;
00030     
00031     /* Treatment of the controller answer */
00032     if(Id == Id_CSS_ANS){
00033         CANMsg2SDO(msg, SDOmsg);
00034         
00035         switch(SDOmsg.idx){
00036             case qBS: // Brushless speed in rpm
00037                 if(SDOmsg.subindex == 1) speed1 = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
00038                 else if(SDOmsg.subindex == 2) speed2 = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
00039             break;
00040             //case 
00041             //..
00042         }
00043     }
00044     else if (Id == Id_TPDO1){
00045         TPDO_receive(1, msg);
00046     }
00047     else if (Id == Id_TPDO2){
00048         TPDO_receive(2, msg);
00049     }
00050     else if (Id == Id_TPDO3){
00051         TPDO_receive(3, msg);
00052     }
00053     else if (Id == Id_TPDO4){
00054         TPDO_receive(4, msg);
00055     }
00056 }
00057 
00058 void RoboteqController::SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx){
00059     char buffer[8];
00060     
00061     buffer[0] = (CSS_QUERY << 4) + (n << 2);
00062     buffer[1] = idx;
00063     buffer[2] = (idx >> 8);
00064     buffer[3] = sub_idx;
00065     
00066     // Clear bytes 4-7 because no data are  uploaded (it's a query)
00067     for(int i = 4 ; i < 7 ; i++){
00068         buffer[i] = 0;
00069     }
00070     
00071     writeOnCAN(Id_CSS_REQ, buffer, 8);
00072 }
00073 
00074 void RoboteqController::SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data){
00075     char msg[8];
00076     
00077     msg[0] = (CSS_CMD << 4) + (n << 2);
00078     msg[1] = idx;
00079     msg[2] = (idx >> 8);
00080     msg[3] = sub_idx;
00081     
00082     // Load bytes 4-7 from data array
00083     for(int i = 0 ; i < 4-n ; i++){
00084         msg[4+i] = *(data+i);
00085     }
00086     
00087     writeOnCAN(Id_CSS_REQ, msg, 8);
00088 }
00089 
00090 void RoboteqController::CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg){
00091     SDOmsg.css = (CANmsg.data[0] & 0xF0 >> 4);
00092     SDOmsg.n = (CANmsg.data[0] & 0x0F >> 2);
00093     SDOmsg.idx = CANmsg.data[1] + (CANmsg.data[2] << 8);
00094     SDOmsg.subindex = CANmsg.data[3];
00095     SDOmsg.data[0] = CANmsg.data[4];
00096     SDOmsg.data[1] = CANmsg.data[5];
00097     SDOmsg.data[2] = CANmsg.data[6];
00098     SDOmsg.data[3] = CANmsg.data[7];
00099 }
00100 
00101 void RoboteqController::RPDO_send(uint8_t n, int32_t user_var1, int32_t user_var2){
00102     char buffer[8];
00103     
00104     buffer[0] = user_var1;
00105     buffer[1] = (user_var1 >> 8);
00106     buffer[2] = (user_var1 >> 16);
00107     buffer[3] = (user_var1 >> 24);
00108     
00109     buffer[4] = user_var2;
00110     buffer[5] = (user_var2 >> 8);
00111     buffer[6] = (user_var2 >> 16);
00112     buffer[7] = (user_var2 >> 24);
00113     
00114     writeOnCAN(RPDO_ID[n-1] + node_id, buffer, 8);
00115 }
00116 
00117 void RoboteqController::TPDO_parse(const CANMessage& CANmsg, int32_t* p_user_var1, int32_t* p_user_var2){
00118     *p_user_var1 = CANmsg.data[0] + (CANmsg.data[1] << 8) + (CANmsg.data[2] << 16) + (CANmsg.data[3] << 24);
00119     *p_user_var2 = CANmsg.data[4] + (CANmsg.data[5] << 8) + (CANmsg.data[6] << 16) + (CANmsg.data[7] << 24);
00120 }
00121 
00122 void RoboteqController::TPDO_receive(uint8_t n, const CANMessage& CANmsg){
00123     /* Nothing to do, use the following structure in a redefinition :
00124     if (n == 1){
00125         // User defined section
00126     }
00127     else if (n == 2){
00128         // User defined section
00129     }
00130     else if (n == 3){
00131         // User defined section
00132     }
00133     else if (n == 4){
00134         // User defined section
00135     }
00136     */
00137 }
00138 
00139 
00140 void RoboteqController::sendCommand(uint8_t mot_num, uint32_t cmd){
00141     uint8_t data[4];
00142     
00143     data[0] = cmd;
00144     data[1] = (cmd>>8);
00145     data[2] = (cmd>>16);
00146     data[3] = (cmd>>24);
00147     
00148     SDO_command(0, cCANGO, mot_num, data);
00149 }
00150 
00151 int16_t RoboteqController::getSpeed(uint8_t mot_num){
00152     if(mot_num == 1) return speed1;
00153     else if(mot_num == 2) return speed2;     
00154 }