File content as of revision 7:7ef85554cec5:

/*
 * Copyright (c) 2013, The Robot Studio
 *  All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Feb 28, 2013
 *      Author: Cyril Jourdan (cyril.jourdan@therobotstudio.com)
 */
 
/*** Includes ***/ 
#include"mbed.h"

#define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 512
#define MODSERIAL_DEFAULT_TX_BUFFER_SIZE 1024
#include "MODSERIAL.h"

/*** Defines ***/
#define NUMBER_MAX_EPOS2_PER_SLAVE  15
#define NUMBER_SLAVE_BOARDS         5 //3
#define NUMBER_MSG_PER_PACKET       NUMBER_MAX_EPOS2_PER_SLAVE*NUMBER_SLAVE_BOARDS //75 //60 //45
#define NUMBER_BYTES_PER_MSG        6
#define NUMBER_BYTES_TO_READ        NUMBER_BYTES_PER_MSG + 2
#define FIRST_NODE_ID_SLAVE_1       1
#define FIRST_NODE_ID_SLAVE_2       FIRST_NODE_ID_SLAVE_1 + NUMBER_MAX_EPOS2_PER_SLAVE
#define FIRST_NODE_ID_SLAVE_3       FIRST_NODE_ID_SLAVE_2 + NUMBER_MAX_EPOS2_PER_SLAVE
#define FIRST_NODE_ID_SLAVE_4       FIRST_NODE_ID_SLAVE_3 + NUMBER_MAX_EPOS2_PER_SLAVE
#define FIRST_NODE_ID_SLAVE_5       FIRST_NODE_ID_SLAVE_4 + NUMBER_MAX_EPOS2_PER_SLAVE
#define EPOS2_OK                    0
#define EPOS2_ERROR                 -1
//#define LOOP_PERIOD_TIME            20000 //20 ms

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

#define OPEN_ARROW          0x3C //< = 60
#define CLOSE_ARROW         0x3E //< = 62
#define DUMMY_BYTE          0x00
#define NUMBER_OF_ARROWS    5

/*** Variables ***/
MODSERIAL ros(p28, p27, 1024, 512); // tx, rx
Serial pc(USBTX, USBRX); //terminal for debug
DigitalOut ledchain[] = {(LED1), (LED2), (LED3), (LED4)}; //used for debugging
DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.

SPI spi(p5, p6, p7); // mosi, miso, sclk
DigitalOut cs[NUMBER_SLAVE_BOARDS] = {(p8), (p9), (p10), (p11), (p12)}; //Slave Mbed number 1 //chip select
DigitalOut sync_slave[NUMBER_SLAVE_BOARDS] = {(p25), (p24), (p23), (p22), (p21)}; //test to sync the slave

char* readBufferSerial; //[NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG]; //buffer of packets read by the master (written by the ros node on pc side)
uint8_t writeBufferSPI[NUMBER_SLAVE_BOARDS][NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_TO_READ]; //buffer ready to be sent over SPI to different slaves
uint8_t readBufferSPI[NUMBER_SLAVE_BOARDS][NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_TO_READ]; //buffer read by the master on SPI bus 

Timer timer;
uint64_t begin, end;
uint8_t numberCmds[NUMBER_SLAVE_BOARDS];

bool newCmd_detected = false;
uint8_t nbArrows = 0;
 
char rByte = 0x00;
char writeChecksum[NUMBER_SLAVE_BOARDS]; //one checksum per board
char readChecksum[NUMBER_SLAVE_BOARDS];
char serialTxChecksum = 0x00;

bool fiveArrowsFound = false;
bool cmdValid = false;
bool dataValid = false;

/*** Functions ***/
int move(char *s, int nbBytes) //custom move function (cp from MODESERIAL without the end character)
{
    int counter = 0;
    char c;
    
    while(ros.readable()) 
    {
        c = ros.getc();        
        *(s++) = c;
        counter++;
        if(counter == nbBytes) break;
    }
    
    return counter;
}

bool verifyCmdChecksum(char* data, int length, char checksum) //verify data comming from he PC
{      
    for(int i=0; i<length; i++)
    {
        checksum += data[i];
    }        
    
    checksum++; //add 1 to obtain 0x00
        
    if(checksum == 0x00) return true;
    else return false;
}

bool verifyDataChecksum() //verify data comming from the slaves on SPI
{
    bool allDataValid = true;
    
    for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
    {   
        for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
        {
            for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
            {
                readChecksum[k] += readBufferSPI[k][i][j];
            }
        }
    
        readChecksum[k]++;
        
        if(readChecksum[k] != 0x00) allDataValid = false; //toggle the flag if one of them is corrupted
    }    
    
    return allDataValid;
}

void calculateSPIChecksum() //compute checksum for each slave to send commands over SPI
{
    for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
    {
        int sum = 0; 
        
        for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
        {
            for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
            {
                sum += writeBufferSPI[k][i][j];
            }
        }
    
        writeChecksum[k] = (char)(~sum); //reverse 0 and 1, and cast as byte
    }
}

void calculateTxChecksum() //compute checksum for all the data sent to the PC over serial
{   
    int sum = 0;
    
    for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
    {   
        for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
        {
            for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
            {
                sum += readBufferSPI[k][i][j];
            }
        }        
    }
    
    serialTxChecksum = (char)(~sum); //reverse 0 and 1, and cast as byte
}

// Called everytime a new character goes into
// the RX buffer. Test that character for '/'
// Note, rxGetLastChar() gets the last char that
// we received but it does NOT remove it from
// the RX buffer.
void rxCallback(MODSERIAL_IRQ_INFO *q) 
{
    //logicPin = 1;
    
    MODSERIAL *serial = q->serial;      
    rByte = serial->rxGetLastChar();
    
    if(!fiveArrowsFound)
    {
        if(nbArrows < NUMBER_OF_ARROWS)
        {
            if(rByte == CLOSE_ARROW)
            {   
                nbArrows++;  
            }
           
            if((nbArrows > 0) && (rByte != CLOSE_ARROW))
            {
                nbArrows = 0; //reset in case the previous arrows was data. 
            }
            
            if(nbArrows == NUMBER_OF_ARROWS) 
            {
                fiveArrowsFound = true;
            } 
        }
    }
    else //fiveArrowsFound, so rByte is the checksum
    {
        move(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG);
        //pc.printf("r cs 0x%02X\n", rByte);
        //pc.printf("move %02X %02X %02X %02X %02X %02X %02X \n", readBufferSerial[0], readBufferSerial[1], readBufferSerial[2], readBufferSerial[3], readBufferSerial[4], readBufferSerial[5], readBufferSerial[6]);
        
        cmdValid = verifyCmdChecksum(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG, rByte);               
        //if(cmdValid) pc.printf("cmdValid\n\r");        
        
        //reset
        serial->rxBufferFlush();  
        nbArrows = 0;
        fiveArrowsFound = false;      
    }   
    
    //logicPin = 0;
}
 
/*** Main ***/ 
int main() 
{
    //Deselect all mbed slaves
    for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
    {
        cs[k] = 1;
        sync_slave[k] = 0;
    }
        
    ros.baud(460800); //460800 works //921600 don't
    pc.baud(115200);
    
    // Setup the spi for 8 bit data, high steady state clock,
    // second edge capture, with a 1MHz clock rate
    spi.format(8, 0); //spi.format(8,3);
    spi.frequency(1000000); //32000000
    
    //init the SPI arrays
    for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
    { 
        for(int j=0; j<NUMBER_MAX_EPOS2_PER_SLAVE; j++)
        { 
            for(int k=0; k<NUMBER_BYTES_TO_READ; k++)
            {
                writeBufferSPI[i][j][k] = 0x00;
                readBufferSPI[i][j][k] = 0x00;
            }
        }
        
        writeChecksum[i] = 0x00;
    }
    
    //init alloc
    readBufferSerial = (char*)malloc(NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG*sizeof(char*));
    
    //uint8_t my_val;
    
    ros.attach(&rxCallback, MODSERIAL::RxIrq);
    
    pc.printf("*** Start Master Main ***\n\r");
    
    logicPin = 0;
 
    // Wait here until we detect a valid message in the serial RX buffer.
    while(1)
    {
        if(cmdValid) //pass it to the SPI bus
        {
            logicPin = 1;
               
            //init the SPI arrays
            for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
            { 
                for(int j=0; j<NUMBER_MAX_EPOS2_PER_SLAVE; j++)
                { 
                    for(int k=0; k<NUMBER_BYTES_TO_READ; k++)
                    {
                        writeBufferSPI[i][j][k] = 0x00; //mode 0 for null command
                        readBufferSPI[i][j][k] = 0x00;
                    }
                }
                
                writeChecksum[i] = 0x00;
            }
            /*
            //init nb cmds per slave //useless ?
            for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
            {
                numberCmds[i] = 0;
            }
            */
            logicPin = 0;
            
            //sort messages for each slave
            for(int i=0; i<NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG; i+=NUMBER_BYTES_PER_MSG)
            {
                uint8_t nodeID = readBufferSerial[i];
                
                if(nodeID>=FIRST_NODE_ID_SLAVE_1 && nodeID<FIRST_NODE_ID_SLAVE_2) //slave 1
                {           
                    for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
                    {
                        writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];                        
                    }                                                                                
                    //numberCmds[0]++;                
                }
                else if(nodeID>=FIRST_NODE_ID_SLAVE_2 && nodeID<FIRST_NODE_ID_SLAVE_3) //slave 2
                {
                    for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
                    {
                        writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG-NUMBER_MAX_EPOS2_PER_SLAVE][j] = readBufferSerial[i+j];
                    }
                    
                    //change nodeID between 1 and 15
                    writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG-NUMBER_MAX_EPOS2_PER_SLAVE][0] -= NUMBER_MAX_EPOS2_PER_SLAVE; //substract a multiple of 15, example : nodeID 16 will be nodeID 1 for slave nb 2
                    
                    //pc.printf("ID %d\n", writeBufferSPI[1][0][0]);
                    //numberCmds[1]++;
                    //pc.printf("ID[%d] %d\n", i/NUMBER_BYTES_PER_MSG, writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG][0]);
                }
                else if(nodeID>=FIRST_NODE_ID_SLAVE_3 && nodeID<FIRST_NODE_ID_SLAVE_4) //slave 3
                {
                    for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
                    {
                        writeBufferSPI[2][i/NUMBER_BYTES_PER_MSG-2*NUMBER_MAX_EPOS2_PER_SLAVE][j] = readBufferSerial[i+j];
                    }
                    
                    //change nodeID between 1 and 15
                    writeBufferSPI[2][i/NUMBER_BYTES_PER_MSG-2*NUMBER_MAX_EPOS2_PER_SLAVE][0] -= 2*NUMBER_MAX_EPOS2_PER_SLAVE; //substract a multiple of 15, example : nodeID 16 will be nodeID 1 for slave nb 2
                    //numberCmds[2]++;
                }   
                else if(nodeID>=FIRST_NODE_ID_SLAVE_4 && nodeID<FIRST_NODE_ID_SLAVE_5) //slave 4
                {
                    for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
                    {
                        writeBufferSPI[3][i/NUMBER_BYTES_PER_MSG-3*NUMBER_MAX_EPOS2_PER_SLAVE][j] = readBufferSerial[i+j];
                    }
                    
                    //change nodeID between 1 and 15
                    writeBufferSPI[3][i/NUMBER_BYTES_PER_MSG-3*NUMBER_MAX_EPOS2_PER_SLAVE][0] -= 3*NUMBER_MAX_EPOS2_PER_SLAVE; //substract a multiple of 15, example : nodeID 16 will be nodeID 1 for slave nb 2
                    //numberCmds[2]++;
                } 
                else if(nodeID>=FIRST_NODE_ID_SLAVE_5) //slave 5 
                {
                    for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
                    {
                        writeBufferSPI[4][i/NUMBER_BYTES_PER_MSG-4*NUMBER_MAX_EPOS2_PER_SLAVE][j] = readBufferSerial[i+j];
                    }
                    
                    //change nodeID between 1 and 15
                    writeBufferSPI[4][i/NUMBER_BYTES_PER_MSG-4*NUMBER_MAX_EPOS2_PER_SLAVE][0] -= 4*NUMBER_MAX_EPOS2_PER_SLAVE; //substract a multiple of 15, example : nodeID 16 will be nodeID 1 for slave nb 2
                    //numberCmds[2]++;
                }  
                //pc.printf("ID %d\n", writeBufferSPI[1][0][0]);  
            }      
            
            //pc.printf("ID %d\n", writeBufferSPI[1][0][0]);
            
            //add dummy bytes
            for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
            {
                for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
                {   
                    for(int j=NUMBER_BYTES_PER_MSG; j<NUMBER_BYTES_TO_READ; j++)
                    {
                        writeBufferSPI[k][i][j] = DUMMY_BYTE;                        
                    }
                }
            }
            
            //now all individual SPI buffers for slaves have been created
            //compute checksum for each slave and update the variable, it'll be sent later at the end of SPI writting
            calculateSPIChecksum(); //this update the writeChecksum[k]
            
            //pc.printf("nbCmd %d %d %d\n", numberCmds[0], numberCmds[1], numberCmds[2]);   
            //pc.printf("1st Cmd %02X %02X %02X %02X %02X %02X %02X\n", writeBufferSPI[0][0][0], writeBufferSPI[0][0][1], writeBufferSPI[0][0][2], writeBufferSPI[0][0][3], writeBufferSPI[0][0][4], writeBufferSPI[0][0][5], writeBufferSPI[0][0][6]);  
                    
            //new commands has been grabbed and are ready to be sent to slaves            
            for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)  //NUMBER_SLAVE_BOARDS for each slave
            {
                //if(k<3) ledchain[k] = 1; //switch on LED nb k
                if(k<4) ledchain[k] = 1; //switch on LED nb k
                
                sync_slave[k] = 1;
                wait_us(10); //pause so the slave can see it's been selected
                sync_slave[k] = 0;
                            
                cs[k] = 0;          
                spi.write(OPEN_ARROW);
                wait_us(5);
                cs[k] = 1;          
                wait_us(8); 
                
                cs[k] = 0;
                spi.write(OPEN_ARROW);
                wait_us(5);
                cs[k] = 1;
                wait_us(8);
                
                cs[k] = 0; 
                spi.write(OPEN_ARROW);
                wait_us(5);
                cs[k] = 1;
                wait_us(8);
                
                for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++) 
                {                    
                    //writeBufferSPI[k][i][0] = writeBufferSPI[k][i][0] - 0x0F;
                    
                    for(int j=0; j<NUMBER_BYTES_TO_READ; j++) 
                    {
                        cs[k] = 0;
                        readBufferSPI[k][i][j] = (char)(spi.write(writeBufferSPI[k][i][j]));
                        wait_us(5);
                        cs[k] = 1;
                        wait_us(8);
                    }
                }
                
                //finally write the command checksum and read the data checksum at the same time
                cs[k] = 0; 
                readChecksum[k] = (char)(spi.write(writeChecksum[k]));
                wait_us(5);
                cs[k] = 1;
                wait_us(8);   
                
                if(k<4) ledchain[k] = 0; //switch on LED nb k                    
            }
            
            //pc.printf("nodeID 1 0x%02X 2 0x%02X 3 0x%02X\n", writeBufferSPI[0][0][0], writeBufferSPI[1][0][0], writeBufferSPI[2][0][0]);
            //pc.printf("nodeID 1-%d 2-%d 3-%d\n", writeBufferSPI[0][0][0], writeBufferSPI[1][0][0], writeBufferSPI[2][0][0]);
            //pc.printf("rc 0x%02X wc 0x%02X\n", readChecksum[0], writeChecksum[0]);
            //pc.printf("%02X\n", writeChecksum[2]);
            /*
            logicPin = 0;
            wait_us(10);
            logicPin = 1;
            wait_us(10);
            */
            //now check the validity of the data
            dataValid = verifyDataChecksum();
            
            //Erase Tx serial buffer
            //ros.txBufferFlush();
                        
            //write the data msg on serial Tx
            //if(dataValid)
            //{
                logicPin = 1;
                
                //compute checksum for all data from slaves
                calculateTxChecksum();
                
                //write data and checksum on Tx
                for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
                {   
                    for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
                    {
                        for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
                        {
                            ros.putc(readBufferSPI[k][i][j]);
                        }
                    }        
                }
                
                ros.putc(serialTxChecksum);
                
                dataValid = false; //toggle flag for next message
                
                logicPin = 0;
            //}
            
            //print the array :
            /*
            for(int i=0; i<2; i++)
            { 
                pc.printf("%02X %02X %02X %02X %02X %02X %02X\n\r", readBufferSPI[0][i][0], readBufferSPI[0][i][1], readBufferSPI[0][i][2], readBufferSPI[0][i][3], readBufferSPI[0][i][4], readBufferSPI[0][i][5], readBufferSPI[0][i][6]);
            }
            */
 /*           int i=0;
            pc.printf("%02X %02X %02X %02X %02X %02X %02X\n", readBufferSPI[0][i][0], readBufferSPI[0][i][1], readBufferSPI[0][i][2], readBufferSPI[0][i][3], readBufferSPI[0][i][4], readBufferSPI[0][i][5], readBufferSPI[0][i][6]);
            i=14;
            pc.printf("%02X %02X %02X %02X %02X %02X %02X\n\r", readBufferSPI[0][i][0], readBufferSPI[0][i][1], readBufferSPI[0][i][2], readBufferSPI[0][i][3], readBufferSPI[0][i][4], readBufferSPI[0][i][5], readBufferSPI[0][i][6]);
            
            //pc.printf("\n\r");    
            logicPin = 0;     
            
            //build the motorDataSet_msg          
            for(int i=0; i<13; i++)
            {            
                motorDataSet_msg.motorData[i].encPosition = 100*i;
                motorDataSet_msg.motorData[i].potiPosition = 10*i;   
                motorDataSet_msg.motorData[i].current = -10*i;
                motorDataSet_msg.motorData[i].force = 2*i; 
            }
     */       
            //logicPin = 0;
            
            cmdValid = false; //toggle flag for next message
            
        }
        
        wait_us(10);
    }       
}