RobotSimulation/pick_and_place/ROS/src/niryo_one_ros/mcp_can_rpi/src/mcp_can_rpi.cpp
INT8U MCP_CAN::mcp2515_init(const INT8U canIDMode, const INT8U canSpeed, const INT8U canClock)
{

  INT8U res;

    mcp2515_reset();
    
    mcpMode = MCP_LOOPBACK;

    res = mcp2515_setCANCTRL_Mode(MODE_CONFIG);
    if(res > 0)
    {
#if DEBUG_MODE
      printf("Entering Configuration Mode Failure...\r\n"); 
#endif
      return res;
    }
#if DEBUG_MODE
    printf("Entering Configuration Mode Successful!\r\n");
#endif

    // Set Baudrate
    if(mcp2515_configRate(canSpeed, canClock))
    {
#if DEBUG_MODE
      printf("Setting Baudrate Failure...\r\n");
#endif
      return res;
    }
#if DEBUG_MODE
    printf("Setting Baudrate Successful!\r\n");
#endif

    if ( res == MCP2515_OK ) {

                                                                        /* init canbuffers              */
        mcp2515_initCANBuffers();

                                                                        /* interrupt mode               */
        mcp2515_setRegister(MCP_CANINTE, MCP_RX0IF | MCP_RX1IF);

        switch(canIDMode)
        {
            case (MCP_ANY):
            mcp2515_modifyRegister(MCP_RXB0CTRL,
            MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK,
            MCP_RXB_RX_ANY | MCP_RXB_BUKT_MASK);
            mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
            MCP_RXB_RX_ANY);
            break;
/*          The followingn two functions of the MCP2515 do not work, there is a bug in the silicon.
            case (MCP_STD): 
            mcp2515_modifyRegister(MCP_RXB0CTRL,
            MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK,
            MCP_RXB_RX_STD | MCP_RXB_BUKT_MASK );
            mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
            MCP_RXB_RX_STD);
            break;
            case (MCP_EXT): 
            mcp2515_modifyRegister(MCP_RXB0CTRL,
            MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK,
            MCP_RXB_RX_EXT | MCP_RXB_BUKT_MASK );
            mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
            MCP_RXB_RX_EXT);
            break;
*/
            case (MCP_STDEXT): 
            mcp2515_modifyRegister(MCP_RXB0CTRL,
            MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK,
            MCP_RXB_RX_STDEXT | MCP_RXB_BUKT_MASK );
            mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
            MCP_RXB_RX_STDEXT);
            break;
    
            default:
#if DEBUG_MODE        
            printf("`Setting ID Mode Failure...\r\n");
#endif           
            return MCP2515_FAIL;
            break;
}    


        res = mcp2515_setCANCTRL_Mode(mcpMode);                                                                
        if(res)
        {
#if DEBUG_MODE        
          printf("Returning to Previous Mode Failure...\r\n");
#endif           
          return res;
        }

    }
    return res;

}
/*********************************************************************************************************
** Function name:           mcp2515_init
** Descriptions:            Initialize the controller
*********************************************************************************************************/
Function name:           mcp2515_init
Descriptions:            Initialize the controller
Function name : mcp2515_init Descriptions : Initialize the controller
INT8U MCP_CAN::mcp2515_init(const INT8U canIDMode, const INT8U canSpeed, const INT8U canClock)
{
  INT8U res;
    mcp2515_reset();
    mcpMode = MCP_LOOPBACK;
    res = mcp2515_setCANCTRL_Mode(MODE_CONFIG);
    if(res > 0)
    {
#if DEBUG_MODE
      printf("Entering Configuration Mode Failure...\r\n"); 
#endif
      return res;
    }
#if DEBUG_MODE
    printf("Entering Configuration Mode Successful!\r\n");
#endif
    if(mcp2515_configRate(canSpeed, canClock))
    {
#if DEBUG_MODE
      printf("Setting Baudrate Failure...\r\n");
#endif
      return res;
    }
#if DEBUG_MODE
    printf("Setting Baudrate Successful!\r\n");
#endif
    if ( res == MCP2515_OK ) {
        mcp2515_initCANBuffers();
        mcp2515_setRegister(MCP_CANINTE, MCP_RX0IF | MCP_RX1IF);
        switch(canIDMode)
        {
            case (MCP_ANY):
            mcp2515_modifyRegister(MCP_RXB0CTRL,
            MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK,
            MCP_RXB_RX_ANY | MCP_RXB_BUKT_MASK);
            mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
            MCP_RXB_RX_ANY);
            break;
            case (MCP_STDEXT): 
            mcp2515_modifyRegister(MCP_RXB0CTRL,
            MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK,
            MCP_RXB_RX_STDEXT | MCP_RXB_BUKT_MASK );
            mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
            MCP_RXB_RX_STDEXT);
            break;
            default:
#if DEBUG_MODE        
            printf("`Setting ID Mode Failure...\r\n");
#endif           
            return MCP2515_FAIL;
            break;
}    
        res = mcp2515_setCANCTRL_Mode(mcpMode);                                                                
        if(res)
        {
#if DEBUG_MODE        
          printf("Returning to Previous Mode Failure...\r\n");
#endif           
          return res;
        }
    }
    return res;
}
INT8U MCP_CAN :: mcp2515_init ( const INT8U canIDMode , const INT8U canSpeed , const INT8U canClock ) { INT8U res ; mcp2515_reset ( ) ; mcpMode = MCP_LOOPBACK ; res = mcp2515_setCANCTRL_Mode ( MODE_CONFIG ) ; if ( res > 0 ) { #if DEBUG_MODE printf ( " \r \n " ) ; #endif return res ; } #if DEBUG_MODE printf ( " \r \n " ) ; #endif if ( mcp2515_configRate ( canSpeed , canClock ) ) { #if DEBUG_MODE printf ( " \r \n " ) ; #endif return res ; } #if DEBUG_MODE printf ( " \r \n " ) ; #endif if ( res == MCP2515_OK ) { mcp2515_initCANBuffers ( ) ; mcp2515_setRegister ( MCP_CANINTE , MCP_RX0IF | MCP_RX1IF ) ; switch ( canIDMode ) { case ( MCP_ANY ) : mcp2515_modifyRegister ( MCP_RXB0CTRL , MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK , MCP_RXB_RX_ANY | MCP_RXB_BUKT_MASK ) ; mcp2515_modifyRegister ( MCP_RXB1CTRL , MCP_RXB_RX_MASK , MCP_RXB_RX_ANY ) ; break ; case ( MCP_STDEXT ) : mcp2515_modifyRegister ( MCP_RXB0CTRL , MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK , MCP_RXB_RX_STDEXT | MCP_RXB_BUKT_MASK ) ; mcp2515_modifyRegister ( MCP_RXB1CTRL , MCP_RXB_RX_MASK , MCP_RXB_RX_STDEXT ) ; break ; default : #if DEBUG_MODE printf ( " \r \n " ) ; #endif return MCP2515_FAIL ; break ; } res = mcp2515_setCANCTRL_Mode ( mcpMode ) ; if ( res ) { #if DEBUG_MODE printf ( " \r \n " ) ; #endif return res ; } } return res ; }
Function name:           mcp2515_init
Descriptions:            Initialize the controller
Function name : mcp2515_init Descriptions : Initialize the controller
// Set Baudrate /* init canbuffers              */ /* interrupt mode               */ /*          The followingn two functions of the MCP2515 do not work, there is a bug in the silicon.
            case (MCP_STD): 
            mcp2515_modifyRegister(MCP_RXB0CTRL,
            MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK,
            MCP_RXB_RX_STD | MCP_RXB_BUKT_MASK );
            mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
            MCP_RXB_RX_STD);
            break;
            case (MCP_EXT): 
            mcp2515_modifyRegister(MCP_RXB0CTRL,
            MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK,
            MCP_RXB_RX_EXT | MCP_RXB_BUKT_MASK );
            mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
            MCP_RXB_RX_EXT);
            break;
*/