File content as of revision 3:e33697420c4a:

#include "mbed.h"
#include "SPI.h"
DigitalOut myled(LED1);
Serial pc(USBTX, USBRX); 

int masterRx = 0;
int16_t slaveRx = 0;
int i = 2;
int k = 0;
int16_t zgHigher = 0;
int16_t zgLower = 0;
int16_t zGyro = 0;
int countx = 0;
int p = 32776;


/*

*/
//------------------Printing-All-values----------------------//
int16_t IMUarray[12]; //Store each separate reading in an array
uint16_t IDarray[12]; //Holds the identification of each data piece
char idx = 0;         //IMUarray Pointer
char dataCount = 0;   //Keeps track of how many data points have been read in using SPI
uint16_t id = 0;      //Used to store extracted data ID

void ProcessAndPrint();
//------------------Printing-All-values----------------------//


int main() {
    
    IDarray[0] = 1;
    IDarray[1] = 0;
    IDarray[2] = 9;
    IDarray[3] = 8;
    IDarray[4] = 17;
    IDarray[5] = 16;
    IDarray[6] = 3;
    IDarray[7] = 2;
    IDarray[8] = 11;
    IDarray[9] = 10;
    IDarray[10] = 19;
    IDarray[11] = 18;
    
    
    init_spi1();
    pc.printf("Begin \n\r");
   // pc.printf("%+d \n\r",p); 
   // p = p & ~(255<< 8);
   // pc.printf("%+d \n\r",p); 
   // p = p >> 1;
   // pc.printf("%d \n\r",p); 
    
    while(1) {
        if(i == 1) {
        for(int x = 1; x < 128; x *= 2) { 
        slaveRx = transfer_spi_slave(x);
        //pc.putc(slaveRx); 
        pc.printf("%d \n\r",slaveRx); 
        }
        for(int x = 128; x > 1; x /= 2) { 
        slaveRx = transfer_spi_slave(x);
        //pc.putc(slaveRx);
        pc.printf("%d \n\r",slaveRx);  
        } 
        }//if(i == 1) 
        
        
        
//------------------------------------------------------------------------------         
        if(i == 0) { 
        slaveRx = transfer_spi_slave(10);
        countx++;
        if(countx == 1) {
            k = slaveRx; 
            k &= ~(255 << 8);
            k = k << 8;
           // if(k == 19) {
                zgHigher = k; 
                //zgHigher = zgHigher << 8;
            //}//if(k == 19)
        }//if(count == 11)
        
        
        else if(countx == 2) {
            k = slaveRx;
            k &= ~(255 << 8); 
            //k = k << 8;
            //if(k == 18) {
                zgLower = k;
                //zgLower = zgLower << 8;
                zGyro = zgHigher + zgLower;
                pc.printf("%+d \n\r",zGyro);
            //}//if(k == 19)
                countx = 0;
        }//else if(count == 12)
        
        }//if(i == 0) 
//------------------------------------------------------------------------------ 
        
        if(i == 2) {
            slaveRx = transfer_spi_slave(10); //get IMU data
            id = slaveRx;                       //Save sample to id for id extraction
            id &= ~(255);                       //get rid of the actual data to only be left with the id
            id =  id >> 8;                      //shift the id to the right for comparison
            //pc.printf("ID: %d \n\r", id);
            if(IDarray[dataCount] == id) {      //compare if the order of data is right and if not wait until it is.
                dataCount++;                      //Increase dataCount as new value has been read in.
                IMUarray[idx] = slaveRx;          //save the newly read value to current free space in the array
                idx++;                            //increment the pointer to point to next free space in the array
            
                if(dataCount == 12) {
                    //reset idx and dataCount
                    dataCount = 0;
                    idx = 0;
                    ProcessAndPrint();       
                }//if(dataCount == 12) 
             }//if(IDarray[dataCount] == id)
             else {
                   // pc.printf("Failed at: %d \n\r", dataCount);
                    dataCount = 0;
                    idx = 0;
             }
       }//if(i == 2)


       
    }
}



void ProcessAndPrint() {
    int16_t MSB = 0;                                    //Store Most Significant Byte of data piece in this variable for processing
    int16_t LSB = 0;                                    //Store Least Significant Byte of data piece in this variable for processing
    char    arrPointer = 0;                                //Array Pointer
//-----------Concatentated-Data-Pieces------------------------------------------
    int16_t gyroX = 0;
    int16_t gyroY = 0;
    int16_t gyroZ = 0;
    
    int16_t accelX = 0;
    int16_t accelY = 0;
    int16_t accelZ = 0;
//-----------Concatentated-Data-Pieces------------------------------------------
    for(char x = 0; x <= 5; x++) {
        MSB = IMUarray[arrPointer];                       //Odd data pieces are MSBs
        MSB &= ~(255<<8);                                 //Mask the MSB bits as they are not part of data
        MSB = MSB << 8;                                   //Shift the Value as its the MSB of the data piece
        arrPointer++;
        LSB = IMUarray[arrPointer];                       //Even data pieces are LSBs
        LSB &= ~(255 << 8);                               //Mask the MSB bits as they are not part of data
        arrPointer++;
        
        switch(x) {
            case 0:
                accelX = MSB + LSB;                       //Combine Accelerometer x-axis data
            break;
            case 1:
                accelY = MSB + LSB;                       //Combine Accelerometer y-axis data
            break;
            case 2:
                accelZ = MSB + LSB;                       //Combine Accelerometer z-axis data
            break;
            case 3:
                gyroX = MSB + LSB;                        //Combine Gyroscope x-axis data
            break;
            case 4:
                gyroY = MSB + LSB;                        //Combine Gyroscope y-axis data
            break;
            case 5:
                gyroZ = MSB + LSB;                        //Combine Gyroscope z-axis data
            break;
            default:
            break; 
        }//switch(x) 
    }//for(char x = 0; x <= 5; x++)
        pc.printf("\r");
        pc.printf("Accel X: %+6d,    Accel Y: %+6d,    Accel Z: %+6d,    Gyro X: %+6d,    Gyro Y: %+6d,    Gyro Z: %+6d", accelX, accelY, accelZ, gyroX, gyroY, gyroZ);
   
}//void ProcessAndPrint()