File content as of revision 2:c66f049c90d4:

#include "mbed.h"
#include "nRF24L01P.h"
#include "FXOS8700Q.h"
#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

Serial pc(USBTX, USBRX); // tx, rx

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);

int main() {

    char txData[TRANSFER_SIZE];
    int txDataCnt = 0;

    int acc_x_array[ACC_SAMPLE_SIZE];
    int acc_y_array[ACC_SAMPLE_SIZE];
    int acc_sample_cnt = 0;

    int acc_x_avg = 0;
    int acc_y_avg = 0;
    
    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);
    
    my_nrf24l01p.setTxAddress(0xDEADBEEF0F);
    my_nrf24l01p.powerUp();
    myled1 = 1;
    myled2 = 1;
    
    // Display the setup of the nRF24L01+ chip
    
    /*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();*/
    

    acc.enable();
    mag.enable();
    
    myled3 = 0;
    
    for (int x=0; x<ACC_SAMPLE_SIZE; x++) {
        acc_x_array[x]=0;
        acc_y_array[x]=0;
    }
    
    while (1) {
        acc.getAxis(acc_raw);
        
        acc_x = acc_raw.x - acc_x_cal;
        acc_y = acc_raw.y - acc_y_cal;

        acc_x_array[acc_sample_cnt]=(int)(acc_x>>2);
        acc_y_array[acc_sample_cnt]=(int)(acc_y>>2);

        acc_sample_cnt++;
        if (acc_sample_cnt>=ACC_SAMPLE_SIZE) acc_sample_cnt = 0;

        acc_x_avg=0;
        acc_y_avg=0;
        for (int x=0; x<ACC_SAMPLE_SIZE; x++) {
            acc_x_avg=acc_x_avg+acc_x_array[x];
            acc_y_avg=acc_y_avg+acc_y_array[x];
        }
        acc_x_avg = (int)(acc_x_avg/ACC_SAMPLE_SIZE);
        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;
        txData[3] = (acc_x_avg>>24)  & 0xff;
    
        txData[4] = (acc_y_avg) & 0xff;
        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;
            
            acc_x_avg=0;
            acc_y_avg=0;
            for (int x=0; x<ACC_SAMPLE_SIZE; x++) {
                acc.getAxis(acc_raw);
                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;
        }
    
    }
}