File content as of revision 0:18d7499b82f3:

#define COMPILE_MAIN_CODE_ROSSERIAL
#ifdef  COMPILE_MAIN_CODE_ROSSERIAL

//include files
#include "include/eposCmd.h"

//SPI RxTx FIFO bits
#define TNF 0x02
#define TFE 0x01
#define RNE 0x04

SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
DigitalOut myled(LED1);
DigitalIn sync_master(p25);

//trs_mbed::MotorCommand motorCmdBuffer[CMD_BUFFER_SIZE];

DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
Timer timer;


uint8_t writeBufferSPI[NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG];
uint8_t readBufferSPI[NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG];

int counter = 0;

//5 variables for median poti position
//int16_t potiVal[NB_SAMPLES_MEDIAN];
//int16_t sortVal[NB_SAMPLES_MEDIAN];

int16_t forceVal[NB_SAMPLES_MEDIAN];
int16_t sortForceVal[NB_SAMPLES_MEDIAN];
//int16_t lastForceVal[NUMBER_EPOS2_BOARDS][NB_SAMPLES_MEDIAN];

/*
int16_t getMedianPotiVal(const int8_t nodeID) 
{        
    //read poti
    for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
    {
        getPotiPosition(nodeID);
        wait_us(1000);
        
        potiVal[m] = potiPosArray[nodeID - 1];        
        sortVal[m] = potiVal[m];

        wait_us(100);
    }
    
    //sort values
    for(int m=1; m<NB_SAMPLES_MEDIAN; ++m)
    {
        int16_t n;
        n = sortVal[m];
        int p;

        for(p = m-1; (p >=0) && (n<sortVal[p]); p--)
        {
            sortVal[p+1] = sortVal[p];
        }
        sortVal[p+1] = n;
    }
       
    return sortVal[2];
}
*/

int16_t getMedianForceVal(const int8_t nodeID) 
{         
    logicPin = 1;
    
    for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
    {          
        sortForceVal[m] = getForce(nodeID);
        wait_us(100); //adjust this
    }
    
    //sort values
    for(int m=1; m<NB_SAMPLES_MEDIAN; ++m)
    {
        int16_t n = sortForceVal[m];
        int p;

        for(p = m-1; (p >=0) && (n<sortForceVal[p]); p--)
        {
            sortForceVal[p+1] = sortForceVal[p];
        }
        sortForceVal[p+1] = n;
    }
    
    logicPin = 0;
       
    return sortForceVal[2];
}

/*** INTERRUPT (for catching Emergency error frames) ***/
void interrupt() 
{
    CANMessage canmsg;
    int64_t data = 0x0000000000000000;     
    int8_t nodeID = 0;
    int16_t cobID = 0;
        
    //read the can message that has triggered the interrupt
    cantoepos.read(canmsg);   
    
    //pc.printf("Interrupt frame : [%02X] [%02X %02X %02X %02X %02X %02X %02X %02X]\n", canmsg.id, canmsg.data[7], canmsg.data[6], canmsg.data[5], canmsg.data[4], canmsg.data[3], canmsg.data[2], canmsg.data[1], canmsg.data[0]);

    nodeID = 0x00F & canmsg.id;
    cobID = 0x0FF0 & canmsg.id;

    for(int i=0; i<=canmsg.len; i++)
    {
        data = data | (canmsg.data[i]<<i*8);        
    }
    
    //check nodeID first
    if((nodeID >= 0) && (nodeID <= NUMBER_EPOS2_BOARDS))
    {        
        switch(cobID)
        {       
            case COB_ID_TRANSMIT_PDO_1_ENABLE : //getPosition
                //pc.printf("getPosition Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
                encPosition[nodeID - 1] = data;
                break;
                
            case COB_ID_TRANSMIT_PDO_2_ENABLE : //getCurrent averaged
                //pc.printf("getCurrent Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
                avgCurrent[nodeID - 1] = data;
                break;
                
            case COB_ID_TRANSMIT_PDO_3_ENABLE : //getVelocity
                //pc.printf("getVelocity Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
                velocity[nodeID - 1] = data;
                break;
                
            case COB_ID_TRANSMIT_PDO_4_ENABLE : //getPotiPosition
                //pc.printf("getPotiPosition Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
                potiPosArray[nodeID - 1] = data;
                break; 
             
            case COB_ID_EMCY_DEFAULT : //Emergency frame
                //pc.printf("Emergency frame, Node ID : [%d], PDO COB-ID [%02X], data = %02X\n", nodeID, cobID, data);
                //pc.printf("EF [%02X][%02X %02X %02X %02X %02X %02X %02X %02X]\n", canmsg.id, canmsg.data[7], canmsg.data[6], canmsg.data[5], canmsg.data[4], canmsg.data[3], canmsg.data[2], canmsg.data[1], canmsg.data[0]);
                pc.printf("EF%02X-%02X%02X\n\r", canmsg.id, canmsg.data[1], canmsg.data[0]);
                ledchain[1] = 1;            
                //nh.logerror("Emergency frame");
                boardStatus[nodeID-1] = 1;                
                //first step : fault reset on controlword
                //pc.printf("Node %d - STEP 1 - faultResetControlword\n", nodeID);
                faultResetControlword(nodeID);                 
                break;  
                
            case COB_ID_SDO_SERVER_TO_CLIENT_DEFAULT : //SDO Acknoledgement frame
                int32_t regData = 0x00000000;
                regData = (int32_t)data;
                
                //pc.printf("Node %d - regData [%02X]\n", nodeID, regData);
                                
                if(regData == 0x00604060) //Controlword
                {
                    if(boardStatus[nodeID-1] == 1)
                    {
                        boardStatus[nodeID-1] = 2;                      
                        //second step : shutdown controlword
                        //pc.printf("Node %d - STEP 2 - shutdownControlwordIT\n", nodeID);
                        shutdownControlwordIT(nodeID);
                    }
                    else if(boardStatus[nodeID-1] == 2)
                    {
                        boardStatus[nodeID-1] = 3;                      
                        //third step : Switch On & Enable Operation on Controlword
                        //pc.printf("Node %d - STEP 3 - switchOnEnableOperationControlwordIT\n", nodeID);
                        switchOnEnableOperationControlwordIT(nodeID);
                    }
                    else if(boardStatus[nodeID-1] == 3)
                    {
                        boardStatus[nodeID-1] = 4;
                        //ask for statusword to check if the board has reset well
                        //pc.printf("Node %d - STEP 4 - getStatusword\n", nodeID);
                        getStatusword(nodeID);
                    }
                } 
                else if(regData == 0x0060414B) //read Statusword
                {
                    int32_t swData = 0x00000000;
                    
                    //pc.printf("Node %d - Statusword [%02X]\n", nodeID, canmsg.data[4]);
                    //pc.printf("Statusword frame : [%02X] [%02X %02X %02X %02X %02X %02X %02X %02X]\n", canmsg.id, canmsg.data[7], canmsg.data[6], canmsg.data[5], canmsg.data[4], canmsg.data[3], canmsg.data[2], canmsg.data[1], canmsg.data[0]);
                    
                    if(boardStatus[nodeID-1] == 4)
                    {
                        //swData = data >> 32;
                        int8_t fault = 0x00;
                        fault = (canmsg.data[4] & 0x08) >> 3;
                                               
                        if(fault == 0) //reset OK
                        {
                            boardStatus[nodeID-1] = 0; //Board is reset and enable OK
                            pc.printf("%d OK\n\r", nodeID);
                            ledchain[1] = 0;
                        }
                        else //try to reset again
                        {
                            //pc.printf("Node %d - try to reset again\n", nodeID);
                            boardStatus[nodeID-1] = 1;                
                            //go back to first step : fault reset on controlword
                            //pc.printf("Node %d - STEP 1 - faultResetControlword\n", nodeID);
                            faultResetControlword(nodeID);                        
                        }
                    }  
                }                                            
                break;  
                           
            default :
                pc.printf("Unknown frame [%02X][%02X %02X %02X %02X %02X %02X %02X %02X]\n\r", canmsg.id, canmsg.data[7], canmsg.data[6], canmsg.data[5], canmsg.data[4], canmsg.data[3], canmsg.data[2], canmsg.data[1], canmsg.data[0]);                            
        } //end switch
    }
    else
    {
        pc.printf("NODEID ERROR\n\r");    
    }
} //end interrupt

void commandPlayer() //called in main every 20ms
{/*
    for(int i= 0; i<CMD_BUFFER_SIZE; i++)
    {        
        if(motorCmdBuffer[i].beatDelay != -1) 
        {       
            switch (motorCmdBuffer[i].mode)
            {                
                case POSITION: //first change modes of motors that will be triggered later   
                    if((activMode[motorCmdBuffer[i].nodeID-1] != POSITION) && (motorCmdBuffer[i].beatDelay == 1)) setModeOfOperationPDO(motorCmdBuffer[i].nodeID, VALUE_POSITION_MODE);         
                    if(motorCmdBuffer[i].beatDelay == 0) setPosition(motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);                    
                    break;
                case CURRENT: //first change modes of motors that will be triggered later (like CURRENT mode needs some time to be active) 
                    //pc.printf("setCurrent(%d, %d)\n", motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
                    if((activMode[motorCmdBuffer[i].nodeID-1] != CURRENT) && (motorCmdBuffer[i].beatDelay == 1)) setModeOfOperationPDO(motorCmdBuffer[i].nodeID, VALUE_CURRENT_MODE);
                    if(motorCmdBuffer[i].beatDelay == 0) setCurrent(motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
                    break;
                case VELOCITY: //first change modes of motors that will be triggered later            
                    if((activMode[motorCmdBuffer[i].nodeID-1] != VELOCITY) && (motorCmdBuffer[i].beatDelay == 1)) setModeOfOperationPDO(motorCmdBuffer[i].nodeID, VALUE_VELOCITY_MODE);         
                    if(motorCmdBuffer[i].beatDelay == 0) setVelocity(motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
                    break;
                default:
                    break;
            }
            
            //decrement only cmd with delay != -1, so cmd that has not been played yet.
            motorCmdBuffer[i].beatDelay = motorCmdBuffer[i].beatDelay - 1;
        }
    }*/
}

void initBufferSPI()
{
    //init the SPI arrays
    for(int i=0; i<NUMBER_MSG_PER_PACKET; i++)
    { 
        for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
        {             
            //writeBufferSPI[i][j] = 0x00;
            readBufferSPI[i][j] = 0x00;            
        }
    }
    
    for(int n=0; n<NUMBER_MSG_PER_PACKET; n++)
    {
        writeBufferSPI[n][0] = n+1; //CANnodeID
        writeBufferSPI[n][1] = 2; //mode of command
        
        writeBufferSPI[n][2] = 0xA0+n; 
        writeBufferSPI[n][3] = 0xB0+n; 
        writeBufferSPI[n][4] = 0xC0+n; 
        writeBufferSPI[n][5] = 0xD0+n;         
        writeBufferSPI[n][6] = 0xE0+n;      
    }
}

int main() 
{          
    pc.baud(115200); //115200 //57600
    pc.printf("*** Start Slave Main ***\n\r");
    
    logicPin = 0;
    timer.start();
    uint64_t begin = 0;
    uint64_t end = 0;
    int64_t pauseTime = 0;
    //int8_t node = 0; //test 
    
    initBufferSPI();
    //sync_master = 0;
    char rByte = 0x00;
    
    char threeArrows = 0;
    char closeArrowChar = 0x62; //>
    bool startReceiving = false;
    bool threeArrowsFound = false;
    bool slaveSelected = false;
    
    int i = 0; //msg
    int j = 0; //byte number
        
    pc.printf("--- Initialise EPOS2 boards ---\n\r");    
    for(int i=1; i<=NUMBER_EPOS2_BOARDS; i++)
    {
        if(i!=5) initEposBoard(i); 
    } 
        
    ledchain[0] = 1;   
    
    pc.printf("--- Enable Interrupts ---\n\r"); 
    //attach the interrupt function
    cantoepos.attach(&interrupt);
    __enable_irq();
    
    device.reply(0x62); //Prime SPI with first reply
    
 /*                   
    pc.printf("--- Get poti positions for calibration...");
    //set the nodeID and potiPosition field of motorDataSet_msg          
    for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
    {
        uint8_t node_id = i+1;
        if(node_id!=5)
        {                                       
            motorDataSet_msg.motorData[i].encPosition = 0;
            motorDataSet_msg.motorData[i].potiPosition = getMedianPotiVal(node_id);
            motorDataSet_msg.motorData[i].current = 0 ;   
            motorDataSet_msg.motorData[i].force = -1; 
        }
        else
        {
            motorDataSet_msg.motorData[i].encPosition = 0;
            motorDataSet_msg.motorData[i].potiPosition = 9999;
            motorDataSet_msg.motorData[i].current = 0 ;   
            motorDataSet_msg.motorData[i].force = -1; 
        }
    }
    
    pc.printf("...OK\n\r");
*/

    //gather first pack of data
    //get the sensor values          
    for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
    {
        uint8_t node_id = i+1;
        if(node_id!=5)
        {                        
            getPosition(node_id);
            wait_us(300);   
            getCurrent(node_id);
            wait_us(300);
            //if((node_id >= 2) && (node_id <= 9)) getMedianForceVal(node_id);                                           
        }            
    }
      
    //build the motorDataSet_msg          
    for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
    {
        uint8_t node_id = i+1;
        if(node_id!=5)
        { 
            //pc.printf("[%d] pos=%d cur=%d\n", node_id, encPosition[node_id-1], avgCurrent[node_id-1]);
            
            //motorDataSet_msg.motorData[i].encPosition = encPosition[node_id-1];    
            //motorDataSet_msg.motorData[i].current = avgCurrent[node_id-1];
            //if((node_id >= 2) && (node_id <= 9)) motorDataSet_msg.motorData[i].force = getMedianForceVal(node_id); //medForce[node_id-1]; 
        }           
    }
    
    
    
    //then start the main loop
    pc.printf("--- Start main loop ---\n\r"); 
    
    //setModeOfOperationPDO(1, VALUE_CURRENT_MODE);
    //setCurrent(1, 150);
    
    uint8_t my_val;
              
    while(1) 
    {         
        //begin = timer.read_us();
        //logicPin = 1;     
        
        //pc.printf("1\n\r");           
             
        
        
        //wait, the master will put the pin high at some point, for 10us
        while(sync_master == 0)
        {
            wait_us(1);  
        }
        
        slaveSelected = true;
        
        while (LPC_SSP1->SR & RNE)          // While RNE-Bit = 1 (FIFO receive buffer not empty)...
        my_val = LPC_SSP1->DR;          // Read the byte in the buffer
        
        //reset for a new message
        i = 0;
        j = 0;
        threeArrows = 0;
        threeArrowsFound = false;
        
        logicPin = 1;
        
        //pc.printf("START - ");
        
        __disable_irq();
        
        while(slaveSelected)
        {
            //SPI polling
            if(device.receive()) 
            {/*
                //pc.printf("startReceiving = true\n\r");
                startReceiving = true;
                //logicPin = 1;
                rByte = device.read(); 
                //pc.printf("0x%02X ", rByte);
                //logicPin = 0;
                
                if(rByte == 0x60) 
                {   
                    threeArrows++;  
                    //pc.printf("3Arrows++ before while\n\r");
                }
                
                device.reply(0x00);
                wait_us(1);
                            
                while(startReceiving) 
                {
                    //pc.printf("2\n"); 
                    if(device.receive()) 
                    {*/
                        //logicPin = 1;   
                        rByte = device.read();   // Read byte from master
                        //pc.printf("0x%02X ", rByte);
                        //wait_ms(100);
                        //logicPin = 0;
                        
                        if(threeArrows < 3)
                        {
                            if(rByte == 0x60) 
                            {   
                                threeArrows++;  
                                //pc.printf("3A++\n\r");
                            }
                            else 
                            {
                                //threeArrows = 0; 
                                //startReceiving = false;   
                                //pc.printf("error3A\n");  
                                //slaveSelected = false; 
                            }
                            
                            if(threeArrows == 3) 
                            {
                                device.reply(writeBufferSPI[i][j]);
                                threeArrowsFound = true;
                            }
                            else
                            {
                                device.reply(0x62); //close arrow : >
                            }
                            
                            //pc.printf("3Arrows %d \n", threeArrows);
                            //if(threeArrows == 3) threeArrowsFound = true;       
                        }
                        else
                        {/*
                            if(threeArrowsFound) //to reset i and j when receiving a new message
                            {
                                //pc.printf("3AFound\n\r");
                                i = 0;
                                j = 0;
                                threeArrowsFound = false;
                            }*/
                            
                            //logicPin = 1;
                            readBufferSPI[i][j] = rByte;
                            //pc.printf("i=%d j=%d\n", i,j);
                            //logicPin = 0;
                                            
                            j++; //write next byte next time
                             
                            if(j >= NUMBER_BYTES_PER_MSG)
                            {
                                j = 0;
                                i++; //next node 
                                
                                if(i >= NUMBER_MSG_PER_PACKET)
                                {
                                    //pc.printf("\n\r");
                                    //pc.printf("final i=%d j=%d\n\r", i,j);
                                    //finished reading the array                        
                                    /*
                                    for(int n=0; n<1; n++)
                                    {
                                        pc.printf("0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X\n", readBufferSPI[n][0], readBufferSPI[n][1], readBufferSPI[n][2], readBufferSPI[n][3], readBufferSPI[n][4], readBufferSPI[n][5], readBufferSPI[n][6]);
                                    }
                                    
                                    pc.printf("\n\r");
                                    */
                                    
                                    //reset
                                    threeArrows = 0;
                                    i = 0;
                                    j = 0;
                                    //startReceiving = false;
                                    //mbedID_ok = false;
                                    slaveSelected = false; //to end the while loop
                                }             
                            } 
                                      
                            device.reply(writeBufferSPI[i][j]);     
                        }
                        
                        //logicPin = 0;    
                    //}//if
                    
                    //wait_us(1);
                //}//while startReceiving                 
            }//if
            
            wait_us(1);
        }//while slaveSelected
                    
        __enable_irq();
        
        //pc.printf(" - END\n\r");
        logicPin = 0; 
        wait_us(10);
        logicPin = 1;
        
        //get the sensor values          
        for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
        {
            uint8_t node_id = i+1;
            if(node_id!=5)
            {                        
                getPosition(node_id);
                wait_us(300);   
                getCurrent(node_id);
                wait_us(300);
                //if((node_id >= 2) && (node_id <= 9)) getMedianForceVal(node_id);                                           
            }            
        }
          
        //build the motorDataSet_msg          
        for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
        {
            uint8_t node_id = i+1;
            if(node_id!=5)
            { 
                //pc.printf("[%d] pos=%d cur=%d\n", node_id, encPosition[node_id-1], avgCurrent[node_id-1]);
                
                //motorDataSet_msg.motorData[i].encPosition = encPosition[node_id-1];    
                //motorDataSet_msg.motorData[i].current = avgCurrent[node_id-1];
                //if((node_id >= 2) && (node_id <= 9)) motorDataSet_msg.motorData[i].force = getMedianForceVal(node_id); //medForce[node_id-1]; 
            }           
        }
        
        logicPin = 0; 
            
        wait_us(1);
        
    /*       
        //disable interrupts and publish it        
        __disable_irq();
        motorDataSetPub.publish(&motorDataSet_msg);                 
        //this check if there are some msg published on the topics, and excecute the cb functions        
        nh.spinOnce();
        __enable_irq();
        
        //this will excecute cmds of the array that are ready (delay 0)         
        commandPlayer();   
      */  
      
        //logicPin = 0;
    /*    
        //pc.printf("before end\n\r");
        end = timer.read_us();         
        pc.printf("end=%d\n", end);
        pauseTime = LOOP_PERIOD_TIME - end + begin - 12; //12us is the time to compute those 2 lines
        //pc.printf("pauseTime=%d\n", pauseTime);
        pc.printf("begin = %d - pause = %d, end = %d\n", begin, pauseTime, end);
        pc.printf("pauseTime=%d\n", pauseTime);
        if(pauseTime > 0) wait_us(pauseTime);
      */  
        //pc.printf("after end\n\r");                                                       
    }// main while end           
}// main end

#endif //COMPILE_MAIN_CODE_ROSSERIAL