this_node.c

Go to the documentation of this file.

//
// © 2010 Shadow Robot Company Limited.
//
// FileName:        this_node.c
// Dependencies:    
// Processor:       PIC32
// Compiler:        MPLAB® C32 
//
//  +------------------------------------------------------------------------+
//  | This file is part of The Shadow Robot PIC32 firmware code base.        |
//  |                                                                        |
//  | It is free software: you can redistribute it and/or modify             |
//  | it under the terms of the GNU General Public License as published by   |
//  | the Free Software Foundation, either version 3 of the License, or      |
//  | (at your option) any later version.                                    |
//  |                                                                        |
//  | It is distributed in the hope that it will be useful,                  |
//  | but WITHOUT ANY WARRANTY; without even the implied warranty of         |
//  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          |
//  | GNU General Public License for more details.                           |
//  |                                                                        |
//  | You should have received a copy of the GNU General Public License      |
//  | along with this code repository. The text of the license can be found  |
//  | in Pic32/License/gpl.txt. If not, see <http://www.gnu.org/licenses/>.  |
//  +------------------------------------------------------------------------+
//
//
//
//
//  Doxygen
//  -------
//
//
//

#include "this_node.h"
#include "tests/assert_shadow.h"
#include "internal_reporting/internal_reporting.h"
#include "hardware/et1200/et1200_interface.h"
#include <peripheral/spi.h>

int64u      node_id                  = 0;
int32u      global_AL_Event_Register = 0;

TACTILE_SENSOR_PROTOCOL_TYPE    tactile_sensor_protocol = TACTILE_SENSOR_PROTOCOL_TYPE_INVALID;

ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND   etherCAT_command_data;              
ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS    etherCAT_status_data;               

ETHERCAT_CAN_BRIDGE_DATA                        can_bridge_data_from_ROS;
ETHERCAT_CAN_BRIDGE_DATA                        can_bridge_data_to_ROS;


#define CORE_TIMER_TO_MICROSECONDS(x) ((x) / (SYSTEM_FREQ_HZ/2000000))              //!< Convert from the number of core ticks returned by ReadCoreTimer to a real time in microseconds.



#ifdef PALM_PCB_00
    int8u palm_EDC_0200_sensor_mapping[64] = {IGNORE, IGNORE, IGNORE, IGNORE,  FFJ4,   THJ3,   THJ1,    IGNORE,           // Channel 0
                                              IGNORE, IGNORE, IGNORE, IGNORE,  MFJ4,   THJ4,   THJ2,    IGNORE,           // Channel 1
                                              FFJ2  , MFJ2  , RFJ2  , LFJ2  ,  RFJ4,   AN0,    IGNORE,  IGNORE,           // Channel 2
                                              FFJ1  , MFJ1  , RFJ1  , LFJ1  ,  THJ5B,  THJ5A,  IGNORE,  IGNORE,           // Channel 3
                                              IGNORE, IGNORE, IGNORE, IGNORE,  WRJ1B,  WRJ1A,  IGNORE,  IGNORE,           // Channel 4
                                              IGNORE, IGNORE, IGNORE, IGNORE,  AN1,    LFJ4,   IGNORE,  IGNORE,           // Channel 5
                                              FFJ3  , MFJ3  , RFJ3  , LFJ3  ,  AN2,    LFJ5,   IGNORE,  IGNORE,           // Channel 6
                                              IGNORE, IGNORE, IGNORE, IGNORE,  AN3,    WRJ2,   IGNORE,  IGNORE            // Channel 7
                                             };
#endif



#ifdef PALM_PCB_01    
    int8u palm_EDC_0200_sensor_mapping[64] = {IGNORE, IGNORE, IGNORE, IGNORE,  FFJ4,   THJ4,   IGNORE,  IGNORE,           // Channel 0
                                              IGNORE, IGNORE, IGNORE, IGNORE,  MFJ4,   THJ3,   IGNORE,  IGNORE,           // Channel 1
                                              RFJ2  , MFJ2  , FFJ2  , LFJ2  ,  RFJ4,   AN0,    THJ1,    IGNORE,           // Channel 2
                                              RFJ1  , MFJ1  , FFJ1  , LFJ1  ,  THJ5B,  THJ5A,  IGNORE,  IGNORE,           // Channel 3
                                              IGNORE, IGNORE, IGNORE, IGNORE,  AN3,    WRJ2,   THJ2,    IGNORE,           // Channel 4
                                              IGNORE, IGNORE, IGNORE, IGNORE,  AN2,    LFJ4,   IGNORE,  IGNORE,           // Channel 5
                                              RFJ3  , MFJ3  , FFJ3  , LFJ3  ,  AN1,    LFJ5,   IGNORE,  IGNORE,           // Channel 6
                                              IGNORE, IGNORE, IGNORE, IGNORE,  WRJ1A,  WRJ1B,  IGNORE,  IGNORE            // Channel 7
                                             };
#endif



int32u frame_start_time = 0;                
int32u idle_time_start  = 0;                




LED_Class *LED_CAN1_TX;                     
LED_Class *LED_CAN1_RX;                     
LED_Class *LED_CAN1_ERR;                    

LED_Class *LED_CAN2_TX;                     
LED_Class *LED_CAN2_ERR;                    

LED_Class *ET1200_AL_err_LED;               


#ifdef LED_CFG_PIN
  LED_Class  *LED_CFG;                      


#endif


#ifdef LED_CAN2_RX_PIN
    LED_Class *LED_CAN2_RX;                 


#endif


int num_motor_CAN_messages_received_this_frame = 0;     



void run_tests(void)
{
    typedef_tests();
    simple_CAN_tests();
}



void approx_1ms_handler(void)
{
    Handle_LEDs();

    #if AUTO_TRIGGER == 1
        etherCAT_command_data.EDC_command = EDC_COMMAND_SENSOR_DATA;
        Read_All_Sensors();
    #endif

    ClearWDT();
}



void initialise_this_node(void)
{
    run_tests();                                                                            // Very important


                                                                                            // Initialise all of the I/O pins and peripherals.
    Output_Pin_Class *ET1200_chip_select    = Get_Output_Pin(ET1200_CHIP_SELECT_PIN);
    Output_Pin_Class *ET1200_reset          = Get_Output_Pin(ET1200_RESET_PIN);
    Input_Pin_Class  *ET1200_eeprom         = Get_Input_Pin (ET1200_EEPROM_PIN);

    SPI_simple_master_class   *SPI_port     = Init_SPI_Simple_Master(SPI_PORT);
    I2C_simple_master_class   *I2C_port     = Init_I2C_Simple_Master(I2C_PORT);

    Output_Pin_Class *SPI_cs                = Get_Output_Pin(SPI_CS_PIN);
    Output_Pin_Class *SPI_clock             = Get_Output_Pin(SPI_CLOCK_PIN);
    Output_Pin_Class *SPI_mosi              = Get_Output_Pin(SPI_MOSI_PIN);
    Input_Pin_Class  *ET1200_somi           = Get_Input_Pin (ET1200_SOMI_PIN);


    Output_Pin_Class *LED_pin_CAN1_TX       = Get_Output_Pin(LED_CAN1_TX_PIN);
    Output_Pin_Class *LED_pin_CAN1_RX       = Get_Output_Pin(LED_CAN1_RX_PIN);
    Output_Pin_Class *LED_pin_CAN1_ERR      = Get_Output_Pin(LED_CAN1_ERR_PIN);

    Output_Pin_Class *LED_pin_CAN2_TX       = Get_Output_Pin(LED_CAN2_TX_PIN);
    Output_Pin_Class *LED_pin_CAN2_ERR      = Get_Output_Pin(LED_CAN2_ERR_PIN);

    Output_Pin_Class *LED_pin_AL_ERR        = Get_Output_Pin(LED_AL_ERR_PIN);

    Output_Pin_Class *accel_CS              = Get_Output_Pin(ACCEL_CS_PIN);
    Pin_Set(accel_CS);
    
    LED_CAN1_TX                             = Get_LED(LED_pin_CAN1_TX);
    LED_CAN1_RX                             = Get_LED(LED_pin_CAN1_RX);
    LED_CAN1_ERR                            = Get_LED(LED_pin_CAN1_ERR);

    LED_CAN2_TX                             = Get_LED(LED_pin_CAN2_TX);
    LED_CAN2_ERR                            = Get_LED(LED_pin_CAN2_ERR);

    ET1200_AL_err_LED                       = Get_LED(LED_pin_AL_ERR);

    #ifdef LED_CFG_PIN
        Output_Pin_Class *LED_pin_CFG       = Get_Output_Pin(LED_CFG_PIN);
        LED_CFG                             = Get_LED(LED_pin_CFG);
    #endif

    #ifdef LED_CAN2_RX_PIN
        Output_Pin_Class *LED_pin_CAN2_RX   = Get_Output_Pin(LED_CAN2_RX_PIN);
        LED_CAN2_RX                         = Get_LED(LED_pin_CAN2_RX);
    #endif


    AD1PCFG = 0xFFFF;                                                                       // Set all pins to Digital, not analog.
    U1CON   = 0x0000;                                                                       // Disable USB


    ET1200_Interface_Initialise(SPI_port, I2C_port, ET1200_reset, ET1200_AL_err_LED, ET1200_eeprom, ET1200_somi, ET1200_chip_select);

    #if AUTO_TRIGGER == 0
        ET1200_Initialise();
        write_ET1200_register_32u(0x204, 0x100);                                            //set AL event mask to get IRQ only on SyncMan0 activity
    #endif


    init_dual_CAN();




    {                                                                                       // Initialise the MOSI bits for each ADC channel
        Input_Pins_Class *SPI_SOMI_pins      = Get_Input_Pins('E', 0, 8);
    
        Initialise_SPI_Parallel(SPI_cs, SPI_clock, SPI_mosi, SPI_SOMI_pins);
        SPIP_Setup_MOSI_Bits(0, 19, 0xC0000000);        // 1100 0000
        SPIP_Setup_MOSI_Bits(1, 19, 0xC8000000);        // 1100 1000
        SPIP_Setup_MOSI_Bits(2, 19, 0xD0000000);        // 1101 0000
        SPIP_Setup_MOSI_Bits(3, 19, 0xD8000000);        // 1101 1000
        SPIP_Setup_MOSI_Bits(4, 19, 0xE0000000);        // 1110 0000
        SPIP_Setup_MOSI_Bits(5, 19, 0xE8000000);        // 1110 1000
        SPIP_Setup_MOSI_Bits(6, 19, 0xF0000000);        // 1111 0000
        SPIP_Setup_MOSI_Bits(7, 19, 0xF8000000);        // 1111 1000
    }


    tactile_sensor_protocol = biotac_autodetect(tactile_sensor_protocol);                   // Auto-detect the tactile sensor type.
    tactile_sensor_protocol =    pst_autodetect(tactile_sensor_protocol);                   // Result is stored in tactile_sensor_protocol.

    switch (tactile_sensor_protocol)                                                        // Now initialise the tactile sensors according to the detected type.
    {
        case TACTILE_SENSOR_PROTOCOL_TYPE_PST3:             init_pst();         break;      // PST3     (new type with normal CS)
        case TACTILE_SENSOR_PROTOCOL_TYPE_BIOTAC_2_3:       init_biotac();      break;      // BioTac   (Syntouch)

        default:                                                                break;
    }


}



void Read_Commands_From_ET1200(void)
{
    assert_dynamic(ETHERCAT_COMMAND_DATA_SIZE > 0);
    //read_ET1200_register_N(EC_PALM_EDC_COMMAND_PHY_BASE, ETHERCAT_COMMAND_DATA_SIZE, (int8u*)(&etherCAT_command_data));
    read_ET1200_register_N( ETHERCAT_COMMAND_DATA_ADDRESS,
                            ETHERCAT_COMMAND_DATA_SIZE,
                            (int8u*)(&etherCAT_command_data)  );
}



inline void write_status_data_To_ET1200(void)
{
    write_ET1200_register_N(ETHERCAT_STATUS_DATA_ADDRESS, ETHERCAT_STATUS_DATA_SIZE, (int8u*)(&etherCAT_status_data));
}



int8u There_is_Command_From_ET1200(void)
{
    global_AL_Event_Register = read_ET1200_register_32u(0x220);
    return ( ( global_AL_Event_Register & ECREG32_AL_EVENT_BIT_SYNCMANAGER_0_INTERRUPT ) != 0) ;
}



int8u ROS_Wants_me_to_send_CAN(void)
{
    return ( ( global_AL_Event_Register & ECREG32_AL_EVENT_BIT_SYNCMANAGER_1_INTERRUPT ) != 0) ;
}



int8u not_all_motor_data_received(void)
{
    return num_motor_CAN_messages_received_this_frame < 10;
}



int32u get_frame_time_us(void)
{
    return (ReadCoreTimer() - frame_start_time) / (SYSTEM_FREQ_HZ/2000000);
}



void Wait_For_All_Motors_To_Send_Data(int32u timeout_us)
{
    do
    {
        collect_motor_data_CAN_messages(CAN1);
        collect_motor_data_CAN_messages(CAN2);
    }
    while (  (not_all_motor_data_received())
          && (get_frame_time_us() < timeout_us)
          );
}



void Wait_For_Until_Frame_Time(int32u frame_time_us)
{
    while (get_frame_time_us() <= frame_time_us)
    {
    }
}



int16u calculate_idle_time(void)
{
    return CORE_TIMER_TO_MICROSECONDS(frame_start_time - idle_time_start);
}




void Read_All_Sensors(void)
{
    int8u adc_channel;
    int8u j=0;
    int32u frame_time;

    Nop();
    Nop();

    for (j=0; j<SENSORS_NUM_0220; j++)
        etherCAT_status_data.sensors[j] = j;

    switch (tactile_sensor_protocol)                                                // Read tactile sensors before joint sensors?
    {                                                                               // ------------------------------------------
        case TACTILE_SENSOR_PROTOCOL_TYPE_PST3:
            break;

        case TACTILE_SENSOR_PROTOCOL_TYPE_BIOTAC_2_3:                               // BioTac Pac needs to be read once before joint sensors.
            frame_time = get_frame_time_us();
            biotac_read_pac(0, &etherCAT_command_data, &etherCAT_status_data);
            break;

        default:
            break;
    }


    j=0;
    for (adc_channel = 0; adc_channel < 8; ++adc_channel)                           // Now read joint sensors.
    {                                                                               // -----------------------
        switch (etherCAT_command_data.EDC_command)
        {
            case EDC_COMMAND_SENSOR_DATA: 
                SPIP_max_bits = 19;
                SPIP_Xceive_Fast(adc_channel);
                break;

            case EDC_COMMAND_SENSOR_CHANNEL_NUMBERS:
                SPIP_PutChannel_Numbers(adc_channel);
                break;

            default: 
                SPIP_PutChannel_Numbers(adc_channel);
                break;
        }

        etherCAT_status_data.sensors[ palm_EDC_0200_sensor_mapping[j++] ] = TRANSLATE_SOMI_MCP3208(0);
        etherCAT_status_data.sensors[ palm_EDC_0200_sensor_mapping[j++] ] = TRANSLATE_SOMI_MCP3208(1);
        etherCAT_status_data.sensors[ palm_EDC_0200_sensor_mapping[j++] ] = TRANSLATE_SOMI_MCP3208(2);
        etherCAT_status_data.sensors[ palm_EDC_0200_sensor_mapping[j++] ] = TRANSLATE_SOMI_MCP3208(3);
        etherCAT_status_data.sensors[ palm_EDC_0200_sensor_mapping[j++] ] = TRANSLATE_SOMI_MCP3208(4);
        etherCAT_status_data.sensors[ palm_EDC_0200_sensor_mapping[j++] ] = TRANSLATE_SOMI_MCP3208(5);
        etherCAT_status_data.sensors[ palm_EDC_0200_sensor_mapping[j++] ] = TRANSLATE_SOMI_MCP3208(6);
        etherCAT_status_data.sensors[ palm_EDC_0200_sensor_mapping[j++] ] = TRANSLATE_SOMI_MCP3208(7);
    }

    #if AUTO_TRIGGER == 1
        etherCAT_command_data.tactile_data_type = TACTILE_SENSOR_TYPE_BIOTAC_PDC;
    #endif


    switch (tactile_sensor_protocol)                                                // Read tactile sensors after joint sensors?
    {                                                                               // -----------------------------------------

        case TACTILE_SENSOR_PROTOCOL_TYPE_PST3:                                     // PST3s are read after
            read_PSTs(&etherCAT_command_data, &etherCAT_status_data);
            break;

        case TACTILE_SENSOR_PROTOCOL_TYPE_BIOTAC_2_3:                               // BioTac: Hall, Misc, and Pac again are read after joint sensors.
            biotac_read_sensors(&etherCAT_command_data, &etherCAT_status_data);

            Wait_For_All_Motors_To_Send_Data(frame_time+490);                       // Use this delay time productively
            Wait_For_Until_Frame_Time(frame_time+500);                              // Now that we're close to the time, use a more accurate wait routine.

            biotac_read_pac(1, &etherCAT_command_data, &etherCAT_status_data);      // Read the Pac again at 500us to achieve 2000Hz.
            break;

        default:
            break;
    }


    if (etherCAT_command_data.tactile_data_type == TACTILE_SENSOR_TYPE_WHICH_SENSORS)   // Is the host asking which tactile sensor protocol we're using?
    {
        etherCAT_status_data.tactile[0].word[0] = tactile_sensor_protocol;              // Then tell it!
        etherCAT_status_data.tactile[1].word[0] = tactile_sensor_protocol;
        etherCAT_status_data.tactile[2].word[0] = tactile_sensor_protocol;
        etherCAT_status_data.tactile[3].word[0] = tactile_sensor_protocol;
        etherCAT_status_data.tactile[4].word[0] = tactile_sensor_protocol;
        etherCAT_status_data.tactile_data_valid = 0x001F;
        etherCAT_status_data.tactile_data_type = TACTILE_SENSOR_TYPE_WHICH_SENSORS;
    }
}



void zero_motor_data_packets(void)
{
    int i;

    for (i=0; i<9; i++)
    {
        etherCAT_status_data.motor_data_packet[i].torque = 0;
        etherCAT_status_data.motor_data_packet[i].misc   = 0;
    }

    etherCAT_status_data.which_motor_data_arrived    = 0x00000000;
    etherCAT_status_data.which_motor_data_had_errors = 0x00000000;
    etherCAT_status_data.which_motors                = etherCAT_command_data.which_motors;
}



void Check_For_EtherCAT_Packet(void)
{
    ET1200_Update();

    #if AUTO_TRIGGER == 1                                                                   // This is just for debugging.
        if (get_frame_time_us() > 1150)                                                     // 
        {                                                                                   // 
            frame_start_time = ReadCoreTimer();                                             // 
            approx_1ms_handler();                                                           // 
        }                                                                                   // 
        return;                                                                             // 
    #endif                                                                                  // 


    if (There_is_Command_From_ET1200())
    {   
        frame_start_time = ReadCoreTimer();                                                 // The frame starts NOW!
        approx_1ms_handler();                                                               // Normally, the EtherCAT packets are used as a 1ms time base.
        Service_EtherCAT_Packet();
        return;
    }

    if (get_frame_time_us() > 1300)                                                         // If there are no EtherCAT packets,
    {                                                                                       // then we need to trigger approx_1ms_handler()
        frame_start_time = ReadCoreTimer();                                                 // And so we think of a frame starting now.
        approx_1ms_handler();                                                               // by time instead. 
        return;
    }
}



void Service_EtherCAT_Packet(void)
{
    Read_Commands_From_ET1200();                                                            // Read the command data


    switch (etherCAT_command_data.EDC_command)
    {
        case EDC_COMMAND_SENSOR_DATA:                                                       // Normal Command / Status packet
                                                                                            // ------------------------------

                                                                                            // This EDC_command is handled,
            LED_Off(ET1200_AL_err_LED);                                                     // so extinguish the Application Layer LED

            send_CAN_request_data_message( etherCAT_command_data.which_motors,
                                           etherCAT_command_data.from_motor_data_type);     // Start of frame message


            zero_motor_data_packets();                                                      // Housekeeping

            Read_All_Sensors();                                                             // Read all joint and tactile sensors.
            num_motor_CAN_messages_received_this_frame = 0;
            Wait_For_All_Motors_To_Send_Data(600);                                          // Wait for 600us max.
            Send_Data_To_Motors(&etherCAT_command_data);                                    // Send CAN messages to motors

            etherCAT_status_data.EDC_command  = EDC_COMMAND_SENSOR_DATA;                    // FIXME: I don't think this calculation is correct
            etherCAT_status_data.idle_time_us = calculate_idle_time();                      // 

            write_status_data_To_ET1200();
            idle_time_start = ReadCoreTimer();                                              // Idle time begins now because the status data
                                                                                            // has been written to the ET1200. The next EtherCAT
                                                                                            // packet can arrive any time after this.
            break;



        case EDC_COMMAND_CAN_DIRECT_MODE:                                                   // CAN Bridge packet
                                                                                            // -----------------

                                                                                            // This EDC_command is handled,
            LED_Off(ET1200_AL_err_LED);                                                     // so extinguish the Application Layer LED

            if ( ROS_Wants_me_to_send_CAN() )
            {
                read_ET1200_register_N(ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS, ETHERCAT_CAN_BRIDGE_DATA_SIZE, (int8u*)(&can_bridge_data_from_ROS));
                send_CAN_message_from_ROS();
                global_AL_Event_Register = read_ET1200_register_32u(0x220);
            }

            etherCAT_status_data.EDC_command  = EDC_COMMAND_SENSOR_DATA;
            etherCAT_status_data.idle_time_us = calculate_idle_time();                      // FIXME: I don't think this calculation is correct
            write_status_data_To_ET1200();                                                  // 

            Wait_For_Until_Frame_Time(600);                                                 // Give the node a little time to respond, and we might be able
                                                                                            // to get the reply message back into the very next EtherCAT packet.

            collect_one_CAN_message();                                                      // From either CAN bus

            write_ET1200_register_N(ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS, ETHERCAT_CAN_BRIDGE_DATA_SIZE, (int8u*)(&can_bridge_data_to_ROS));
            idle_time_start = ReadCoreTimer();                                              // Idle time begins now

            break;


        default:                                                                            // This EDC_command is not handled, and considered an error
            LED_On(ET1200_AL_err_LED);                                                      // So light the red Application Layer LED.
            break;
    }
}



void Platform_Init_Callback(void)
{
    unsigned char i;

    etherCAT_status_data.EDC_command         = 0xCC;//EDC_COMMAND_SENSOR_DATA;
    etherCAT_command_data.EDC_command        = 0xDD;//EDC_COMMAND_SENSOR_DATA;
    can_bridge_data_from_ROS.message_data[0] = 0xff;

    for (i = 1; i < ETHERCAT_STATUS_DATA_SIZE / 2 ; ++i)
        ((int16s *)&etherCAT_status_data)[i] = 0xDEAD;
    
    write_ET1200_register_N  ( ETHERCAT_STATUS_DATA_ADDRESS, ETHERCAT_STATUS_DATA_SIZE,   (int8u*)&etherCAT_status_data);
}



void report_error_code(ERROR_CODE  error_code)
{
    switch (error_code)
    {
        case CAN1_TX_NO_BUFFER:
        case CAN1_TX_BUFFER_FULL:
        case CAN1_RX_BUFFER_FULL:
        case CAN1_BECOME_SAD:
        case SIMPLE_CAN_BAD_MESSAGE_BUS_1:          
            LED_Single_Flicker(LED_CAN1_ERR);
            break;

        case CAN2_TX_NO_BUFFER:
        case CAN2_TX_BUFFER_FULL:
        case CAN2_RX_BUFFER_FULL:
        case CAN2_BECOME_SAD:
        case SIMPLE_CAN_BAD_MESSAGE_BUS_2:
            LED_Single_Flicker(LED_CAN2_ERR);
            break;

        default:
            NYI();                                  // Otherwise, panic!
            break;
    }
}



void report_event_code   (EVENT_CODE  event_code)
{
    switch (event_code)
    {
/*
        case CAN1_RECEIVED_MESSAGE:         LED_Single_Flicker(LED_CAN1_RX);            break;
        case CAN1_TRANSMITTED_MESSAGE:      LED_Single_Flicker(LED_CAN1_TX);            break;

        #ifdef LED_CAN2_RX_PIN
            case CAN2_RECEIVED_MESSAGE:         LED_Single_Flicker(LED_CAN2_RX);            break;
        #endif

        case CAN2_TRANSMITTED_MESSAGE:      LED_Single_Flicker(LED_CAN2_TX);            break;
*/

        case CAN1_RECEIVED_MESSAGE:         LED_On_For_5ms(LED_CAN1_RX);            break;
        case CAN1_TRANSMITTED_MESSAGE:      LED_On_For_5ms(LED_CAN1_TX);            break;

        #ifdef LED_CAN2_RX_PIN
            case CAN2_RECEIVED_MESSAGE:         LED_On_For_5ms(LED_CAN2_RX);            break;
        #endif

        case CAN2_TRANSMITTED_MESSAGE:      LED_On_For_5ms(LED_CAN2_TX);            break;

        default:
            break;
    }
}



void report_error_string (char *error_string, ...)
{
    
    assert_dynamic(0);                  // Protection while function is as yet unimplemented
    error_string = NULL;
}



void report_event_string (char *event_string, ...)
{
    //assert_dynamic(0);                  // Protection while function is as yet unimplemented
    event_string = NULL; // to avoid warning
}



void __assert_dynamic_fail(const char* UNUSED(fmt), ...)
{
    unsigned int i;

    TRISDCLR = 0b00000100111110001;
    PORTDCLR = 0b00000100111110001;

    while(1)
    {
        PORTDINV = 0b00000100111110001;
    
        for (i=0; i<1000000; i++)
        {
            Nop();
            Nop();
            Nop();
            Nop();
        }
    }    
}



void Received_Motor_Data(FROM_MOTOR_DATA_TYPE data_type, int8u motor_number, int16s measured_torque, int16u other_value)
{
    if (motor_number > 19)                                                                              // A motor_number > 19 is an error
    {                                                                                                   // Let's just ignore it and hope it goes away.
        return;
    }

    FROM_MOTOR_DATA_TYPE    expected_data_type   = etherCAT_command_data.from_motor_data_type;

    num_motor_CAN_messages_received_this_frame++;

    if (data_type == expected_data_type)                                                                // This is good :)
    {
        etherCAT_status_data.motor_data_type                            = data_type;
        etherCAT_status_data.motor_data_packet[motor_number>>1].torque  = measured_torque;
        etherCAT_status_data.motor_data_packet[motor_number>>1].misc    = other_value;

        etherCAT_status_data.which_motor_data_arrived                  |= 0x00000001 << motor_number;   // Record that a message arrived
        etherCAT_status_data.which_motor_data_had_errors               &= 0xFFFFFFFE << motor_number;   // and that it was good (as far as we can tell)
    }
    else
    {
        etherCAT_status_data.which_motor_data_arrived                  |= 0x00000001 << motor_number;   // Record that a message arrived
        etherCAT_status_data.which_motor_data_had_errors               |= 0x00000001 << motor_number;   // and that it was bad :(
    }
}



void delay_us(int32u microseconds) 
{ 
    int32u start = ReadCoreTimer(); 
    int32u stop  = (microseconds-1) * (SYSTEM_FREQ_HZ/2000000);     // Subtract 1 to account for call overheads.
    
    stop += start;                                  //
    
    while (ReadCoreTimer() < stop)                  // wait till Count reaches the stop value
    {
    }
}



void delay_ms(int32u milliseconds)
{ 
    int32u start     = ReadCoreTimer(); 
    int32u num_ticks = milliseconds * (SYSTEM_FREQ_HZ/2000);
    
    while (ReadCoreTimer()-start < num_ticks)                  // wait till Count reaches the stop value
    {
    }
}