File content as of revision 7:33d64a3c48a2:

#ifndef ROBOTEQ_CONTROLLER_H
#define ROBOTEQ_CONTROLLER_H

#include "mbed.h"
#include "PeripheralCAN.h"
#include "object_dict.h"

// Client Command Specifier
#define CSS_CMD         2
#define CSS_QUERY       4
#define CSS_SUCCES      6
#define CSS_ERR         8

// CAN id
#define SDO_REQUEST    0x600
#define SDO_ANSWER     0x580
#define ID_TPDO1       0x180
#define ID_TPDO2       0x280
#define ID_TPDO3       0x380
#define ID_TPDO4       0x480

// Motor modes
#define MODE_OPEN_LOOP          0 // Open-loop
#define MODE_CLOSED_SPEED       1 // Closed-loop speed
#define MODE_CLOSED_POS_REL     2 // Closed-loop position relative
#define MODE_CLOSED_COUNT_POS   3 // Closed-loop count position
#define MODE_CLOSED_POS_TRACK   4 // Closed-loop position tracking
#define MODE_CLOSED_TORQUE      5 // Torque
#define MODE_CLOSED_SPEED_POS   6 // Closed-loop speed position

// Temperatures
#define TEMPERATURE_MCU         1   // Mcu temperature
#define TEMPERATURE_CH1         2   // Channel 1 heatsink
#define TEMPERATURE_CH2         3   // Channel 2 heatsink

class RoboteqController : public PeripheralCAN{
    public:
    /** Constructors and destructor
     */
    RoboteqController();
    
    /** Constructor
     *
     * @param controller A pointer on the CAN controller
     * @param node_id The controller node id
     * @param TPDO_enabled A flag that enables reception of TPDO messages
     */
    RoboteqController(ControllerCAN* controller, uint8_t node_id, bool TPDO_enabled);
    ~RoboteqController();
    
    /** Update callback by the CANController
     *
     * @param Id Id of the concerned data structure
     * @param msg CANMessage instance containing the new data
     */
    virtual void update(const unsigned short& Id, const CANMessage& msg);
    
    void sendCommand(uint8_t mot_num, uint32_t cmd);
    
    int16_t getSpeed(uint8_t mot_num); 
    
    protected:
    
    static const uint16_t RPDO_ID[4];   // RPDO msg IDs to send data to the controller
    
    typedef struct {
        uint8_t css;
        uint8_t n;
        uint16_t idx;
        uint8_t subindex;
        uint8_t data[4];
    } SDOMessage;
    /*  1> SDO CMD OR QUERY FRAME:
        -----------------------------------------------------------------------------
        Header   |   DLC   |                        Payload
        -----------------------------------------------------------------------------
                 |         |        Byte0             | Byte1-2 |  Byte 3  | Bytes4-7
                            ---------------------------------------------------------
        0x600+nd |    8    | bits 4-7 bits2-3 bits0-1 |         |          |
                           ---------------------------
                 |         |    css      n      xx    |  index  | subindex |   data
        Note:
            • nd is the destination node id.
            • ccs is the Client Command Specifier, if 2 it is command if 4 it is query.
            • n is the Number of bytes in the data part which do not contain data
            • xx not necessary for basic operation. For more details advise CANOpen standard.
            • index is the object dictionary index of the data to be accessed
            • subindex is the subindex of the object dictionary variable
            • data contains the data to be uploaded.
            
        2> SDO ANSWER FRAME:
        -----------------------------------------------------------------------------
        Header   |   DLC   |                        Payload
        -----------------------------------------------------------------------------
                 |         |        Byte0             | Byte1-2 |  Byte 3  | Bytes4-7
                            ---------------------------------------------------------
        0x580+nd |    8    | bits 4-7 bits2-3 bits0-1 |         |          |
                           ---------------------------
                 |         |    css      n      xx    |  index  | subindex |   data
        Note:
            • nd is the source node id.
            • ccs is the Client Command Specifier, if 4 it is query response, 6 it is a successful
            response to command, 8 is an error in message received.
            • n is the Number of bytes in the data part which do not contain data
            • xx not necessary for the simplistic way. For more details advise CANOpen standard.
            • index is the object dictionary index of the data to be accessed.
            • subindex is the subindex of the object dictionary variable
            • data contains the data to be uploaded. Applicable only if css=4.
    */
    void SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx);
    void SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data);
    
    /** Helper function that convert a CAN message to a SDO message
     *
     * @param CANmsg A reference to the CAN message
     * @param CANmsg A reference to the SDO message
     */
    void CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg);
    
    /** Send an RPDO message to the controller
     *
     * @param n The RPDO message number (from 1 to 4)
     * @param user_var1 The first signed 32-bit integer to be send (the first 4 bytes)
     * @param user_var2 The second signed 32-bit integer to be send (the last 4 bytes)
     */
    void RPDO_send(uint8_t n, int32_t user_var1, int32_t user_var2);
    
    /** Parse a TPDO message to user variables
     *
     * @param CANmsg A reference to the CAN message
     * @param user_var1 The first signed 32-bit integer to be read (the first 4 bytes)
     * @param user_var2 The second signed 32-bit integer to be read (the last 4 bytes)
     */
    void TPDO_parse(const CANMessage& CANmsg, int32_t* p_user_var1, int32_t* p_user_var2);
    
    /** Receive TPDO frames from the controller. If TPDO are used, this virtual
     * method must be redefined in a class that inherits from this one.
     *
     * @param n The TPDO message number (from 1 to 4)
     * @param CANmsg A reference to the CAN message
     */
    virtual void TPDO_receive(uint8_t n, const CANMessage& CANmsg);
    
    uint8_t node_id;                // Node id of the controller
    
    /* General Configuration and Safety */
    // TBD
    
    /* Analog, Digital, Pulse IO Configurations  */
    // TBD
    
    /* Motor configuration (non-exhaustiv list) */    
    uint16_t alim;                  // Motor ampere limit (0.1 A)
    uint8_t kp;                     // PID proportionnal gain
    uint8_t ki;                     // PID integral gain
    uint8_t kd;                     // PID derivative gain
    int32_t mac;                    // Motor acceleration rate (0.1 RPM)
    int32_t mdec;                   // Motor deceleration rate (0.1 RPM)
    uint8_t mmod;                   // Motor mode
    uint16_t mxrpm;                 // Maximal RPM value (eq to a cmd of 1000)
    
    int16_t amp_bat1;               // Battery current (0.1 A) Motor 1
    int16_t amp_bat2;               // Battery current (0.1 A) Motor 2
    int16_t amp_motor1;             // Motor current (0.1 A) Motor 1
    int16_t amp_motor2;             // Motor current (0.1 A) Motor 2
    int16_t volt_bat;               // Battery voltage
    int16_t feedback;               // Closed loop feedback
    int32_t error;                  // Closed loop error
    int8_t temp_MCU;                // MCU temperature
    int8_t temp_ch1;                // channel 1 temperature
    int8_t temp_ch2;                // channel 2 temperature
    int32_t vars[16];               // Integer user variables
    bool booleans[16];              // Boolean user variables
    int16_t speed1;                 // Rotor speed Motor 1
    int16_t speed2;                 // Rotor speed Motor 2
    
    /* CAN Identifiers */  
    unsigned short Id_CSS_REQ;      // CAN ID : CSS request (query or command)
    unsigned short Id_CSS_ANS;      // CAN ID : CSS answer (from query or command)
    unsigned short Id_TPDO1;        // CAN ID : TPDO1 message
    unsigned short Id_TPDO2;        // CAN ID : TPDO2 message
    unsigned short Id_TPDO3;        // CAN ID : TPDO3 message
    unsigned short Id_TPDO4;        // CAN ID : TPDO4 message
};

#endif