A Roboteq controller c++ interface to allow a Brushless motor control on a CAN bus. Used for a FBL2360 reference.
Dependencies: CAN_FIFO_Triporteur
RoboteqController.cpp
- Committer:
- garivetm
- Date:
- 2018-05-05
- Revision:
- 0:faf0960419c1
- Child:
- 1:57b8f6ed930b
File content as of revision 0:faf0960419c1:
#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]; }