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.h@7:33d64a3c48a2, 2018-06-25 (annotated)

Committer:
zephyrcare
Date:
Mon Jun 25 14:52:57 2018 +0000
Revision:
7:33d64a3c48a2
Parent:
6:f9c4795448c1 
Seuil de direction et d'acc?l?ration; offset direction et accel

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garivetm 0:faf0960419c1 1 #ifndef ROBOTEQ_CONTROLLER_H
garivetm 0:faf0960419c1 2 #define ROBOTEQ_CONTROLLER_H
garivetm 0:faf0960419c1 3
garivetm 0:faf0960419c1 4 #include "mbed.h"
garivetm 0:faf0960419c1 5 #include "PeripheralCAN.h"
garivetm 0:faf0960419c1 6 #include "object_dict.h"
garivetm 0:faf0960419c1 7
garivetm 0:faf0960419c1 8 // Client Command Specifier
garivetm 0:faf0960419c1 9 #define CSS_CMD 2
garivetm 0:faf0960419c1 10 #define CSS_QUERY 4
garivetm 0:faf0960419c1 11 #define CSS_SUCCES 6
garivetm 0:faf0960419c1 12 #define CSS_ERR 8
garivetm 0:faf0960419c1 13
garivetm 0:faf0960419c1 14 // CAN id
garivetm 0:faf0960419c1 15 #define SDO_REQUEST 0x600
garivetm 0:faf0960419c1 16 #define SDO_ANSWER 0x580
garivetm 3:a03c91082856 17 #define ID_TPDO1 0x180
garivetm 3:a03c91082856 18 #define ID_TPDO2 0x280
garivetm 3:a03c91082856 19 #define ID_TPDO3 0x380
garivetm 3:a03c91082856 20 #define ID_TPDO4 0x480
garivetm 0:faf0960419c1 21
garivetm 2:d61b4a989cab 22 // Motor modes
garivetm 2:d61b4a989cab 23 #define MODE_OPEN_LOOP 0 // Open-loop
garivetm 2:d61b4a989cab 24 #define MODE_CLOSED_SPEED 1 // Closed-loop speed
garivetm 2:d61b4a989cab 25 #define MODE_CLOSED_POS_REL 2 // Closed-loop position relative
garivetm 2:d61b4a989cab 26 #define MODE_CLOSED_COUNT_POS 3 // Closed-loop count position
garivetm 2:d61b4a989cab 27 #define MODE_CLOSED_POS_TRACK 4 // Closed-loop position tracking
garivetm 2:d61b4a989cab 28 #define MODE_CLOSED_TORQUE 5 // Torque
garivetm 2:d61b4a989cab 29 #define MODE_CLOSED_SPEED_POS 6 // Closed-loop speed position
garivetm 2:d61b4a989cab 30
garivetm 2:d61b4a989cab 31 // Temperatures
garivetm 2:d61b4a989cab 32 #define TEMPERATURE_MCU 1 // Mcu temperature
garivetm 2:d61b4a989cab 33 #define TEMPERATURE_CH1 2 // Channel 1 heatsink
garivetm 2:d61b4a989cab 34 #define TEMPERATURE_CH2 3 // Channel 2 heatsink
garivetm 2:d61b4a989cab 35
garivetm 0:faf0960419c1 36 class RoboteqController : public PeripheralCAN{
garivetm 0:faf0960419c1 37 public:
garivetm 0:faf0960419c1 38 /** Constructors and destructor
garivetm 0:faf0960419c1 39 */
garivetm 0:faf0960419c1 40 RoboteqController();
garivetm 3:a03c91082856 41
garivetm 3:a03c91082856 42 /** Constructor
garivetm 3:a03c91082856 43 *
garivetm 3:a03c91082856 44 * @param controller A pointer on the CAN controller
garivetm 3:a03c91082856 45 * @param node_id The controller node id
garivetm 3:a03c91082856 46 * @param TPDO_enabled A flag that enables reception of TPDO messages
garivetm 3:a03c91082856 47 */
zephyrcare 6:f9c4795448c1 48 RoboteqController(ControllerCAN* controller, uint8_t node_id, bool TPDO_enabled);
garivetm 0:faf0960419c1 49 ~RoboteqController();
garivetm 0:faf0960419c1 50
garivetm 0:faf0960419c1 51 /** Update callback by the CANController
garivetm 0:faf0960419c1 52 *
garivetm 0:faf0960419c1 53 * @param Id Id of the concerned data structure
garivetm 0:faf0960419c1 54 * @param msg CANMessage instance containing the new data
garivetm 0:faf0960419c1 55 */
zephyrcare 4:1ee0a235c997 56 virtual void update(const unsigned short& Id, const CANMessage& msg);
zephyrcare 4:1ee0a235c997 57
zephyrcare 6:f9c4795448c1 58 void sendCommand(uint8_t mot_num, uint32_t cmd);
garivetm 0:faf0960419c1 59
zephyrcare 7:33d64a3c48a2 60 int16_t getSpeed(uint8_t mot_num);
zephyrcare 7:33d64a3c48a2 61
garivetm 3:a03c91082856 62 protected:
garivetm 0:faf0960419c1 63
garivetm 3:a03c91082856 64 static const uint16_t RPDO_ID[4]; // RPDO msg IDs to send data to the controller
garivetm 3:a03c91082856 65
garivetm 0:faf0960419c1 66 typedef struct {
garivetm 0:faf0960419c1 67 uint8_t css;
garivetm 0:faf0960419c1 68 uint8_t n;
zephyrcare 4:1ee0a235c997 69 uint16_t idx;
garivetm 0:faf0960419c1 70 uint8_t subindex;
garivetm 0:faf0960419c1 71 uint8_t data[4];
garivetm 0:faf0960419c1 72 } SDOMessage;
garivetm 0:faf0960419c1 73 /* 1> SDO CMD OR QUERY FRAME:
garivetm 0:faf0960419c1 74 -----------------------------------------------------------------------------
garivetm 0:faf0960419c1 75 Header | DLC | Payload
garivetm 0:faf0960419c1 76 -----------------------------------------------------------------------------
garivetm 0:faf0960419c1 77 | | Byte0 | Byte1-2 | Byte 3 | Bytes4-7
garivetm 0:faf0960419c1 78 ---------------------------------------------------------
garivetm 0:faf0960419c1 79 0x600+nd | 8 | bits 4-7 bits2-3 bits0-1 | | |
garivetm 0:faf0960419c1 80 ---------------------------
garivetm 0:faf0960419c1 81 | | css n xx | index | subindex | data
garivetm 0:faf0960419c1 82 Note:
garivetm 0:faf0960419c1 83 • nd is the destination node id.
garivetm 0:faf0960419c1 84 • ccs is the Client Command Specifier, if 2 it is command if 4 it is query.
garivetm 0:faf0960419c1 85 • n is the Number of bytes in the data part which do not contain data
garivetm 0:faf0960419c1 86 • xx not necessary for basic operation. For more details advise CANOpen standard.
garivetm 0:faf0960419c1 87 • index is the object dictionary index of the data to be accessed
garivetm 0:faf0960419c1 88 • subindex is the subindex of the object dictionary variable
garivetm 0:faf0960419c1 89 • data contains the data to be uploaded.
garivetm 0:faf0960419c1 90
garivetm 0:faf0960419c1 91 2> SDO ANSWER FRAME:
garivetm 0:faf0960419c1 92 -----------------------------------------------------------------------------
garivetm 0:faf0960419c1 93 Header | DLC | Payload
garivetm 0:faf0960419c1 94 -----------------------------------------------------------------------------
garivetm 0:faf0960419c1 95 | | Byte0 | Byte1-2 | Byte 3 | Bytes4-7
garivetm 0:faf0960419c1 96 ---------------------------------------------------------
garivetm 0:faf0960419c1 97 0x580+nd | 8 | bits 4-7 bits2-3 bits0-1 | | |
garivetm 0:faf0960419c1 98 ---------------------------
garivetm 0:faf0960419c1 99 | | css n xx | index | subindex | data
garivetm 0:faf0960419c1 100 Note:
garivetm 0:faf0960419c1 101 • nd is the source node id.
garivetm 0:faf0960419c1 102 • ccs is the Client Command Specifier, if 4 it is query response, 6 it is a successful
garivetm 0:faf0960419c1 103 response to command, 8 is an error in message received.
garivetm 0:faf0960419c1 104 • n is the Number of bytes in the data part which do not contain data
garivetm 0:faf0960419c1 105 • xx not necessary for the simplistic way. For more details advise CANOpen standard.
garivetm 0:faf0960419c1 106 • index is the object dictionary index of the data to be accessed.
garivetm 0:faf0960419c1 107 • subindex is the subindex of the object dictionary variable
garivetm 0:faf0960419c1 108 • data contains the data to be uploaded. Applicable only if css=4.
garivetm 0:faf0960419c1 109 */
garivetm 0:faf0960419c1 110 void SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx);
garivetm 0:faf0960419c1 111 void SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data);
garivetm 3:a03c91082856 112
garivetm 3:a03c91082856 113 /** Helper function that convert a CAN message to a SDO message
garivetm 3:a03c91082856 114 *
garivetm 3:a03c91082856 115 * @param CANmsg A reference to the CAN message
garivetm 3:a03c91082856 116 * @param CANmsg A reference to the SDO message
garivetm 3:a03c91082856 117 */
garivetm 0:faf0960419c1 118 void CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg);
garivetm 0:faf0960419c1 119
garivetm 3:a03c91082856 120 /** Send an RPDO message to the controller
garivetm 3:a03c91082856 121 *
garivetm 3:a03c91082856 122 * @param n The RPDO message number (from 1 to 4)
garivetm 3:a03c91082856 123 * @param user_var1 The first signed 32-bit integer to be send (the first 4 bytes)
garivetm 3:a03c91082856 124 * @param user_var2 The second signed 32-bit integer to be send (the last 4 bytes)
garivetm 3:a03c91082856 125 */
garivetm 3:a03c91082856 126 void RPDO_send(uint8_t n, int32_t user_var1, int32_t user_var2);
garivetm 3:a03c91082856 127
garivetm 3:a03c91082856 128 /** Parse a TPDO message to user variables
garivetm 3:a03c91082856 129 *
garivetm 3:a03c91082856 130 * @param CANmsg A reference to the CAN message
garivetm 3:a03c91082856 131 * @param user_var1 The first signed 32-bit integer to be read (the first 4 bytes)
garivetm 3:a03c91082856 132 * @param user_var2 The second signed 32-bit integer to be read (the last 4 bytes)
garivetm 3:a03c91082856 133 */
zephyrcare 4:1ee0a235c997 134 void TPDO_parse(const CANMessage& CANmsg, int32_t* p_user_var1, int32_t* p_user_var2);
garivetm 3:a03c91082856 135
garivetm 3:a03c91082856 136 /** Receive TPDO frames from the controller. If TPDO are used, this virtual
garivetm 3:a03c91082856 137 * method must be redefined in a class that inherits from this one.
garivetm 3:a03c91082856 138 *
garivetm 3:a03c91082856 139 * @param n The TPDO message number (from 1 to 4)
garivetm 3:a03c91082856 140 * @param CANmsg A reference to the CAN message
garivetm 3:a03c91082856 141 */
garivetm 3:a03c91082856 142 virtual void TPDO_receive(uint8_t n, const CANMessage& CANmsg);
garivetm 3:a03c91082856 143
garivetm 0:faf0960419c1 144 uint8_t node_id; // Node id of the controller
garivetm 0:faf0960419c1 145
garivetm 2:d61b4a989cab 146 /* General Configuration and Safety */
garivetm 2:d61b4a989cab 147 // TBD
garivetm 2:d61b4a989cab 148
garivetm 2:d61b4a989cab 149 /* Analog, Digital, Pulse IO Configurations */
garivetm 2:d61b4a989cab 150 // TBD
garivetm 0:faf0960419c1 151
garivetm 2:d61b4a989cab 152 /* Motor configuration (non-exhaustiv list) */
garivetm 2:d61b4a989cab 153 uint16_t alim; // Motor ampere limit (0.1 A)
garivetm 2:d61b4a989cab 154 uint8_t kp; // PID proportionnal gain
garivetm 2:d61b4a989cab 155 uint8_t ki; // PID integral gain
garivetm 2:d61b4a989cab 156 uint8_t kd; // PID derivative gain
garivetm 2:d61b4a989cab 157 int32_t mac; // Motor acceleration rate (0.1 RPM)
garivetm 2:d61b4a989cab 158 int32_t mdec; // Motor deceleration rate (0.1 RPM)
garivetm 2:d61b4a989cab 159 uint8_t mmod; // Motor mode
garivetm 2:d61b4a989cab 160 uint16_t mxrpm; // Maximal RPM value (eq to a cmd of 1000)
garivetm 2:d61b4a989cab 161
zephyrcare 6:f9c4795448c1 162 int16_t amp_bat1; // Battery current (0.1 A) Motor 1
zephyrcare 6:f9c4795448c1 163 int16_t amp_bat2; // Battery current (0.1 A) Motor 2
zephyrcare 6:f9c4795448c1 164 int16_t amp_motor1; // Motor current (0.1 A) Motor 1
zephyrcare 6:f9c4795448c1 165 int16_t amp_motor2; // Motor current (0.1 A) Motor 2
zephyrcare 6:f9c4795448c1 166 int16_t volt_bat; // Battery voltage
garivetm 2:d61b4a989cab 167 int16_t feedback; // Closed loop feedback
garivetm 3:a03c91082856 168 int32_t error; // Closed loop error
garivetm 2:d61b4a989cab 169 int8_t temp_MCU; // MCU temperature
garivetm 3:a03c91082856 170 int8_t temp_ch1; // channel 1 temperature
garivetm 2:d61b4a989cab 171 int8_t temp_ch2; // channel 2 temperature
garivetm 2:d61b4a989cab 172 int32_t vars[16]; // Integer user variables
garivetm 2:d61b4a989cab 173 bool booleans[16]; // Boolean user variables
zephyrcare 6:f9c4795448c1 174 int16_t speed1; // Rotor speed Motor 1
zephyrcare 6:f9c4795448c1 175 int16_t speed2; // Rotor speed Motor 2
zephyrcare 6:f9c4795448c1 176
garivetm 2:d61b4a989cab 177 /* CAN Identifiers */
garivetm 0:faf0960419c1 178 unsigned short Id_CSS_REQ; // CAN ID : CSS request (query or command)
garivetm 0:faf0960419c1 179 unsigned short Id_CSS_ANS; // CAN ID : CSS answer (from query or command)
garivetm 3:a03c91082856 180 unsigned short Id_TPDO1; // CAN ID : TPDO1 message
garivetm 3:a03c91082856 181 unsigned short Id_TPDO2; // CAN ID : TPDO2 message
garivetm 3:a03c91082856 182 unsigned short Id_TPDO3; // CAN ID : TPDO3 message
garivetm 3:a03c91082856 183 unsigned short Id_TPDO4; // CAN ID : TPDO4 message
garivetm 0:faf0960419c1 184 };
garivetm 0:faf0960419c1 185
garivetm 0:faf0960419c1 186 #endif