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@2:d61b4a989cab, 2018-05-17 (annotated)

Committer:
garivetm
Date:
Thu May 17 14:26:20 2018 +0000
Revision:
2:d61b4a989cab
Parent:
1:57b8f6ed930b
Child:
3:a03c91082856
Ajout des attributs moteur principaux

Who changed what in which revision?

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