2Chx3dof Magnetrometer supported M-Series Random Sequence Generator Servo Control
Sampling Frequency
Sampling Frequency in main.cpp
#define SampleFreq 200 // [Hz]
Auto Stop Setting
Auto-stop Timer 15sec after
// auto-stop when 15sec after if(smpl_cnt>3000){stop_dump();}
The number of 3000 means Sample Count. The number is given by SampleFreq[Hz] * Auto-Stop Time [sec].
M-Series Random Sequence
M-series Random Update Term in main.cpp
// M-series update flag #define M_TERM 200;
Unit is sample count.
cf.) 200 equals to 200 [samples] which equals to 1 [second] where SampleFreq = 200 [Hz}.
See above.
M-Series Random Servo Control
main.cpp@11:f23a77c2296d, 2021-02-10 (annotated)
- Committer:
- mfurukawa
- Date:
- Wed Feb 10 21:59:00 2021 +0000
- Branch:
- MPU-9250-MagSensServo
- Revision:
- 11:f23a77c2296d
- Parent:
- 10:f5a805d998d6
e
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mfurukawa | 0:4656a133ed1a | 1 | /** |
mfurukawa | 0:4656a133ed1a | 2 | * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp |
mfurukawa | 0:4656a133ed1a | 3 | * |
mfurukawa | 0:4656a133ed1a | 4 | * Dec 26, 2017 |
mfurukawa | 0:4656a133ed1a | 5 | * Aug 29, 2018 |
mfurukawa | 0:4656a133ed1a | 6 | * Dec 17, 2019 @ Digital Low Pass Filter 5Hz -> No LPF |
mfurukawa | 0:4656a133ed1a | 7 | @ ACC range 2G -> 4G |
mfurukawa | 0:4656a133ed1a | 8 | @ GYRO range 250 -> 500 Degree per second |
mfurukawa | 0:4656a133ed1a | 9 | @ Deleted magnet sensor checking |
mfurukawa | 0:4656a133ed1a | 10 | @ Set Acc Data Rates, Enable Acc LPF , Bandwidth 218Hz |
mfurukawa | 0:4656a133ed1a | 11 | @ Use DLPF set Gyroscope bandwidth 5Hz, temperature bandwidth 5Hz |
mfurukawa | 8:07c3cb01a5b6 | 12 | * Feb 1, 2021 @ Magnetro Meter Ch1 & Ch3, |
mfurukawa | 8:07c3cb01a5b6 | 13 | @ 2000DPS, 2G, DLP 5Hz |
mfurukawa | 8:07c3cb01a5b6 | 14 | @ M-Series Random Sequence Added |
mfurukawa | 8:07c3cb01a5b6 | 15 | @ Servo Motor PWM (p22), |
mfurukawa | 8:07c3cb01a5b6 | 16 | @ Text Dump is prioritized |
mfurukawa | 8:07c3cb01a5b6 | 17 | @ LED Flashing to appeal ERROR State |
mfurukawa | 0:4656a133ed1a | 18 | * |
mfurukawa | 0:4656a133ed1a | 19 | * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch4) |
mfurukawa | 0:4656a133ed1a | 20 | * |
mfurukawa | 0:4656a133ed1a | 21 | **/ |
mfurukawa | 0:4656a133ed1a | 22 | |
mfurukawa | 0:4656a133ed1a | 23 | /* |
mfurukawa | 0:4656a133ed1a | 24 | https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf |
mfurukawa | 0:4656a133ed1a | 25 | |
mfurukawa | 0:4656a133ed1a | 26 | 3.3 Magnetometer Specifications |
mfurukawa | 0:4656a133ed1a | 27 | |
mfurukawa | 0:4656a133ed1a | 28 | Typical Operating Circuit of section 4.2, |
mfurukawa | 0:4656a133ed1a | 29 | VDD = 2.5V, |
mfurukawa | 0:4656a133ed1a | 30 | VDDIO = 2.5V, |
mfurukawa | 0:4656a133ed1a | 31 | TA=25°C, unless otherwise noted. |
mfurukawa | 0:4656a133ed1a | 32 | |
mfurukawa | 0:4656a133ed1a | 33 | PARAMETER CONDITIONS MIN TYP MAX UNITS |
mfurukawa | 0:4656a133ed1a | 34 | MAGNETOMETER SENSITIVITY |
mfurukawa | 0:4656a133ed1a | 35 | Full-Scale Range ±4800 µT |
mfurukawa | 0:4656a133ed1a | 36 | ADC Word Length 14 bits |
mfurukawa | 0:4656a133ed1a | 37 | Sensitivity Scale Factor 0.6 µT / LSB |
mfurukawa | 0:4656a133ed1a | 38 | ZERO-FIELD OUTPUT |
mfurukawa | 0:4656a133ed1a | 39 | Initial Calibration Tolerance ±500 LSB |
mfurukawa | 0:4656a133ed1a | 40 | */ |
mfurukawa | 0:4656a133ed1a | 41 | |
mfurukawa | 0:4656a133ed1a | 42 | |
mfurukawa | 0:4656a133ed1a | 43 | #include "mbed.h" |
mfurukawa | 0:4656a133ed1a | 44 | #include "MPU9250.h" |
mfurukawa | 1:3bcd844dd707 | 45 | #include "KST_Servo.h" |
mfurukawa | 1:3bcd844dd707 | 46 | #include "M-Series.h" |
mfurukawa | 3:70be84fad39e | 47 | #include "error_led_flash.h" |
mfurukawa | 0:4656a133ed1a | 48 | |
mfurukawa | 0:4656a133ed1a | 49 | /* MPU9250 Library |
mfurukawa | 0:4656a133ed1a | 50 | * |
mfurukawa | 0:4656a133ed1a | 51 | * https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp |
mfurukawa | 0:4656a133ed1a | 52 | * |
mfurukawa | 0:4656a133ed1a | 53 | * MOSI (Master Out Slave In) p5 |
mfurukawa | 0:4656a133ed1a | 54 | * MISO (Master In Slave Out p6 |
mfurukawa | 0:4656a133ed1a | 55 | * SCK (Serial Clock) p7 |
mfurukawa | 0:4656a133ed1a | 56 | * ~CS (Chip Select) p8 -> p30 |
mfurukawa | 0:4656a133ed1a | 57 | */ |
mfurukawa | 0:4656a133ed1a | 58 | |
mfurukawa | 0:4656a133ed1a | 59 | // define serial objects |
mfurukawa | 0:4656a133ed1a | 60 | Serial pc(USBTX, USBRX); |
mfurukawa | 0:4656a133ed1a | 61 | |
mfurukawa | 6:493df7718ecb | 62 | long int smpl_cnt = 0; |
mfurukawa | 0:4656a133ed1a | 63 | Ticker ticker; |
mfurukawa | 0:4656a133ed1a | 64 | Timer timer; |
mfurukawa | 0:4656a133ed1a | 65 | |
mfurukawa | 3:70be84fad39e | 66 | #define SampleFreq 200 // [Hz] |
mfurukawa | 0:4656a133ed1a | 67 | #define nCh 4 // number of ch |
mfurukawa | 0:4656a133ed1a | 68 | #define baudRate 921600 //921600 / 115200 |
mfurukawa | 0:4656a133ed1a | 69 | |
mfurukawa | 0:4656a133ed1a | 70 | unsigned int counter = 0; |
mfurukawa | 0:4656a133ed1a | 71 | unsigned int usCycle = 1000000/SampleFreq ; |
mfurukawa | 0:4656a133ed1a | 72 | |
mfurukawa | 0:4656a133ed1a | 73 | int errFlag = 0; |
mfurukawa | 0:4656a133ed1a | 74 | |
mfurukawa | 0:4656a133ed1a | 75 | //define the mpu9250 object |
mfurukawa | 1:3bcd844dd707 | 76 | mpu9250_spi *imu[nCh]; |
mfurukawa | 0:4656a133ed1a | 77 | |
mfurukawa | 0:4656a133ed1a | 78 | // define SPI object for imu objects |
mfurukawa | 1:3bcd844dd707 | 79 | SPI spi1(p5, p6, p7); |
mfurukawa | 1:3bcd844dd707 | 80 | SPI spi2(p11, p12, p13); |
mfurukawa | 1:3bcd844dd707 | 81 | // pins already used for SPI Chip selector pin |
mfurukawa | 1:3bcd844dd707 | 82 | // p21, p23, p29, p30 |
mfurukawa | 1:3bcd844dd707 | 83 | |
mfurukawa | 1:3bcd844dd707 | 84 | // Servo Motor Control |
mfurukawa | 1:3bcd844dd707 | 85 | PwmOut pwm_(p22); |
mfurukawa | 1:3bcd844dd707 | 86 | |
mfurukawa | 1:3bcd844dd707 | 87 | // M-Series Random Sequence |
mfurukawa | 2:3470f2c07582 | 88 | Mseries m; |
mfurukawa | 1:3bcd844dd707 | 89 | |
mfurukawa | 8:07c3cb01a5b6 | 90 | // M-series update flag |
mfurukawa | 11:f23a77c2296d | 91 | #define M_TERM 200; |
mfurukawa | 9:b32312aacbba | 92 | int m_cnt = M_TERM; |
mfurukawa | 8:07c3cb01a5b6 | 93 | |
mfurukawa | 3:70be84fad39e | 94 | #define BINARY_MODE 0 |
mfurukawa | 3:70be84fad39e | 95 | #define ASCII_MODE 1 |
mfurukawa | 3:70be84fad39e | 96 | int send_mode = BINARY_MODE; |
mfurukawa | 0:4656a133ed1a | 97 | |
mfurukawa | 8:07c3cb01a5b6 | 98 | #define CSV_TITLE_COLUMN "smpl_cnt,time[sec],M_stat,accX,accY,accZ,gyroX,gyroY,gyroZ,magX,magY,magZ,accX,accY,accZ,gyroX,gyroY,gyroZ,magX,magY,magZ,\r\n" |
mfurukawa | 6:493df7718ecb | 99 | |
mfurukawa | 11:f23a77c2296d | 100 | void stop_dump(void); |
mfurukawa | 0:4656a133ed1a | 101 | |
mfurukawa | 3:70be84fad39e | 102 | void servo_test(void) |
mfurukawa | 3:70be84fad39e | 103 | { |
mfurukawa | 3:70be84fad39e | 104 | while(1) { |
mfurukawa | 3:70be84fad39e | 105 | if(m.update()) |
mfurukawa | 10:f5a805d998d6 | 106 | pwm_.pulsewidth_us(ONDOT_SERVO_KRS2572HV_USEC_MIN); |
mfurukawa | 3:70be84fad39e | 107 | else |
mfurukawa | 11:f23a77c2296d | 108 | pwm_.pulsewidth_us(KONDO_SERVO_KRS2572HV_USEC_90); |
mfurukawa | 3:70be84fad39e | 109 | wait(.5); |
mfurukawa | 0:4656a133ed1a | 110 | |
mfurukawa | 3:70be84fad39e | 111 | } |
mfurukawa | 3:70be84fad39e | 112 | } |
mfurukawa | 3:70be84fad39e | 113 | void init_sensor(void) |
mfurukawa | 3:70be84fad39e | 114 | { |
mfurukawa | 0:4656a133ed1a | 115 | for(int i=0; i<nCh; i++) { |
mfurukawa | 0:4656a133ed1a | 116 | |
mfurukawa | 0:4656a133ed1a | 117 | imu[0]->deselect(); |
mfurukawa | 0:4656a133ed1a | 118 | imu[1]->deselect(); |
mfurukawa | 0:4656a133ed1a | 119 | imu[2]->deselect(); |
mfurukawa | 0:4656a133ed1a | 120 | imu[3]->deselect(); |
mfurukawa | 0:4656a133ed1a | 121 | |
mfurukawa | 0:4656a133ed1a | 122 | imu[i]->select(); |
mfurukawa | 0:4656a133ed1a | 123 | |
mfurukawa | 0:4656a133ed1a | 124 | //INIT the mpu9250 |
mfurukawa | 0:4656a133ed1a | 125 | //if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) |
mfurukawa | 3:70be84fad39e | 126 | if(imu[i]->init(1,BITS_DLPF_CFG_5HZ)) { |
mfurukawa | 3:70be84fad39e | 127 | //if(imu[i]->init(1,BITS_DLPF_CFG_256HZ_NOLPF2)) { |
mfurukawa | 0:4656a133ed1a | 128 | printf("\nCH %d\n\nCouldn't initialize MPU9250 via SPI!", i+1); |
mfurukawa | 0:4656a133ed1a | 129 | wait(90); |
mfurukawa | 0:4656a133ed1a | 130 | } |
mfurukawa | 0:4656a133ed1a | 131 | |
mfurukawa | 0:4656a133ed1a | 132 | //output the I2C address to know if SPI is working, it should be 104 |
mfurukawa | 0:4656a133ed1a | 133 | printf("\nCH %d\nWHOAMI = 0x%2x\n",i+1, imu[i]->whoami()); |
mfurukawa | 0:4656a133ed1a | 134 | |
mfurukawa | 0:4656a133ed1a | 135 | if(imu[i]->whoami() != 0x71) { |
mfurukawa | 0:4656a133ed1a | 136 | printf(" *** ERROR *** acc and gyro sensor does not respond correctly!\n"); |
mfurukawa | 0:4656a133ed1a | 137 | errFlag |= 0x01<<(i*2); |
mfurukawa | 6:493df7718ecb | 138 | |
mfurukawa | 3:70be84fad39e | 139 | if(i==0||i==2) LED_flash_error_notice(i); |
mfurukawa | 0:4656a133ed1a | 140 | continue; |
mfurukawa | 0:4656a133ed1a | 141 | } |
mfurukawa | 0:4656a133ed1a | 142 | |
mfurukawa | 3:70be84fad39e | 143 | printf("Gyro_scale = %u[DPS]\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS)); //Set 500DPS scale range for gyros //0706 wada 500to2000 |
mfurukawa | 0:4656a133ed1a | 144 | wait_ms(20); |
mfurukawa | 0:4656a133ed1a | 145 | |
mfurukawa | 3:70be84fad39e | 146 | printf("Acc_scale = %u[G]\n",imu[i]->set_acc_scale(BITS_FS_2G)); //Set 4G scale range for accs //0706 wada 4to16 |
mfurukawa | 0:4656a133ed1a | 147 | wait_ms(20); |
mfurukawa | 0:4656a133ed1a | 148 | |
mfurukawa | 0:4656a133ed1a | 149 | printf("AK8963 WHIAM = 0x%2x\n",imu[i]->AK8963_whoami()); |
mfurukawa | 0:4656a133ed1a | 150 | |
mfurukawa | 0:4656a133ed1a | 151 | if(imu[i]->AK8963_whoami() != 0x48) { |
mfurukawa | 0:4656a133ed1a | 152 | printf(" *** ERROR *** magnetrometer does not respond correctly!\n"); |
mfurukawa | 0:4656a133ed1a | 153 | errFlag |= 0x02<<(i*2); |
mfurukawa | 6:493df7718ecb | 154 | |
mfurukawa | 3:70be84fad39e | 155 | if(i==0||i==2) LED_flash_error_notice(i); |
mfurukawa | 0:4656a133ed1a | 156 | continue; |
mfurukawa | 0:4656a133ed1a | 157 | } |
mfurukawa | 6:493df7718ecb | 158 | |
mfurukawa | 5:521f1c79123d | 159 | imu[i]->calib_acc(); |
mfurukawa | 5:521f1c79123d | 160 | wait_ms(100); |
mfurukawa | 5:521f1c79123d | 161 | printf("Calibrated Acc\n"); |
mfurukawa | 6:493df7718ecb | 162 | |
mfurukawa | 0:4656a133ed1a | 163 | imu[i]->AK8963_calib_Magnetometer(); |
mfurukawa | 0:4656a133ed1a | 164 | wait_ms(100); |
mfurukawa | 0:4656a133ed1a | 165 | printf("Calibrated Magnetrometer\n"); |
mfurukawa | 0:4656a133ed1a | 166 | } |
mfurukawa | 1:3bcd844dd707 | 167 | |
mfurukawa | 3:70be84fad39e | 168 | } |
mfurukawa | 1:3bcd844dd707 | 169 | |
mfurukawa | 3:70be84fad39e | 170 | void init(void) |
mfurukawa | 3:70be84fad39e | 171 | { |
mfurukawa | 2:3470f2c07582 | 172 | // servo requires a 20ms period |
mfurukawa | 2:3470f2c07582 | 173 | pwm_.period_ms(20); |
mfurukawa | 1:3bcd844dd707 | 174 | |
mfurukawa | 3:70be84fad39e | 175 | pc.baud(baudRate); |
mfurukawa | 3:70be84fad39e | 176 | |
mfurukawa | 3:70be84fad39e | 177 | printf("\nrev Feb 1, 2021 for Magnetrometer by Masahiro Furukawa\n"); |
mfurukawa | 1:3bcd844dd707 | 178 | |
mfurukawa | 3:70be84fad39e | 179 | imu[3] = new mpu9250_spi(spi2, p21); |
mfurukawa | 3:70be84fad39e | 180 | imu[2] = new mpu9250_spi(spi2, p23); |
mfurukawa | 3:70be84fad39e | 181 | imu[1] = new mpu9250_spi(spi1, p29); |
mfurukawa | 3:70be84fad39e | 182 | imu[0] = new mpu9250_spi(spi1, p30); |
mfurukawa | 3:70be84fad39e | 183 | |
mfurukawa | 3:70be84fad39e | 184 | init_sensor(); |
mfurukawa | 3:70be84fad39e | 185 | |
mfurukawa | 3:70be84fad39e | 186 | // servo_test(); |
mfurukawa | 6:493df7718ecb | 187 | |
mfurukawa | 3:70be84fad39e | 188 | printf("\nHit Key [t] to start. Hit Key [r] to finish. [s] for binary sending.\n"); |
mfurukawa | 3:70be84fad39e | 189 | |
mfurukawa | 6:493df7718ecb | 190 | // fixed channel usage (only Ch1, Ch3 is connected) |
mfurukawa | 6:493df7718ecb | 191 | imu[0]->select(); // Ch1 |
mfurukawa | 6:493df7718ecb | 192 | imu[1]->deselect(); |
mfurukawa | 6:493df7718ecb | 193 | imu[2]->select(); // Ch3 |
mfurukawa | 6:493df7718ecb | 194 | imu[3]->deselect(); |
mfurukawa | 6:493df7718ecb | 195 | |
mfurukawa | 0:4656a133ed1a | 196 | } |
mfurukawa | 0:4656a133ed1a | 197 | |
mfurukawa | 0:4656a133ed1a | 198 | void eventFunc(void) |
mfurukawa | 0:4656a133ed1a | 199 | { |
mfurukawa | 6:493df7718ecb | 200 | int t_ms = timer.read_ms() - 5; // initial call takes 5ms |
mfurukawa | 9:b32312aacbba | 201 | |
mfurukawa | 9:b32312aacbba | 202 | // when M-series counter overflow occurs |
mfurukawa | 9:b32312aacbba | 203 | if(--m_cnt < 0) { |
mfurukawa | 9:b32312aacbba | 204 | m.update(); |
mfurukawa | 9:b32312aacbba | 205 | if(m.get()) |
mfurukawa | 10:f5a805d998d6 | 206 | pwm_.pulsewidth_us(ONDOT_SERVO_KRS2572HV_USEC_MIN); |
mfurukawa | 9:b32312aacbba | 207 | else |
mfurukawa | 11:f23a77c2296d | 208 | pwm_.pulsewidth_us(KONDO_SERVO_KRS2572HV_USEC_90); |
mfurukawa | 9:b32312aacbba | 209 | m_cnt = M_TERM; |
mfurukawa | 9:b32312aacbba | 210 | } |
mfurukawa | 9:b32312aacbba | 211 | |
mfurukawa | 0:4656a133ed1a | 212 | // limitation on sending bytes at 921600bps - 92bits(under 100us/sample) |
mfurukawa | 0:4656a133ed1a | 213 | // requirement : 1kHz sampling |
mfurukawa | 0:4656a133ed1a | 214 | // = 921.5 bits/sample |
mfurukawa | 0:4656a133ed1a | 215 | // = 115.1 bytes/sample |
mfurukawa | 0:4656a133ed1a | 216 | // = 50 bytes/axis (2byte/axis) |
mfurukawa | 0:4656a133ed1a | 217 | |
mfurukawa | 0:4656a133ed1a | 218 | // 2 byte * 6 axes * 4 ch = 48 bytes/sample |
mfurukawa | 0:4656a133ed1a | 219 | |
mfurukawa | 5:521f1c79123d | 220 | // imu[0]->read_acc(); |
mfurukawa | 5:521f1c79123d | 221 | // imu[0]->read_rot(); |
mfurukawa | 5:521f1c79123d | 222 | // imu[0]->AK8963_read_Magnetometer(); |
mfurukawa | 5:521f1c79123d | 223 | // |
mfurukawa | 5:521f1c79123d | 224 | // imu[2]->read_acc(); |
mfurukawa | 5:521f1c79123d | 225 | // imu[2]->read_rot(); |
mfurukawa | 5:521f1c79123d | 226 | // imu[2]->AK8963_read_Magnetometer(); |
mfurukawa | 6:493df7718ecb | 227 | |
mfurukawa | 6:493df7718ecb | 228 | // measurement step takes 3ms |
mfurukawa | 5:521f1c79123d | 229 | imu[0]->read_all(); |
mfurukawa | 5:521f1c79123d | 230 | imu[2]->read_all(); |
mfurukawa | 6:493df7718ecb | 231 | |
mfurukawa | 3:70be84fad39e | 232 | // imu[0]->deselect(); |
mfurukawa | 3:70be84fad39e | 233 | // imu[1]->select(); |
mfurukawa | 3:70be84fad39e | 234 | // imu[2]->deselect(); |
mfurukawa | 3:70be84fad39e | 235 | // imu[3]->select(); |
mfurukawa | 3:70be84fad39e | 236 | |
mfurukawa | 3:70be84fad39e | 237 | // imu[1]->read_acc(); |
mfurukawa | 3:70be84fad39e | 238 | // imu[1]->read_rot(); |
mfurukawa | 3:70be84fad39e | 239 | // imu[1]->AK8963_read_Magnetometer(); |
mfurukawa | 3:70be84fad39e | 240 | |
mfurukawa | 3:70be84fad39e | 241 | // imu[3]->read_acc(); |
mfurukawa | 3:70be84fad39e | 242 | // imu[3]->read_rot(); |
mfurukawa | 3:70be84fad39e | 243 | // imu[3]->AK8963_read_Magnetometer(); |
mfurukawa | 3:70be84fad39e | 244 | |
mfurukawa | 3:70be84fad39e | 245 | switch(send_mode) { |
mfurukawa | 0:4656a133ed1a | 246 | |
mfurukawa | 3:70be84fad39e | 247 | case ASCII_MODE: |
mfurukawa | 6:493df7718ecb | 248 | printf("%ld,",smpl_cnt++); |
mfurukawa | 6:493df7718ecb | 249 | printf("%1.3f,",t_ms/1000.0f); |
mfurukawa | 9:b32312aacbba | 250 | printf("%d,",m.get()); |
mfurukawa | 3:70be84fad39e | 251 | for(int i=0; i<3; i+=2) { |
mfurukawa | 6:493df7718ecb | 252 | for(int j=0; j<3; j++) printf("%1.3f,",imu[i]->accelerometer_data[j]); |
mfurukawa | 6:493df7718ecb | 253 | for(int j=0; j<3; j++) printf("%1.3f,",imu[i]->gyroscope_data[j]); |
mfurukawa | 6:493df7718ecb | 254 | for(int j=0; j<3; j++) printf("%1.3f,",imu[i]->Magnetometer[j] / 1000.0f); |
mfurukawa | 3:70be84fad39e | 255 | } |
mfurukawa | 5:521f1c79123d | 256 | // printf("[mT]"); |
mfurukawa | 3:70be84fad39e | 257 | break; |
mfurukawa | 0:4656a133ed1a | 258 | |
mfurukawa | 3:70be84fad39e | 259 | case BINARY_MODE: |
mfurukawa | 3:70be84fad39e | 260 | |
mfurukawa | 6:493df7718ecb | 261 | // TO BE IMPLEMENTED ... |
mfurukawa | 6:493df7718ecb | 262 | // for(int i=0; i<3; i+=2) { |
mfurukawa | 6:493df7718ecb | 263 | // for(int j=0; j<6; j++) putc(imu[i]->accelerometer_response[j], stdout); |
mfurukawa | 6:493df7718ecb | 264 | // for(int j=0; j<6; j++) putc(imu[i]->gyroscope_response[j], stdout); |
mfurukawa | 6:493df7718ecb | 265 | // } |
mfurukawa | 3:70be84fad39e | 266 | break; |
mfurukawa | 0:4656a133ed1a | 267 | } |
mfurukawa | 0:4656a133ed1a | 268 | |
mfurukawa | 0:4656a133ed1a | 269 | putc(13, stdout); //0x0d CR(復帰) |
mfurukawa | 0:4656a133ed1a | 270 | putc(10, stdout); //0x0a LF(改行) |
mfurukawa | 11:f23a77c2296d | 271 | |
mfurukawa | 11:f23a77c2296d | 272 | // auto-stop when 15sec after |
mfurukawa | 11:f23a77c2296d | 273 | if(smpl_cnt>3000){stop_dump();} |
mfurukawa | 11:f23a77c2296d | 274 | } |
mfurukawa | 11:f23a77c2296d | 275 | |
mfurukawa | 11:f23a77c2296d | 276 | void stop_dump(void) |
mfurukawa | 11:f23a77c2296d | 277 | { |
mfurukawa | 11:f23a77c2296d | 278 | ticker.detach(); |
mfurukawa | 11:f23a77c2296d | 279 | timer.stop(); |
mfurukawa | 11:f23a77c2296d | 280 | smpl_cnt = 0; |
mfurukawa | 0:4656a133ed1a | 281 | } |
mfurukawa | 0:4656a133ed1a | 282 | |
mfurukawa | 0:4656a133ed1a | 283 | int main() |
mfurukawa | 0:4656a133ed1a | 284 | { |
mfurukawa | 0:4656a133ed1a | 285 | // make instances and check sensors |
mfurukawa | 0:4656a133ed1a | 286 | init(); |
mfurukawa | 0:4656a133ed1a | 287 | |
mfurukawa | 0:4656a133ed1a | 288 | char c; |
mfurukawa | 0:4656a133ed1a | 289 | |
mfurukawa | 0:4656a133ed1a | 290 | while(1) { |
mfurukawa | 0:4656a133ed1a | 291 | if(pc.readable()) { |
mfurukawa | 0:4656a133ed1a | 292 | c = pc.getc(); |
mfurukawa | 0:4656a133ed1a | 293 | |
mfurukawa | 3:70be84fad39e | 294 | switch(c) { |
mfurukawa | 3:70be84fad39e | 295 | case 'r': |
mfurukawa | 11:f23a77c2296d | 296 | stop_dump(); |
mfurukawa | 3:70be84fad39e | 297 | break; |
mfurukawa | 3:70be84fad39e | 298 | |
mfurukawa | 3:70be84fad39e | 299 | case 'R': |
mfurukawa | 6:493df7718ecb | 300 | smpl_cnt = 0; |
mfurukawa | 3:70be84fad39e | 301 | init_sensor(); |
mfurukawa | 6:493df7718ecb | 302 | timer.stop(); |
mfurukawa | 6:493df7718ecb | 303 | timer.reset(); |
mfurukawa | 3:70be84fad39e | 304 | break; |
mfurukawa | 6:493df7718ecb | 305 | |
mfurukawa | 3:70be84fad39e | 306 | case 't': |
mfurukawa | 8:07c3cb01a5b6 | 307 | printf(CSV_TITLE_COLUMN); |
mfurukawa | 6:493df7718ecb | 308 | smpl_cnt = 0; |
mfurukawa | 8:07c3cb01a5b6 | 309 | m.init(); |
mfurukawa | 6:493df7718ecb | 310 | timer.reset(); |
mfurukawa | 6:493df7718ecb | 311 | timer.start(); |
mfurukawa | 3:70be84fad39e | 312 | send_mode = ASCII_MODE; |
mfurukawa | 6:493df7718ecb | 313 | ticker.attach_us(eventFunc, 1000000/SampleFreq); |
mfurukawa | 6:493df7718ecb | 314 | break; |
mfurukawa | 6:493df7718ecb | 315 | |
mfurukawa | 6:493df7718ecb | 316 | case 'c': |
mfurukawa | 8:07c3cb01a5b6 | 317 | printf(CSV_TITLE_COLUMN); |
mfurukawa | 3:70be84fad39e | 318 | break; |
mfurukawa | 3:70be84fad39e | 319 | |
mfurukawa | 3:70be84fad39e | 320 | case 's': |
mfurukawa | 6:493df7718ecb | 321 | smpl_cnt = 0; |
mfurukawa | 6:493df7718ecb | 322 | timer.reset(); |
mfurukawa | 6:493df7718ecb | 323 | timer.start(); |
mfurukawa | 3:70be84fad39e | 324 | send_mode = BINARY_MODE; |
mfurukawa | 6:493df7718ecb | 325 | ticker.attach_us(eventFunc, 1000000/SampleFreq); |
mfurukawa | 3:70be84fad39e | 326 | break; |
mfurukawa | 0:4656a133ed1a | 327 | } |
mfurukawa | 0:4656a133ed1a | 328 | } |
mfurukawa | 0:4656a133ed1a | 329 | } |
mfurukawa | 0:4656a133ed1a | 330 | } |