RoboteqController.h

00001 #ifndef ROBOTEQ_CONTROLLER_H
00002 #define ROBOTEQ_CONTROLLER_H
00003 
00004 #include "mbed.h"
00005 #include "PeripheralCAN.h"
00006 #include "object_dict.h"
00007 
00008 // Client Command Specifier
00009 #define CSS_CMD         2
00010 #define CSS_QUERY       4
00011 #define CSS_SUCCES      6
00012 #define CSS_ERR         8
00013 
00014 // CAN id
00015 #define SDO_REQUEST    0x600
00016 #define SDO_ANSWER     0x580
00017 #define ID_TPDO1       0x180
00018 #define ID_TPDO2       0x280
00019 #define ID_TPDO3       0x380
00020 #define ID_TPDO4       0x480
00021 
00022 // Motor modes
00023 #define MODE_OPEN_LOOP          0 // Open-loop
00024 #define MODE_CLOSED_SPEED       1 // Closed-loop speed
00025 #define MODE_CLOSED_POS_REL     2 // Closed-loop position relative
00026 #define MODE_CLOSED_COUNT_POS   3 // Closed-loop count position
00027 #define MODE_CLOSED_POS_TRACK   4 // Closed-loop position tracking
00028 #define MODE_CLOSED_TORQUE      5 // Torque
00029 #define MODE_CLOSED_SPEED_POS   6 // Closed-loop speed position
00030 
00031 // Temperatures
00032 #define TEMPERATURE_MCU         1   // Mcu temperature
00033 #define TEMPERATURE_CH1         2   // Channel 1 heatsink
00034 #define TEMPERATURE_CH2         3   // Channel 2 heatsink
00035 
00036 class RoboteqController : public PeripheralCAN{
00037     public:
00038     /** Constructors and destructor
00039      */
00040     RoboteqController();
00041     
00042     /** Constructor
00043      *
00044      * @param controller A pointer on the CAN controller
00045      * @param node_id The controller node id
00046      * @param TPDO_enabled A flag that enables reception of TPDO messages
00047      */
00048     RoboteqController(ControllerCAN* controller, uint8_t node_id, bool TPDO_enabled);
00049     ~RoboteqController();
00050     
00051     /** Update callback by the CANController
00052      *
00053      * @param Id Id of the concerned data structure
00054      * @param msg CANMessage instance containing the new data
00055      */
00056     virtual void update(const unsigned short& Id, const CANMessage& msg);
00057     
00058     void sendCommand(uint8_t mot_num, uint32_t cmd);
00059     
00060     int16_t getSpeed(uint8_t mot_num); 
00061     
00062     protected:
00063     
00064     static const uint16_t RPDO_ID[4];   // RPDO msg IDs to send data to the controller
00065     
00066     typedef struct {
00067         uint8_t css;
00068         uint8_t n;
00069         uint16_t idx;
00070         uint8_t subindex;
00071         uint8_t data[4];
00072     } SDOMessage;
00073     /*  1> SDO CMD OR QUERY FRAME:
00074         -----------------------------------------------------------------------------
00075         Header   |   DLC   |                        Payload
00076         -----------------------------------------------------------------------------
00077                  |         |        Byte0             | Byte1-2 |  Byte 3  | Bytes4-7
00078                             ---------------------------------------------------------
00079         0x600+nd |    8    | bits 4-7 bits2-3 bits0-1 |         |          |
00080                            ---------------------------
00081                  |         |    css      n      xx    |  index  | subindex |   data
00082         Note:
00083             • nd is the destination node id.
00084             • ccs is the Client Command Specifier, if 2 it is command if 4 it is query.
00085             • n is the Number of bytes in the data part which do not contain data
00086             • xx not necessary for basic operation. For more details advise CANOpen standard.
00087             • index is the object dictionary index of the data to be accessed
00088             • subindex is the subindex of the object dictionary variable
00089             • data contains the data to be uploaded.
00090             
00091         2> SDO ANSWER FRAME:
00092         -----------------------------------------------------------------------------
00093         Header   |   DLC   |                        Payload
00094         -----------------------------------------------------------------------------
00095                  |         |        Byte0             | Byte1-2 |  Byte 3  | Bytes4-7
00096                             ---------------------------------------------------------
00097         0x580+nd |    8    | bits 4-7 bits2-3 bits0-1 |         |          |
00098                            ---------------------------
00099                  |         |    css      n      xx    |  index  | subindex |   data
00100         Note:
00101             • nd is the source node id.
00102             • ccs is the Client Command Specifier, if 4 it is query response, 6 it is a successful
00103             response to command, 8 is an error in message received.
00104             • n is the Number of bytes in the data part which do not contain data
00105             • xx not necessary for the simplistic way. For more details advise CANOpen standard.
00106             • index is the object dictionary index of the data to be accessed.
00107             • subindex is the subindex of the object dictionary variable
00108             • data contains the data to be uploaded. Applicable only if css=4.
00109     */
00110     void SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx);
00111     void SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data);
00112     
00113     /** Helper function that convert a CAN message to a SDO message
00114      *
00115      * @param CANmsg A reference to the CAN message
00116      * @param CANmsg A reference to the SDO message
00117      */
00118     void CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg);
00119     
00120     /** Send an RPDO message to the controller
00121      *
00122      * @param n The RPDO message number (from 1 to 4)
00123      * @param user_var1 The first signed 32-bit integer to be send (the first 4 bytes)
00124      * @param user_var2 The second signed 32-bit integer to be send (the last 4 bytes)
00125      */
00126     void RPDO_send(uint8_t n, int32_t user_var1, int32_t user_var2);
00127     
00128     /** Parse a TPDO message to user variables
00129      *
00130      * @param CANmsg A reference to the CAN message
00131      * @param user_var1 The first signed 32-bit integer to be read (the first 4 bytes)
00132      * @param user_var2 The second signed 32-bit integer to be read (the last 4 bytes)
00133      */
00134     void TPDO_parse(const CANMessage& CANmsg, int32_t* p_user_var1, int32_t* p_user_var2);
00135     
00136     /** Receive TPDO frames from the controller. If TPDO are used, this virtual
00137      * method must be redefined in a class that inherits from this one.
00138      *
00139      * @param n The TPDO message number (from 1 to 4)
00140      * @param CANmsg A reference to the CAN message
00141      */
00142     virtual void TPDO_receive(uint8_t n, const CANMessage& CANmsg);
00143     
00144     uint8_t node_id;                // Node id of the controller
00145     
00146     /* General Configuration and Safety */
00147     // TBD
00148     
00149     /* Analog, Digital, Pulse IO Configurations  */
00150     // TBD
00151     
00152     /* Motor configuration (non-exhaustiv list) */    
00153     uint16_t alim;                  // Motor ampere limit (0.1 A)
00154     uint8_t kp;                     // PID proportionnal gain
00155     uint8_t ki;                     // PID integral gain
00156     uint8_t kd;                     // PID derivative gain
00157     int32_t mac;                    // Motor acceleration rate (0.1 RPM)
00158     int32_t mdec;                   // Motor deceleration rate (0.1 RPM)
00159     uint8_t mmod;                   // Motor mode
00160     uint16_t mxrpm;                 // Maximal RPM value (eq to a cmd of 1000)
00161     
00162     int16_t amp_bat1;               // Battery current (0.1 A) Motor 1
00163     int16_t amp_bat2;               // Battery current (0.1 A) Motor 2
00164     int16_t amp_motor1;             // Motor current (0.1 A) Motor 1
00165     int16_t amp_motor2;             // Motor current (0.1 A) Motor 2
00166     int16_t volt_bat;               // Battery voltage
00167     int16_t feedback;               // Closed loop feedback
00168     int32_t error;                  // Closed loop error
00169     int8_t temp_MCU;                // MCU temperature
00170     int8_t temp_ch1;                // channel 1 temperature
00171     int8_t temp_ch2;                // channel 2 temperature
00172     int32_t vars[16];               // Integer user variables
00173     bool booleans[16];              // Boolean user variables
00174     int16_t speed1;                 // Rotor speed Motor 1
00175     int16_t speed2;                 // Rotor speed Motor 2
00176     
00177     /* CAN Identifiers */  
00178     unsigned short Id_CSS_REQ;      // CAN ID : CSS request (query or command)
00179     unsigned short Id_CSS_ANS;      // CAN ID : CSS answer (from query or command)
00180     unsigned short Id_TPDO1;        // CAN ID : TPDO1 message
00181     unsigned short Id_TPDO2;        // CAN ID : TPDO2 message
00182     unsigned short Id_TPDO3;        // CAN ID : TPDO3 message
00183     unsigned short Id_TPDO4;        // CAN ID : TPDO4 message
00184 };
00185 
00186 #endif