main.cpp

00001 //This is an intern project to port 9DOF sensor fusion to the Nerf gun.
00002 
00003 #include "mbed.h"
00004 #include "FXOS8700Q.h"
00005 #include "FXAS21000.h"
00006 #include "MadgwickAHRS.h"
00007 #include "MahonyAHRS.h"
00008 #include "nRF24L01P.h"
00009 #include "DebounceIn.h"
00010 
00011 #include <iostream>
00012 using namespace std;
00013 
00014 #define TRANSFER_SIZE   9
00015 
00016 FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
00017 FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
00018 FXAS21000 gyro(D14, D15);
00019 nRF24L01P my_nrf24l01p(PTD6, PTD7, PTD5, PTD4, PTC12, PTC18);    // mosi, miso, sck, csn, ce, irq
00020 
00021 DigitalOut myled1(LED1);
00022 DigitalOut myled2(LED2);
00023 DigitalOut myled3(LED3);
00024 
00025 Serial pc(USBTX, USBRX);
00026 
00027 DebounceIn fire_button(PTA4);
00028 DebounceIn cal_button(PTC6);
00029 
00030 int main()
00031 {
00032 
00033     pc.baud(115200);
00034 
00035     MahonyAHRS ahrs(50.0f);
00036     
00037     int16_t roll;
00038     int16_t pitch;
00039     int16_t yaw;
00040     int16_t avg_roll = 0;
00041     int16_t avg_pitch = 0;
00042     int16_t avg_yaw = 0;
00043     int16_t off_yaw = 0;
00044 
00045     float gyro_data[3];
00046     MotionSensorDataUnits adata;
00047     MotionSensorDataUnits mdata;
00048 
00049     float acc[3];
00050     float mag[3];
00051     float gyr[3];
00052     float avg_acc[3];
00053     float avg_mag[3];
00054     float avg_gyr[3];
00055     
00056     char txData[TRANSFER_SIZE];
00057     
00058     my_nrf24l01p.setTxAddress(0xDEADBEEF0F);
00059     my_nrf24l01p.powerUp();
00060     myled1 = 1;
00061     myled2 = 1;
00062     
00063     // Display the setup of the nRF24L01+ chip
00064     
00065     pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency() );
00066     pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  my_nrf24l01p.getRfOutputPower() );
00067     pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
00068     pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
00069     pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
00070     
00071 
00072     my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
00073     my_nrf24l01p.enable();
00074 
00075     //=============================================================================================
00076     combo_acc.enable();
00077     combo_mag.enable();
00078 
00079     //Calibration
00080     printf("Please Set Device Right Side Up on a Flat Surface\r\n");
00081     printf("Calibrating...\r\n");
00082 
00083     int count = 0;
00084     int samples = 100;
00085     int i;
00086 
00087     float cal_adatax = 0;
00088     float cal_adatay = 0;
00089     float cal_adataz = 0;
00090 
00091     float cal_gdatax = 0;
00092     float cal_gdatay = 0;
00093     float cal_gdataz = 0;
00094 
00095     //Transformation matrix obtained via MagMaster 1.0
00096     float cal_mdatax[3] = {1.012, 0.03, -0.024};
00097     float cal_mdatay[3] = {-0.021, 0.997, -0.044};
00098     float cal_mdataz[3] = {0.008, -0.012, 0.993};
00099     float bias[3] = {0.008,0.018,0.003};
00100 
00101     while(count < samples) {
00102         combo_acc.getAxis(adata);
00103         gyro.ReadXYZ(gyro_data);
00104 
00105         cal_adatax += adata.x;
00106         cal_adatay += adata.y;
00107         cal_adataz += adata.z;
00108         cal_gdatax += gyro_data[0];
00109         cal_gdatay += gyro_data[1];
00110         cal_gdataz += gyro_data[2];
00111 
00112         count++;
00113         wait(.1);
00114     }
00115 
00116     cal_adatax = -cal_adatax / samples;
00117     cal_adatay = -cal_adatay / samples;
00118     cal_adataz = -cal_adataz / samples + 1;
00119     cal_gdatax = -cal_gdatax / samples;
00120     cal_gdatay = -cal_gdatay / samples;
00121     cal_gdataz = -cal_gdataz / samples;
00122 
00123     printf("\r\n");
00124     
00125     //==============================================================================================
00126 
00127       count = 0;
00128     
00129     while(1) {
00130 
00131         myled1 = 0;
00132         
00133         i = 0;
00134         while(i < samples){
00135             
00136             //Read sensors
00137             combo_acc.getAxis(adata);
00138             combo_mag.getAxis(mdata);
00139             gyro.ReadXYZ(gyro_data);
00140             avg_acc[0] = avg_gyr[0] + gyr[0];
00141             avg_acc[1] = avg_acc[1] + acc[1];
00142             avg_acc[2] = avg_acc[2] + acc[2];
00143             avg_mag[0] = avg_mag[0] + mag[0];
00144             avg_mag[1] = avg_mag[1] + mag[1];
00145             avg_mag[2] = avg_mag[2] + mag[2];
00146             avg_gyr[0] = avg_gyr[0] + gyr[0];
00147             avg_gyr[1] = avg_gyr[1] + gyr[1];
00148             avg_gyr[2] = avg_gyr[2] + gyr[2];
00149             i++;
00150             }
00151             
00152             acc[0] = avg_acc[0]/samples;
00153             acc[1] = avg_acc[1]/samples;
00154             acc[2] = avg_acc[2]/samples;
00155             mag[0] = avg_mag[0]/samples;
00156             mag[1] = avg_mag[1]/samples;
00157             mag[2] = avg_mag[2]/samples;
00158             gyr[0] = avg_gyr[0]/samples;
00159             gyr[1] = avg_gyr[1]/samples;
00160             gyr[2] = avg_gyr[2]/samples;
00161 
00162         //Apply accelerometer zero offset calibration
00163         acc[0] = adata.x + cal_adatax;
00164         acc[1] = adata.y + cal_adatay;
00165         acc[2] = adata.z + cal_adataz;
00166 
00167         //Apply gyro zero offset calibration
00168         gyr[0] = gyro_data[0] + cal_gdatax;
00169         gyr[1] = gyro_data[1] + cal_gdatay;
00170         gyr[2] = gyro_data[2] + cal_gdataz;
00171 
00172         //Apply magnetometer soft-iron calibration
00173         mag[0] = (mdata.x-bias[0])*cal_mdatax[0] + (mdata.y-bias[1])*cal_mdatax[1] + (mdata.z-bias[2])*cal_mdatax[2];
00174         mag[1] = (mdata.x-bias[0])*cal_mdatay[0] + (mdata.y-bias[1])*cal_mdatay[1] + (mdata.z-bias[2])*cal_mdatay[2];
00175         mag[2] = (mdata.x-bias[0])*cal_mdataz[0] + (mdata.y-bias[1])*cal_mdataz[1] + (mdata.z-bias[2])*cal_mdataz[2];
00176 
00177 
00178         ahrs.update(gyr[0], gyr[1], gyr[2], acc[0], acc[1], acc[2], mag[0], mag[1], mag[2]);
00179         ahrs.getEuler();
00180         
00181         roll = ahrs.getRoll() * 8;
00182         pitch = ahrs.getPitch() * 8;
00183         yaw = (ahrs.getYaw() - off_yaw) * 8;
00184         
00185         count ++;
00186         if(count == 1){
00187             off_yaw = yaw;
00188             yaw = yaw - off_yaw;
00189             }
00190         
00191         //printf("Roll: %d\tPitch: %d\tYaw: %d\r\n", roll, pitch, yaw);
00192         //printf("Acc: %6.3f, %6.3f, %6.3f    Mag: %6.3f, %6.3f, %6.3f    Gyro: %6.3f, %6.3f, %6.3f\r\n", acc[0], acc[1], acc[2], mag[0], mag[1], mag[2], gyr[0], gyr[1], gyr[2]);
00193         
00194         txData[0] = (pitch) & 0xff;
00195         txData[1] = (pitch>>8)  & 0xff;
00196         txData[2] = (pitch>>16)  & 0xff;
00197         txData[3] = (pitch>>24)  & 0xff;
00198     
00199         txData[4] = (yaw) & 0xff;
00200         txData[5] = (yaw>>8)  & 0xff;
00201         txData[6] = (yaw>>16)  & 0xff;
00202         txData[7] = (yaw>>24)  & 0xff;
00203             
00204         txData[8] = (char)(fire_button.read());
00205                 
00206         myled1 = fire_button.read();
00207         myled3 = !myled1;
00208         
00209         my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, sizeof( txData ) );
00210                
00211         wait(.001);
00212         
00213                 
00214     }
00215 }