Diff: main.cpp

Revision:

3:e33697420c4a

Parent:

2:4cc880ea466d

Child:

4:e36c7042d3bb

diff -r 4cc880ea466d -r e33697420c4a main.cpp
--- a/main.cpp	Wed Feb 06 12:28:59 2019 +0000
+++ b/main.cpp	Tue Feb 19 01:44:53 2019 +0000
@@ -4,20 +4,180 @@
 Serial pc(USBTX, USBRX); 
 
 int masterRx = 0;
-unsigned int slaveRx = 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.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.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()
+