Diff: main.cpp

Revision:

2:c66f049c90d4

Parent:

1:ebfa9cb235de

--- a/main.cpp	Wed May 21 13:47:07 2014 +0000
+++ b/main.cpp	Thu Aug 13 21:50:01 2015 +0000
@@ -4,6 +4,7 @@
 #include "DebounceIn.h"
 
 #define ACC_SAMPLE_SIZE 200
+#define MAG_SAMPLE_SIZE 200
 #define ACC_X_GAIN 1
 #define ACC_Y_GAIN 2
 #define TRANSFER_SIZE   9
@@ -12,12 +13,15 @@
 
 nRF24L01P my_nrf24l01p(PTD6, PTD7, PTD5, PTD4, PTC12, PTC18);    // mosi, miso, sck, csn, ce, irq
 FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
+FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
 
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
 DigitalOut myled3(LED3);
 MotionSensorDataCounts acc_raw;
+MotionSensorDataCounts mag_raw;
 int16_t acc_x, acc_y;
+int16_t mag_x, mag_y, mag_z;
 DebounceIn fire_button(PTA4);
 DebounceIn cal_button(PTC6);
 
@@ -35,6 +39,21 @@
     
     int acc_x_cal = 0;
     int acc_y_cal = 0;
+    
+    int mag_x_array[MAG_SAMPLE_SIZE];
+    int mag_y_array[MAG_SAMPLE_SIZE];
+    int mag_z_array[MAG_SAMPLE_SIZE];
+    int mag_sample_cnt = 0;
+
+    int mag_x_avg = 0;
+    int mag_y_avg = 0;
+    int mag_z_avg = 0;
+    
+    int mag_x_cal = 0;
+    int mag_y_cal = 0;
+    int mag_z_cal = 0;
+    
+    int new_mag = 0;
 
     pc.baud(115200);
     
@@ -44,16 +63,20 @@
     myled2 = 1;
     
     // Display the setup of the nRF24L01+ chip
-    pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency() );
+    
+    /*pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency() );
     pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  my_nrf24l01p.getRfOutputPower() );
     pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
     pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
     pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
+    
 
     my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
-    my_nrf24l01p.enable();
+    my_nrf24l01p.enable();*/
+    
 
     acc.enable();
+    mag.enable();
     
     myled3 = 0;
     
@@ -84,7 +107,7 @@
         acc_y_avg = (int)(acc_y_avg/ACC_SAMPLE_SIZE);
 
         //pc.printf("%d (%d)\t%d (%d)\n\r",acc_x_avg,acc_raw.x,acc_y_avg,acc_raw.y);
-
+/*
         txData[0] = (acc_x_avg) & 0xff;
         txData[1] = (acc_x_avg>>8)  & 0xff;
         txData[2] = (acc_x_avg>>16)  & 0xff;
@@ -94,16 +117,80 @@
         txData[5] = (acc_y_avg>>8)  & 0xff;
         txData[6] = (acc_y_avg>>16)  & 0xff;
         txData[7] = (acc_y_avg>>24)  & 0xff;
+            
+        txData[8] = (char)(fire_button.read());
+        //txData[9] = (acc_y_avg>>24)  & 0xff;
+                
+        myled1 = fire_button.read();
+        myled3 = !myled1;
+        
+        my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, sizeof( txData ) );
+ */       
+        //below this line added
+        
+        mag.getAxis(mag_raw);
+        
+        mag_x = mag_raw.x - mag_x_cal;
+        mag_y = mag_raw.y - mag_y_cal;
+        mag_z = mag_raw.z - mag_z_cal;
+
+        mag_x_array[mag_sample_cnt]=(int)(mag_x>>2);
+        mag_y_array[mag_sample_cnt]=(int)(mag_y>>2);
+        mag_z_array[mag_sample_cnt]=(int)(mag_z>>2);
+
+        mag_sample_cnt++;
+        if (mag_sample_cnt>=MAG_SAMPLE_SIZE) mag_sample_cnt = 0;
+
+        mag_x_avg=0;
+        mag_y_avg=0;
+        mag_z_avg=0;
+        for (int x=0; x<MAG_SAMPLE_SIZE; x++) {
+            mag_x_avg=mag_x_avg+mag_x_array[x];
+            mag_y_avg=mag_y_avg+mag_y_array[x];
+            mag_z_avg=mag_z_avg+mag_z_array[x];
+        }
+        mag_x_avg = (int)(mag_x_avg/MAG_SAMPLE_SIZE);
+        mag_y_avg = (int)(mag_y_avg/MAG_SAMPLE_SIZE);
+        mag_z_avg = (int)(mag_z_avg/MAG_SAMPLE_SIZE);
+
+        if(mag_x_avg >= 0) new_mag = mag_x_avg*50;
+        if(mag_x_avg < 0) new_mag = mag_y_avg*-50;
+        //new_mag += acc_y_avg;
+        
+        
+        if(acc_x_avg < 0) new_mag += acc_x_avg*3;
+        //if(acc_x_avg < 0 && acc_y_avg < 0) new_mag += acc_x_avg;
+        //if(acc_x_avg > 0 && acc_y_avg > 0) new_mag += acc_x_avg;
+        //if(acc_x_avg > 0 && acc_y_avg < 0) new_mag = new_mag - acc_x_avg;
+        //if(acc_x_avg < 0 && acc_y_avg > 0) new_mag = new_mag - acc_x_avg;
+        
+
+        //pc.printf("%d, %d, %d\n\r",mag_raw.x,mag_raw.y,mag_raw.z);
+        
+        
+        /*txData[0] = (acc_x_avg) & 0xff;
+        txData[1] = (acc_x_avg>>8)  & 0xff;
+        txData[2] = (acc_x_avg>>16)  & 0xff;
+        txData[3] = (acc_x_avg>>24)  & 0xff;
+    
+        txData[4] = (new_mag) & 0xff;
+        txData[5] = (new_mag>>8)  & 0xff;
+        txData[6] = (new_mag>>16)  & 0xff;
+        txData[7] = (new_mag>>24)  & 0xff;
         
         txData[8] = (char)(fire_button.read());
         myled1 = fire_button.read();
         myled3 = !myled1;
         
+        
         my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, sizeof( txData ) );
+        pc.printf("Sending Data: H = %d\tV = %d\r\n", new_mag, acc_x_avg);*/
         
         wait(0.001);
         
+        //Calibration
         if (cal_button.read()==0) {
+            pc.printf("Calibrating");
             myled2 = 0;
             myled3 = 1;
             
@@ -114,9 +201,27 @@
                 acc_x_avg=acc_x_avg+acc_raw.x;
                 acc_y_avg=acc_y_avg+acc_raw.y;
                 wait(0.01);
+                }
+                //Added everything below this line
+            
+            mag_x_avg=0;
+            mag_y_avg=0;
+            mag_z_avg=0;
+            
+            for (int x=0; x<MAG_SAMPLE_SIZE; x++) {
+                mag.getAxis(mag_raw);
+                mag_x_avg=mag_x_avg+mag_raw.x;
+                mag_y_avg=mag_y_avg+mag_raw.y;
+                mag_z_avg=mag_z_avg+mag_raw.z;
+                wait(0.01);  
             }
             acc_x_cal = (int)(acc_x_avg/ACC_SAMPLE_SIZE);
             acc_y_cal = (int)(acc_y_avg/ACC_SAMPLE_SIZE);
+            
+            mag_x_cal = (int)(mag_x_avg/MAG_SAMPLE_SIZE);  //added
+            mag_y_cal = (int)(mag_y_avg/MAG_SAMPLE_SIZE);  //added
+            mag_z_cal = (int)(mag_z_avg/MAG_SAMPLE_SIZE);  //added
+            
             myled2 = 1;
             myled3 = 0;
         }