Rio Harris / Mbed 2 deprecated NerfGun_nRF24L01P_TX_9dof

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

Dependencies:   DebounceIn FXOS8700Q NerfGun_nRF24L01P_TX mbed nRF24L01P

Fork of NerfGun_nRF24L01P_TX by John Mc

main.cpp@2:c66f049c90d4, 2015-08-13 (annotated)

Committer:
b50559
Date:
Thu Aug 13 21:50:01 2015 +0000
Revision:
2:c66f049c90d4
Parent:
1:ebfa9cb235de 
Initial commit.  Not fully functional.  Yaw is impacted by both pitch and roll.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clarkjarvis 0:b5e995814400 1 #include "mbed.h"
clarkjarvis 0:b5e995814400 2 #include "nRF24L01P.h"
clarkjarvis 0:b5e995814400 3 #include "FXOS8700Q.h"
clarkjarvis 0:b5e995814400 4 #include "DebounceIn.h"
clarkjarvis 0:b5e995814400 5
clarkjarvis 0:b5e995814400 6 #define ACC_SAMPLE_SIZE 200
b50559 2:c66f049c90d4 7 #define MAG_SAMPLE_SIZE 200
clarkjarvis 0:b5e995814400 8 #define ACC_X_GAIN 1
clarkjarvis 0:b5e995814400 9 #define ACC_Y_GAIN 2
clarkjarvis 0:b5e995814400 10 #define TRANSFER_SIZE 9
clarkjarvis 0:b5e995814400 11
clarkjarvis 0:b5e995814400 12 Serial pc(USBTX, USBRX); // tx, rx
clarkjarvis 0:b5e995814400 13
clarkjarvis 0:b5e995814400 14 nRF24L01P my_nrf24l01p(PTD6, PTD7, PTD5, PTD4, PTC12, PTC18); // mosi, miso, sck, csn, ce, irq
clarkjarvis 0:b5e995814400 15 FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
b50559 2:c66f049c90d4 16 FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
clarkjarvis 0:b5e995814400 17
clarkjarvis 0:b5e995814400 18 DigitalOut myled1(LED1);
clarkjarvis 0:b5e995814400 19 DigitalOut myled2(LED2);
clarkjarvis 0:b5e995814400 20 DigitalOut myled3(LED3);
clarkjarvis 0:b5e995814400 21 MotionSensorDataCounts acc_raw;
b50559 2:c66f049c90d4 22 MotionSensorDataCounts mag_raw;
clarkjarvis 0:b5e995814400 23 int16_t acc_x, acc_y;
b50559 2:c66f049c90d4 24 int16_t mag_x, mag_y, mag_z;
clarkjarvis 0:b5e995814400 25 DebounceIn fire_button(PTA4);
clarkjarvis 0:b5e995814400 26 DebounceIn cal_button(PTC6);
clarkjarvis 0:b5e995814400 27
clarkjarvis 0:b5e995814400 28 int main() {
clarkjarvis 0:b5e995814400 29
clarkjarvis 0:b5e995814400 30 char txData[TRANSFER_SIZE];
clarkjarvis 0:b5e995814400 31 int txDataCnt = 0;
clarkjarvis 0:b5e995814400 32
clarkjarvis 0:b5e995814400 33 int acc_x_array[ACC_SAMPLE_SIZE];
clarkjarvis 0:b5e995814400 34 int acc_y_array[ACC_SAMPLE_SIZE];
clarkjarvis 0:b5e995814400 35 int acc_sample_cnt = 0;
clarkjarvis 0:b5e995814400 36
clarkjarvis 0:b5e995814400 37 int acc_x_avg = 0;
clarkjarvis 0:b5e995814400 38 int acc_y_avg = 0;
clarkjarvis 0:b5e995814400 39
clarkjarvis 0:b5e995814400 40 int acc_x_cal = 0;
clarkjarvis 0:b5e995814400 41 int acc_y_cal = 0;
b50559 2:c66f049c90d4 42
b50559 2:c66f049c90d4 43 int mag_x_array[MAG_SAMPLE_SIZE];
b50559 2:c66f049c90d4 44 int mag_y_array[MAG_SAMPLE_SIZE];
b50559 2:c66f049c90d4 45 int mag_z_array[MAG_SAMPLE_SIZE];
b50559 2:c66f049c90d4 46 int mag_sample_cnt = 0;
b50559 2:c66f049c90d4 47
b50559 2:c66f049c90d4 48 int mag_x_avg = 0;
b50559 2:c66f049c90d4 49 int mag_y_avg = 0;
b50559 2:c66f049c90d4 50 int mag_z_avg = 0;
b50559 2:c66f049c90d4 51
b50559 2:c66f049c90d4 52 int mag_x_cal = 0;
b50559 2:c66f049c90d4 53 int mag_y_cal = 0;
b50559 2:c66f049c90d4 54 int mag_z_cal = 0;
b50559 2:c66f049c90d4 55
b50559 2:c66f049c90d4 56 int new_mag = 0;
clarkjarvis 0:b5e995814400 57
clarkjarvis 0:b5e995814400 58 pc.baud(115200);
clarkjarvis 0:b5e995814400 59
johnmc 1:ebfa9cb235de 60 my_nrf24l01p.setTxAddress(0xDEADBEEF0F);
clarkjarvis 0:b5e995814400 61 my_nrf24l01p.powerUp();
clarkjarvis 0:b5e995814400 62 myled1 = 1;
clarkjarvis 0:b5e995814400 63 myled2 = 1;
clarkjarvis 0:b5e995814400 64
clarkjarvis 0:b5e995814400 65 // Display the setup of the nRF24L01+ chip
b50559 2:c66f049c90d4 66
b50559 2:c66f049c90d4 67 /*pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", my_nrf24l01p.getRfFrequency() );
clarkjarvis 0:b5e995814400 68 pc.printf( "nRF24L01+ Output power : %d dBm\r\n", my_nrf24l01p.getRfOutputPower() );
clarkjarvis 0:b5e995814400 69 pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
clarkjarvis 0:b5e995814400 70 pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
clarkjarvis 0:b5e995814400 71 pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
b50559 2:c66f049c90d4 72
clarkjarvis 0:b5e995814400 73
clarkjarvis 0:b5e995814400 74 my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
b50559 2:c66f049c90d4 75 my_nrf24l01p.enable();*/
b50559 2:c66f049c90d4 76
clarkjarvis 0:b5e995814400 77
clarkjarvis 0:b5e995814400 78 acc.enable();
b50559 2:c66f049c90d4 79 mag.enable();
clarkjarvis 0:b5e995814400 80
clarkjarvis 0:b5e995814400 81 myled3 = 0;
clarkjarvis 0:b5e995814400 82
clarkjarvis 0:b5e995814400 83 for (int x=0; x<ACC_SAMPLE_SIZE; x++) {
clarkjarvis 0:b5e995814400 84 acc_x_array[x]=0;
clarkjarvis 0:b5e995814400 85 acc_y_array[x]=0;
clarkjarvis 0:b5e995814400 86 }
clarkjarvis 0:b5e995814400 87
clarkjarvis 0:b5e995814400 88 while (1) {
clarkjarvis 0:b5e995814400 89 acc.getAxis(acc_raw);
clarkjarvis 0:b5e995814400 90
clarkjarvis 0:b5e995814400 91 acc_x = acc_raw.x - acc_x_cal;
clarkjarvis 0:b5e995814400 92 acc_y = acc_raw.y - acc_y_cal;
clarkjarvis 0:b5e995814400 93
clarkjarvis 0:b5e995814400 94 acc_x_array[acc_sample_cnt]=(int)(acc_x>>2);
clarkjarvis 0:b5e995814400 95 acc_y_array[acc_sample_cnt]=(int)(acc_y>>2);
clarkjarvis 0:b5e995814400 96
clarkjarvis 0:b5e995814400 97 acc_sample_cnt++;
clarkjarvis 0:b5e995814400 98 if (acc_sample_cnt>=ACC_SAMPLE_SIZE) acc_sample_cnt = 0;
clarkjarvis 0:b5e995814400 99
clarkjarvis 0:b5e995814400 100 acc_x_avg=0;
clarkjarvis 0:b5e995814400 101 acc_y_avg=0;
clarkjarvis 0:b5e995814400 102 for (int x=0; x<ACC_SAMPLE_SIZE; x++) {
clarkjarvis 0:b5e995814400 103 acc_x_avg=acc_x_avg+acc_x_array[x];
clarkjarvis 0:b5e995814400 104 acc_y_avg=acc_y_avg+acc_y_array[x];
clarkjarvis 0:b5e995814400 105 }
clarkjarvis 0:b5e995814400 106 acc_x_avg = (int)(acc_x_avg/ACC_SAMPLE_SIZE);
clarkjarvis 0:b5e995814400 107 acc_y_avg = (int)(acc_y_avg/ACC_SAMPLE_SIZE);
clarkjarvis 0:b5e995814400 108
clarkjarvis 0:b5e995814400 109 //pc.printf("%d (%d)\t%d (%d)\n\r",acc_x_avg,acc_raw.x,acc_y_avg,acc_raw.y);
b50559 2:c66f049c90d4 110 /*
clarkjarvis 0:b5e995814400 111 txData[0] = (acc_x_avg) & 0xff;
clarkjarvis 0:b5e995814400 112 txData[1] = (acc_x_avg>>8) & 0xff;
clarkjarvis 0:b5e995814400 113 txData[2] = (acc_x_avg>>16) & 0xff;
clarkjarvis 0:b5e995814400 114 txData[3] = (acc_x_avg>>24) & 0xff;
clarkjarvis 0:b5e995814400 115
clarkjarvis 0:b5e995814400 116 txData[4] = (acc_y_avg) & 0xff;
clarkjarvis 0:b5e995814400 117 txData[5] = (acc_y_avg>>8) & 0xff;
clarkjarvis 0:b5e995814400 118 txData[6] = (acc_y_avg>>16) & 0xff;
clarkjarvis 0:b5e995814400 119 txData[7] = (acc_y_avg>>24) & 0xff;
b50559 2:c66f049c90d4 120
b50559 2:c66f049c90d4 121 txData[8] = (char)(fire_button.read());
b50559 2:c66f049c90d4 122 //txData[9] = (acc_y_avg>>24) & 0xff;
b50559 2:c66f049c90d4 123
b50559 2:c66f049c90d4 124 myled1 = fire_button.read();
b50559 2:c66f049c90d4 125 myled3 = !myled1;
b50559 2:c66f049c90d4 126
b50559 2:c66f049c90d4 127 my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, sizeof( txData ) );
b50559 2:c66f049c90d4 128 */
b50559 2:c66f049c90d4 129 //below this line added
b50559 2:c66f049c90d4 130
b50559 2:c66f049c90d4 131 mag.getAxis(mag_raw);
b50559 2:c66f049c90d4 132
b50559 2:c66f049c90d4 133 mag_x = mag_raw.x - mag_x_cal;
b50559 2:c66f049c90d4 134 mag_y = mag_raw.y - mag_y_cal;
b50559 2:c66f049c90d4 135 mag_z = mag_raw.z - mag_z_cal;
b50559 2:c66f049c90d4 136
b50559 2:c66f049c90d4 137 mag_x_array[mag_sample_cnt]=(int)(mag_x>>2);
b50559 2:c66f049c90d4 138 mag_y_array[mag_sample_cnt]=(int)(mag_y>>2);
b50559 2:c66f049c90d4 139 mag_z_array[mag_sample_cnt]=(int)(mag_z>>2);
b50559 2:c66f049c90d4 140
b50559 2:c66f049c90d4 141 mag_sample_cnt++;
b50559 2:c66f049c90d4 142 if (mag_sample_cnt>=MAG_SAMPLE_SIZE) mag_sample_cnt = 0;
b50559 2:c66f049c90d4 143
b50559 2:c66f049c90d4 144 mag_x_avg=0;
b50559 2:c66f049c90d4 145 mag_y_avg=0;
b50559 2:c66f049c90d4 146 mag_z_avg=0;
b50559 2:c66f049c90d4 147 for (int x=0; x<MAG_SAMPLE_SIZE; x++) {
b50559 2:c66f049c90d4 148 mag_x_avg=mag_x_avg+mag_x_array[x];
b50559 2:c66f049c90d4 149 mag_y_avg=mag_y_avg+mag_y_array[x];
b50559 2:c66f049c90d4 150 mag_z_avg=mag_z_avg+mag_z_array[x];
b50559 2:c66f049c90d4 151 }
b50559 2:c66f049c90d4 152 mag_x_avg = (int)(mag_x_avg/MAG_SAMPLE_SIZE);
b50559 2:c66f049c90d4 153 mag_y_avg = (int)(mag_y_avg/MAG_SAMPLE_SIZE);
b50559 2:c66f049c90d4 154 mag_z_avg = (int)(mag_z_avg/MAG_SAMPLE_SIZE);
b50559 2:c66f049c90d4 155
b50559 2:c66f049c90d4 156 if(mag_x_avg >= 0) new_mag = mag_x_avg*50;
b50559 2:c66f049c90d4 157 if(mag_x_avg < 0) new_mag = mag_y_avg*-50;
b50559 2:c66f049c90d4 158 //new_mag += acc_y_avg;
b50559 2:c66f049c90d4 159
b50559 2:c66f049c90d4 160
b50559 2:c66f049c90d4 161 if(acc_x_avg < 0) new_mag += acc_x_avg*3;
b50559 2:c66f049c90d4 162 //if(acc_x_avg < 0 && acc_y_avg < 0) new_mag += acc_x_avg;
b50559 2:c66f049c90d4 163 //if(acc_x_avg > 0 && acc_y_avg > 0) new_mag += acc_x_avg;
b50559 2:c66f049c90d4 164 //if(acc_x_avg > 0 && acc_y_avg < 0) new_mag = new_mag - acc_x_avg;
b50559 2:c66f049c90d4 165 //if(acc_x_avg < 0 && acc_y_avg > 0) new_mag = new_mag - acc_x_avg;
b50559 2:c66f049c90d4 166
b50559 2:c66f049c90d4 167
b50559 2:c66f049c90d4 168 //pc.printf("%d, %d, %d\n\r",mag_raw.x,mag_raw.y,mag_raw.z);
b50559 2:c66f049c90d4 169
b50559 2:c66f049c90d4 170
b50559 2:c66f049c90d4 171 /*txData[0] = (acc_x_avg) & 0xff;
b50559 2:c66f049c90d4 172 txData[1] = (acc_x_avg>>8) & 0xff;
b50559 2:c66f049c90d4 173 txData[2] = (acc_x_avg>>16) & 0xff;
b50559 2:c66f049c90d4 174 txData[3] = (acc_x_avg>>24) & 0xff;
b50559 2:c66f049c90d4 175
b50559 2:c66f049c90d4 176 txData[4] = (new_mag) & 0xff;
b50559 2:c66f049c90d4 177 txData[5] = (new_mag>>8) & 0xff;
b50559 2:c66f049c90d4 178 txData[6] = (new_mag>>16) & 0xff;
b50559 2:c66f049c90d4 179 txData[7] = (new_mag>>24) & 0xff;
clarkjarvis 0:b5e995814400 180
clarkjarvis 0:b5e995814400 181 txData[8] = (char)(fire_button.read());
clarkjarvis 0:b5e995814400 182 myled1 = fire_button.read();
clarkjarvis 0:b5e995814400 183 myled3 = !myled1;
clarkjarvis 0:b5e995814400 184
b50559 2:c66f049c90d4 185
clarkjarvis 0:b5e995814400 186 my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, sizeof( txData ) );
b50559 2:c66f049c90d4 187 pc.printf("Sending Data: H = %d\tV = %d\r\n", new_mag, acc_x_avg);*/
clarkjarvis 0:b5e995814400 188
clarkjarvis 0:b5e995814400 189 wait(0.001);
clarkjarvis 0:b5e995814400 190
b50559 2:c66f049c90d4 191 //Calibration
clarkjarvis 0:b5e995814400 192 if (cal_button.read()==0) {
b50559 2:c66f049c90d4 193 pc.printf("Calibrating");
clarkjarvis 0:b5e995814400 194 myled2 = 0;
clarkjarvis 0:b5e995814400 195 myled3 = 1;
clarkjarvis 0:b5e995814400 196
clarkjarvis 0:b5e995814400 197 acc_x_avg=0;
clarkjarvis 0:b5e995814400 198 acc_y_avg=0;
clarkjarvis 0:b5e995814400 199 for (int x=0; x<ACC_SAMPLE_SIZE; x++) {
clarkjarvis 0:b5e995814400 200 acc.getAxis(acc_raw);
clarkjarvis 0:b5e995814400 201 acc_x_avg=acc_x_avg+acc_raw.x;
clarkjarvis 0:b5e995814400 202 acc_y_avg=acc_y_avg+acc_raw.y;
clarkjarvis 0:b5e995814400 203 wait(0.01);
b50559 2:c66f049c90d4 204 }
b50559 2:c66f049c90d4 205 //Added everything below this line
b50559 2:c66f049c90d4 206
b50559 2:c66f049c90d4 207 mag_x_avg=0;
b50559 2:c66f049c90d4 208 mag_y_avg=0;
b50559 2:c66f049c90d4 209 mag_z_avg=0;
b50559 2:c66f049c90d4 210
b50559 2:c66f049c90d4 211 for (int x=0; x<MAG_SAMPLE_SIZE; x++) {
b50559 2:c66f049c90d4 212 mag.getAxis(mag_raw);
b50559 2:c66f049c90d4 213 mag_x_avg=mag_x_avg+mag_raw.x;
b50559 2:c66f049c90d4 214 mag_y_avg=mag_y_avg+mag_raw.y;
b50559 2:c66f049c90d4 215 mag_z_avg=mag_z_avg+mag_raw.z;
b50559 2:c66f049c90d4 216 wait(0.01);
clarkjarvis 0:b5e995814400 217 }
clarkjarvis 0:b5e995814400 218 acc_x_cal = (int)(acc_x_avg/ACC_SAMPLE_SIZE);
clarkjarvis 0:b5e995814400 219 acc_y_cal = (int)(acc_y_avg/ACC_SAMPLE_SIZE);
b50559 2:c66f049c90d4 220
b50559 2:c66f049c90d4 221 mag_x_cal = (int)(mag_x_avg/MAG_SAMPLE_SIZE); //added
b50559 2:c66f049c90d4 222 mag_y_cal = (int)(mag_y_avg/MAG_SAMPLE_SIZE); //added
b50559 2:c66f049c90d4 223 mag_z_cal = (int)(mag_z_avg/MAG_SAMPLE_SIZE); //added
b50559 2:c66f049c90d4 224
clarkjarvis 0:b5e995814400 225 myled2 = 1;
clarkjarvis 0:b5e995814400 226 myled3 = 0;
clarkjarvis 0:b5e995814400 227 }
clarkjarvis 0:b5e995814400 228
clarkjarvis 0:b5e995814400 229 }
clarkjarvis 0:b5e995814400 230 }