Hublex / Roboteq_CANOpen

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@0:faf0960419c1, 2018-05-05 (annotated)

Committer:
garivetm
Date:
Sat May 05 15:41:25 2018 +0000
Revision:
0:faf0960419c1
Child:
1:57b8f6ed930b
First commit class structure and minimal stuff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garivetm 0:faf0960419c1 1 #include "RoboteqController.h"
garivetm 0:faf0960419c1 2
garivetm 0:faf0960419c1 3 RoboteqController::RoboteqController() : node_id(1), mot_num(0), Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id){
garivetm 0:faf0960419c1 4 }
garivetm 0:faf0960419c1 5
garivetm 0:faf0960419c1 6 RoboteqController::RoboteqController(uint8_t node_id, uint8_t mot_num) : node_id(node_id), mot_num(mot_num),
garivetm 0:faf0960419c1 7 Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id) {
garivetm 0:faf0960419c1 8 PeripheralCAN::addIdRead(&Id_CSS_REQ);
garivetm 0:faf0960419c1 9 PeripheralCAN::addIdRead(&Id_CSS_ANS);
garivetm 0:faf0960419c1 10 }
garivetm 0:faf0960419c1 11
garivetm 0:faf0960419c1 12 RoboteqController::~RoboteqController(){}
garivetm 0:faf0960419c1 13
garivetm 0:faf0960419c1 14
garivetm 0:faf0960419c1 15 void RoboteqController::update(const unsigned short& Id, const CANMessage& msg){
garivetm 0:faf0960419c1 16 SDOMessage SDOmsg;
garivetm 0:faf0960419c1 17
garivetm 0:faf0960419c1 18 /* Treatment of the controller answer */
garivetm 0:faf0960419c1 19 if(Id == Id_CSS_ANS){
garivetm 0:faf0960419c1 20 CANMsg2SDO(msg, SDOmsg);
garivetm 0:faf0960419c1 21
garivetm 0:faf0960419c1 22 switch(SDOmsg.index){
garivetm 0:faf0960419c1 23 case qBS: // Brushless speed in rpm
garivetm 0:faf0960419c1 24 rotor_speed = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
garivetm 0:faf0960419c1 25 break;
garivetm 0:faf0960419c1 26 //case
garivetm 0:faf0960419c1 27 //..
garivetm 0:faf0960419c1 28 }
garivetm 0:faf0960419c1 29 }
garivetm 0:faf0960419c1 30 }
garivetm 0:faf0960419c1 31
garivetm 0:faf0960419c1 32 void RoboteqController::SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx){
garivetm 0:faf0960419c1 33 char buffer[8];
garivetm 0:faf0960419c1 34
garivetm 0:faf0960419c1 35 buffer[0] = (CSS_QUERY << 4) + (n << 2);
garivetm 0:faf0960419c1 36 buffer[1] = idx;
garivetm 0:faf0960419c1 37 buffer[2] = (idx >> 8);
garivetm 0:faf0960419c1 38 buffer[3] = sub_idx;
garivetm 0:faf0960419c1 39
garivetm 0:faf0960419c1 40 // Clear bytes 4-7 because no data are uploaded (it's a query)
garivetm 0:faf0960419c1 41 for(int i = 4 ; i < 7 ; i++){
garivetm 0:faf0960419c1 42 buffer[i] = 0;
garivetm 0:faf0960419c1 43 }
garivetm 0:faf0960419c1 44
garivetm 0:faf0960419c1 45 writeOnCAN(Id_CSS_REQ, buffer, 8);
garivetm 0:faf0960419c1 46 }
garivetm 0:faf0960419c1 47
garivetm 0:faf0960419c1 48 void RoboteqController::SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data){
garivetm 0:faf0960419c1 49 char buffer[8];
garivetm 0:faf0960419c1 50
garivetm 0:faf0960419c1 51 buffer[0] = (CSS_CMD << 4) + (n << 2);
garivetm 0:faf0960419c1 52 buffer[1] = idx;
garivetm 0:faf0960419c1 53 buffer[2] = (idx >> 8);
garivetm 0:faf0960419c1 54 buffer[3] = sub_idx;
garivetm 0:faf0960419c1 55
garivetm 0:faf0960419c1 56 // Load bytes 4-7 from data array
garivetm 0:faf0960419c1 57 for(int i = 0 ; i < 4-n ; i++){
garivetm 0:faf0960419c1 58 buffer[4+i] = *(data+i);
garivetm 0:faf0960419c1 59 }
garivetm 0:faf0960419c1 60
garivetm 0:faf0960419c1 61 writeOnCAN(Id_CSS_REQ, buffer, 8);
garivetm 0:faf0960419c1 62 }
garivetm 0:faf0960419c1 63
garivetm 0:faf0960419c1 64 void RoboteqController::CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg){
garivetm 0:faf0960419c1 65 SDOmsg.css = (CANmsg.data[0] & 0xF0 >> 4);
garivetm 0:faf0960419c1 66 SDOmsg.n = (CANmsg.data[0] & 0x0F >> 2);
garivetm 0:faf0960419c1 67 SDOmsg.index = CANmsg.data[1] + (CANmsg.data[2] << 8);
garivetm 0:faf0960419c1 68 SDOmsg.subindex = CANmsg.data[3];
garivetm 0:faf0960419c1 69 SDOmsg.data[0] = CANmsg.data[4];
garivetm 0:faf0960419c1 70 SDOmsg.data[1] = CANmsg.data[5];
garivetm 0:faf0960419c1 71 SDOmsg.data[2] = CANmsg.data[6];
garivetm 0:faf0960419c1 72 SDOmsg.data[3] = CANmsg.data[7];
garivetm 0:faf0960419c1 73 }