Rio Harris / Mbed 2 deprecated NerfGun_nRF24L01P_TX_9d0f

This code includes FRDM-STBC-AGM01 in order to add 9dof Sensor Fusion to Nerf Gun Demo. 9dof = accelerometer, magnetometer and gyro.

Dependencies:   DebounceIn FXAS21000 FXOS8700Q Madgwick_Algorithm Mahony_Algorithm mbed nRF24L01P

Fork of FRDM-STBC-AGM01 by angus taggart

The goal of this project is to incorporate sensor fusion capability to the already existing Nerf Gun Demo. The demo is controlled using Freescale's FRDM-K64F. I'm looking to add the FRDM-STBC-AGM01 to the project, which not only has FSL's combo accelerometer and magnetometer part FXOS8700Q but also FSL's gyro part FXAS21000. Hopefully the combination of all three sensors for 9dof sensor fusion will improve the mobility and control of the the demo's movement.

Details of the project can be found on the Freescale online community at https://community.freescale.com/groups/demolab/projects/fun-with-sensorfusion-nerf-gun-and-openrov-projects.

main.cpp@4:651a5029e003, 2015-08-14 (annotated)

Committer:
igalloway
Date:
Fri Aug 14 15:15:14 2015 +0000
Revision:
4:651a5029e003
Parent:
3:10f487bae65c 
added a comment in main.; Note the FXAS2100 library may have switched back to jim Carvers - may need to get Rio's again.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
igalloway 4:651a5029e003 1 //This is an intern project to port 9DOF sensor fusion to the Nerf gun.
igalloway 4:651a5029e003 2
screamer 0:bfb567985c64 3 #include "mbed.h"
screamer 0:bfb567985c64 4 #include "FXOS8700Q.h"
screamer 0:bfb567985c64 5 #include "FXAS21000.h"
b50559 3:10f487bae65c 6 #include "MadgwickAHRS.h"
b50559 3:10f487bae65c 7 #include "MahonyAHRS.h"
b50559 3:10f487bae65c 8 #include "nRF24L01P.h"
b50559 3:10f487bae65c 9 #include "DebounceIn.h"
screamer 0:bfb567985c64 10
b50559 3:10f487bae65c 11 #include <iostream>
b50559 3:10f487bae65c 12 using namespace std;
b50559 3:10f487bae65c 13
b50559 3:10f487bae65c 14 #define TRANSFER_SIZE 9
screamer 0:bfb567985c64 15
screamer 0:bfb567985c64 16 FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
screamer 0:bfb567985c64 17 FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
screamer 0:bfb567985c64 18 FXAS21000 gyro(D14, D15);
b50559 3:10f487bae65c 19 nRF24L01P my_nrf24l01p(PTD6, PTD7, PTD5, PTD4, PTC12, PTC18); // mosi, miso, sck, csn, ce, irq
screamer 0:bfb567985c64 20
b50559 3:10f487bae65c 21 DigitalOut myled1(LED1);
b50559 3:10f487bae65c 22 DigitalOut myled2(LED2);
b50559 3:10f487bae65c 23 DigitalOut myled3(LED3);
screamer 0:bfb567985c64 24
screamer 0:bfb567985c64 25 Serial pc(USBTX, USBRX);
screamer 0:bfb567985c64 26
b50559 3:10f487bae65c 27 DebounceIn fire_button(PTA4);
b50559 3:10f487bae65c 28 DebounceIn cal_button(PTC6);
b50559 3:10f487bae65c 29
screamer 0:bfb567985c64 30 int main()
screamer 0:bfb567985c64 31 {
b50559 3:10f487bae65c 32
b50559 3:10f487bae65c 33 pc.baud(115200);
b50559 3:10f487bae65c 34
b50559 3:10f487bae65c 35 MahonyAHRS ahrs(50.0f);
ajtag 2:0cccf85f9b3f 36
b50559 3:10f487bae65c 37 int16_t roll;
b50559 3:10f487bae65c 38 int16_t pitch;
b50559 3:10f487bae65c 39 int16_t yaw;
b50559 3:10f487bae65c 40 int16_t avg_roll = 0;
b50559 3:10f487bae65c 41 int16_t avg_pitch = 0;
b50559 3:10f487bae65c 42 int16_t avg_yaw = 0;
b50559 3:10f487bae65c 43 int16_t off_yaw = 0;
b50559 3:10f487bae65c 44
ajtag 2:0cccf85f9b3f 45 float gyro_data[3];
screamer 0:bfb567985c64 46 MotionSensorDataUnits adata;
screamer 0:bfb567985c64 47 MotionSensorDataUnits mdata;
screamer 0:bfb567985c64 48
b50559 3:10f487bae65c 49 float acc[3];
b50559 3:10f487bae65c 50 float mag[3];
b50559 3:10f487bae65c 51 float gyr[3];
b50559 3:10f487bae65c 52 float avg_acc[3];
b50559 3:10f487bae65c 53 float avg_mag[3];
b50559 3:10f487bae65c 54 float avg_gyr[3];
b50559 3:10f487bae65c 55
b50559 3:10f487bae65c 56 char txData[TRANSFER_SIZE];
b50559 3:10f487bae65c 57
b50559 3:10f487bae65c 58 my_nrf24l01p.setTxAddress(0xDEADBEEF0F);
b50559 3:10f487bae65c 59 my_nrf24l01p.powerUp();
b50559 3:10f487bae65c 60 myled1 = 1;
b50559 3:10f487bae65c 61 myled2 = 1;
b50559 3:10f487bae65c 62
b50559 3:10f487bae65c 63 // Display the setup of the nRF24L01+ chip
b50559 3:10f487bae65c 64
b50559 3:10f487bae65c 65 pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", my_nrf24l01p.getRfFrequency() );
b50559 3:10f487bae65c 66 pc.printf( "nRF24L01+ Output power : %d dBm\r\n", my_nrf24l01p.getRfOutputPower() );
b50559 3:10f487bae65c 67 pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
b50559 3:10f487bae65c 68 pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
b50559 3:10f487bae65c 69 pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
b50559 3:10f487bae65c 70
screamer 0:bfb567985c64 71
b50559 3:10f487bae65c 72 my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
b50559 3:10f487bae65c 73 my_nrf24l01p.enable();
b50559 3:10f487bae65c 74
b50559 3:10f487bae65c 75 //=============================================================================================
screamer 0:bfb567985c64 76 combo_acc.enable();
screamer 0:bfb567985c64 77 combo_mag.enable();
b50559 3:10f487bae65c 78
b50559 3:10f487bae65c 79 //Calibration
b50559 3:10f487bae65c 80 printf("Please Set Device Right Side Up on a Flat Surface\r\n");
b50559 3:10f487bae65c 81 printf("Calibrating...\r\n");
b50559 3:10f487bae65c 82
b50559 3:10f487bae65c 83 int count = 0;
b50559 3:10f487bae65c 84 int samples = 100;
b50559 3:10f487bae65c 85 int i;
b50559 3:10f487bae65c 86
b50559 3:10f487bae65c 87 float cal_adatax = 0;
b50559 3:10f487bae65c 88 float cal_adatay = 0;
b50559 3:10f487bae65c 89 float cal_adataz = 0;
b50559 3:10f487bae65c 90
b50559 3:10f487bae65c 91 float cal_gdatax = 0;
b50559 3:10f487bae65c 92 float cal_gdatay = 0;
b50559 3:10f487bae65c 93 float cal_gdataz = 0;
b50559 3:10f487bae65c 94
b50559 3:10f487bae65c 95 //Transformation matrix obtained via MagMaster 1.0
b50559 3:10f487bae65c 96 float cal_mdatax[3] = {1.012, 0.03, -0.024};
b50559 3:10f487bae65c 97 float cal_mdatay[3] = {-0.021, 0.997, -0.044};
b50559 3:10f487bae65c 98 float cal_mdataz[3] = {0.008, -0.012, 0.993};
b50559 3:10f487bae65c 99 float bias[3] = {0.008,0.018,0.003};
ajtag 2:0cccf85f9b3f 100
b50559 3:10f487bae65c 101 while(count < samples) {
b50559 3:10f487bae65c 102 combo_acc.getAxis(adata);
b50559 3:10f487bae65c 103 gyro.ReadXYZ(gyro_data);
b50559 3:10f487bae65c 104
b50559 3:10f487bae65c 105 cal_adatax += adata.x;
b50559 3:10f487bae65c 106 cal_adatay += adata.y;
b50559 3:10f487bae65c 107 cal_adataz += adata.z;
b50559 3:10f487bae65c 108 cal_gdatax += gyro_data[0];
b50559 3:10f487bae65c 109 cal_gdatay += gyro_data[1];
b50559 3:10f487bae65c 110 cal_gdataz += gyro_data[2];
b50559 3:10f487bae65c 111
b50559 3:10f487bae65c 112 count++;
b50559 3:10f487bae65c 113 wait(.1);
b50559 3:10f487bae65c 114 }
b50559 3:10f487bae65c 115
b50559 3:10f487bae65c 116 cal_adatax = -cal_adatax / samples;
b50559 3:10f487bae65c 117 cal_adatay = -cal_adatay / samples;
b50559 3:10f487bae65c 118 cal_adataz = -cal_adataz / samples + 1;
b50559 3:10f487bae65c 119 cal_gdatax = -cal_gdatax / samples;
b50559 3:10f487bae65c 120 cal_gdatay = -cal_gdatay / samples;
b50559 3:10f487bae65c 121 cal_gdataz = -cal_gdataz / samples;
b50559 3:10f487bae65c 122
b50559 3:10f487bae65c 123 printf("\r\n");
screamer 0:bfb567985c64 124
b50559 3:10f487bae65c 125 //==============================================================================================
b50559 3:10f487bae65c 126
b50559 3:10f487bae65c 127 count = 0;
screamer 0:bfb567985c64 128
screamer 0:bfb567985c64 129 while(1) {
b50559 3:10f487bae65c 130
b50559 3:10f487bae65c 131 myled1 = 0;
screamer 0:bfb567985c64 132
b50559 3:10f487bae65c 133 i = 0;
b50559 3:10f487bae65c 134 while(i < samples){
b50559 3:10f487bae65c 135
b50559 3:10f487bae65c 136 //Read sensors
b50559 3:10f487bae65c 137 combo_acc.getAxis(adata);
b50559 3:10f487bae65c 138 combo_mag.getAxis(mdata);
b50559 3:10f487bae65c 139 gyro.ReadXYZ(gyro_data);
b50559 3:10f487bae65c 140 avg_acc[0] = avg_gyr[0] + gyr[0];
b50559 3:10f487bae65c 141 avg_acc[1] = avg_acc[1] + acc[1];
b50559 3:10f487bae65c 142 avg_acc[2] = avg_acc[2] + acc[2];
b50559 3:10f487bae65c 143 avg_mag[0] = avg_mag[0] + mag[0];
b50559 3:10f487bae65c 144 avg_mag[1] = avg_mag[1] + mag[1];
b50559 3:10f487bae65c 145 avg_mag[2] = avg_mag[2] + mag[2];
b50559 3:10f487bae65c 146 avg_gyr[0] = avg_gyr[0] + gyr[0];
b50559 3:10f487bae65c 147 avg_gyr[1] = avg_gyr[1] + gyr[1];
b50559 3:10f487bae65c 148 avg_gyr[2] = avg_gyr[2] + gyr[2];
b50559 3:10f487bae65c 149 i++;
b50559 3:10f487bae65c 150 }
b50559 3:10f487bae65c 151
b50559 3:10f487bae65c 152 acc[0] = avg_acc[0]/samples;
b50559 3:10f487bae65c 153 acc[1] = avg_acc[1]/samples;
b50559 3:10f487bae65c 154 acc[2] = avg_acc[2]/samples;
b50559 3:10f487bae65c 155 mag[0] = avg_mag[0]/samples;
b50559 3:10f487bae65c 156 mag[1] = avg_mag[1]/samples;
b50559 3:10f487bae65c 157 mag[2] = avg_mag[2]/samples;
b50559 3:10f487bae65c 158 gyr[0] = avg_gyr[0]/samples;
b50559 3:10f487bae65c 159 gyr[1] = avg_gyr[1]/samples;
b50559 3:10f487bae65c 160 gyr[2] = avg_gyr[2]/samples;
b50559 3:10f487bae65c 161
b50559 3:10f487bae65c 162 //Apply accelerometer zero offset calibration
b50559 3:10f487bae65c 163 acc[0] = adata.x + cal_adatax;
b50559 3:10f487bae65c 164 acc[1] = adata.y + cal_adatay;
b50559 3:10f487bae65c 165 acc[2] = adata.z + cal_adataz;
b50559 3:10f487bae65c 166
b50559 3:10f487bae65c 167 //Apply gyro zero offset calibration
b50559 3:10f487bae65c 168 gyr[0] = gyro_data[0] + cal_gdatax;
b50559 3:10f487bae65c 169 gyr[1] = gyro_data[1] + cal_gdatay;
b50559 3:10f487bae65c 170 gyr[2] = gyro_data[2] + cal_gdataz;
screamer 0:bfb567985c64 171
b50559 3:10f487bae65c 172 //Apply magnetometer soft-iron calibration
b50559 3:10f487bae65c 173 mag[0] = (mdata.x-bias[0])*cal_mdatax[0] + (mdata.y-bias[1])*cal_mdatax[1] + (mdata.z-bias[2])*cal_mdatax[2];
b50559 3:10f487bae65c 174 mag[1] = (mdata.x-bias[0])*cal_mdatay[0] + (mdata.y-bias[1])*cal_mdatay[1] + (mdata.z-bias[2])*cal_mdatay[2];
b50559 3:10f487bae65c 175 mag[2] = (mdata.x-bias[0])*cal_mdataz[0] + (mdata.y-bias[1])*cal_mdataz[1] + (mdata.z-bias[2])*cal_mdataz[2];
b50559 3:10f487bae65c 176
b50559 3:10f487bae65c 177
b50559 3:10f487bae65c 178 ahrs.update(gyr[0], gyr[1], gyr[2], acc[0], acc[1], acc[2], mag[0], mag[1], mag[2]);
b50559 3:10f487bae65c 179 ahrs.getEuler();
b50559 3:10f487bae65c 180
b50559 3:10f487bae65c 181 roll = ahrs.getRoll() * 8;
b50559 3:10f487bae65c 182 pitch = ahrs.getPitch() * 8;
b50559 3:10f487bae65c 183 yaw = (ahrs.getYaw() - off_yaw) * 8;
b50559 3:10f487bae65c 184
b50559 3:10f487bae65c 185 count ++;
b50559 3:10f487bae65c 186 if(count == 1){
b50559 3:10f487bae65c 187 off_yaw = yaw;
b50559 3:10f487bae65c 188 yaw = yaw - off_yaw;
b50559 3:10f487bae65c 189 }
screamer 0:bfb567985c64 190
b50559 3:10f487bae65c 191 //printf("Roll: %d\tPitch: %d\tYaw: %d\r\n", roll, pitch, yaw);
b50559 3:10f487bae65c 192 //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]);
screamer 0:bfb567985c64 193
b50559 3:10f487bae65c 194 txData[0] = (pitch) & 0xff;
b50559 3:10f487bae65c 195 txData[1] = (pitch>>8) & 0xff;
b50559 3:10f487bae65c 196 txData[2] = (pitch>>16) & 0xff;
b50559 3:10f487bae65c 197 txData[3] = (pitch>>24) & 0xff;
b50559 3:10f487bae65c 198
b50559 3:10f487bae65c 199 txData[4] = (yaw) & 0xff;
b50559 3:10f487bae65c 200 txData[5] = (yaw>>8) & 0xff;
b50559 3:10f487bae65c 201 txData[6] = (yaw>>16) & 0xff;
b50559 3:10f487bae65c 202 txData[7] = (yaw>>24) & 0xff;
b50559 3:10f487bae65c 203
b50559 3:10f487bae65c 204 txData[8] = (char)(fire_button.read());
b50559 3:10f487bae65c 205
b50559 3:10f487bae65c 206 myled1 = fire_button.read();
b50559 3:10f487bae65c 207 myled3 = !myled1;
ajtag 2:0cccf85f9b3f 208
b50559 3:10f487bae65c 209 my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, sizeof( txData ) );
b50559 3:10f487bae65c 210
b50559 3:10f487bae65c 211 wait(.001);
b50559 3:10f487bae65c 212
b50559 3:10f487bae65c 213
screamer 0:bfb567985c64 214 }
screamer 0:bfb567985c64 215 }