2018年度計器mbed用プログラム
Dependencies: BufferedSoftSerial2 INA226_ver1 mbed-rtos mbed SDFileSystem-RTOS
Fork of keiki2017 by
main.cpp@30:66fa18093418, 2017-02-17 (annotated)
- Committer:
- tsumagari
- Date:
- Fri Feb 17 09:04:48 2017 +0000
- Revision:
- 30:66fa18093418
- Parent:
- 29:2da9b8d03c0b
- Child:
- 32:fe006d07c1ea
mpu6050_DMP6.h????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
taurin | 1:5ec2409854da | 1 | //計器プログラム |
YusukeWakuta | 18:0a0c4277d960 | 2 | |
taurin | 0:085b2c5e3254 | 3 | #include "mbed.h" |
tsumagari | 22:5cbebf097600 | 4 | #include "rtos.h" |
taurin | 0:085b2c5e3254 | 5 | #include "Fusokukei.h" |
tsumagari | 29:2da9b8d03c0b | 6 | //#include "xMPU6050.h" |
tsumagari | 29:2da9b8d03c0b | 7 | #include "MPU6050_DMP6.h" |
taurin | 0:085b2c5e3254 | 8 | #include "SDFileSystem.h" |
tsumagari | 15:6966299bea4c | 9 | #include "BufferedSoftSerial.h" |
tsumagari | 15:6966299bea4c | 10 | #include "Cadence.h" |
taurin | 0:085b2c5e3254 | 11 | |
taurin | 0:085b2c5e3254 | 12 | #define KX_VALUE_MIN 0.4 |
taurin | 0:085b2c5e3254 | 13 | #define KX_VALUE_MAX 0.8 |
tsumagari | 26:50272431cd1e | 14 | #define SOUDA_DATAS_NUM 18 |
tsumagari | 26:50272431cd1e | 15 | #define YOKUTAN_DATAS_NUM 14 |
tsumagari | 26:50272431cd1e | 16 | #define WRITE_DATAS_NUM 18 |
taurin | 0:085b2c5e3254 | 17 | #define AIR_LOOP_TIME 0.01 |
taurin | 3:8dc516be2e7e | 18 | #define WRITE_DATAS_LOOP_TIME 1 |
taurin | 3:8dc516be2e7e | 19 | #define ROLL_R_MAX_DEG 2 |
taurin | 3:8dc516be2e7e | 20 | #define ROLL_L_MAX_DEG 2 |
tsumagari | 26:50272431cd1e | 21 | #define SD_WRITE_NUM 20 |
taurin | 4:a863a092141c | 22 | #define INIT_SERVO_PERIOD_MS 20 |
taurin | 0:085b2c5e3254 | 23 | |
tsumagari | 26:50272431cd1e | 24 | #define MPU_LOOP_TIME 0.01 |
tsumagari | 26:50272431cd1e | 25 | #define MPU_DELT_MIN 250 |
tsumagari | 26:50272431cd1e | 26 | |
tsumagari | 26:50272431cd1e | 27 | Mutex ssMutex; |
tsumagari | 26:50272431cd1e | 28 | |
tsumagari | 26:50272431cd1e | 29 | //-----------------------------------(resetInterrupt def) |
tsumagari | 26:50272431cd1e | 30 | extern "C" void mbed_reset(); |
tsumagari | 26:50272431cd1e | 31 | InterruptIn resetPin(p26); |
tsumagari | 26:50272431cd1e | 32 | Timer resetTimeCount; |
tsumagari | 26:50272431cd1e | 33 | void resetInterrupt(){ |
tsumagari | 26:50272431cd1e | 34 | while(resetPin){ |
tsumagari | 26:50272431cd1e | 35 | resetTimeCount.start(); |
tsumagari | 26:50272431cd1e | 36 | if(resetTimeCount.read()>3) mbed_reset(); |
tsumagari | 26:50272431cd1e | 37 | } |
tsumagari | 26:50272431cd1e | 38 | resetTimeCount.reset(); |
tsumagari | 26:50272431cd1e | 39 | } |
tsumagari | 26:50272431cd1e | 40 | //------------------------------------------------------- |
tsumagari | 26:50272431cd1e | 41 | |
tsumagari | 26:50272431cd1e | 42 | |
tsumagari | 21:8802034b7810 | 43 | Cadence cadence(p13,p14); |
tsumagari | 15:6966299bea4c | 44 | |
tsumagari | 21:8802034b7810 | 45 | RawSerial pc(USBTX,USBRX); |
tsumagari | 26:50272431cd1e | 46 | BufferedSoftSerial twe(p11,p12); |
tsumagari | 26:50272431cd1e | 47 | RawSerial Android(p9,p10); |
tsumagari | 15:6966299bea4c | 48 | BufferedSoftSerial soudaSerial(p17,p18); |
taurin | 3:8dc516be2e7e | 49 | Timer writeTimer; |
taurin | 0:085b2c5e3254 | 50 | |
tsumagari | 30:66fa18093418 | 51 | InterruptIn FusokukeiPin(p23); |
taurin | 0:085b2c5e3254 | 52 | Ticker FusokukeiTicker; |
taurin | 0:085b2c5e3254 | 53 | Fusokukei air; |
taurin | 0:085b2c5e3254 | 54 | volatile int air_kaitensu= 0; |
taurin | 0:085b2c5e3254 | 55 | |
tsumagari | 26:50272431cd1e | 56 | float sum = 0; |
tsumagari | 26:50272431cd1e | 57 | uint32_t sumCount = 0; |
tsumagari | 29:2da9b8d03c0b | 58 | //MPU6050 mpu6050; |
tsumagari | 29:2da9b8d03c0b | 59 | float yaw,pitch,roll; |
tsumagari | 29:2da9b8d03c0b | 60 | float pre_ypr[3]; |
tsumagari | 29:2da9b8d03c0b | 61 | float offset_ypr[3]; |
taurin | 0:085b2c5e3254 | 62 | Timer t; |
tsumagari | 26:50272431cd1e | 63 | // |
tsumagari | 26:50272431cd1e | 64 | //AnalogIn kx_X(p17); |
tsumagari | 26:50272431cd1e | 65 | //AnalogIn kx_Y(p16); |
tsumagari | 26:50272431cd1e | 66 | //AnalogIn kx_Z(p15); |
tsumagari | 26:50272431cd1e | 67 | //float KX_X,KX_Y,KX_Z; |
taurin | 0:085b2c5e3254 | 68 | |
tsumagari | 26:50272431cd1e | 69 | |
tsumagari | 26:50272431cd1e | 70 | //Timer sonarTimer; |
tsumagari | 26:50272431cd1e | 71 | //InterruptIn sonarPin(p22); |
tsumagari | 26:50272431cd1e | 72 | AnalogIn sonarPin(p15); |
tsumagari | 26:50272431cd1e | 73 | //double sonarDistTime=0; |
tsumagari | 26:50272431cd1e | 74 | double sonarV = 0.0, sonarDist = 0.0; |
taurin | 0:085b2c5e3254 | 75 | |
taurin | 0:085b2c5e3254 | 76 | DigitalOut RollAlarmR(p20); |
taurin | 0:085b2c5e3254 | 77 | DigitalOut RollAlarmL(p19); |
tsumagari | 29:2da9b8d03c0b | 78 | //DigitalOut led(LED1);//for mpu |
taurin | 1:5ec2409854da | 79 | DigitalOut led2(LED2); |
tsumagari | 26:50272431cd1e | 80 | DigitalOut led3(LED3); |
tsumagari | 23:e59afb2044df | 81 | DigitalOut led4(LED4); |
taurin | 0:085b2c5e3254 | 82 | |
taurin | 0:085b2c5e3254 | 83 | SDFileSystem sd(p5, p6, p7, p8, "sd"); |
taurin | 0:085b2c5e3254 | 84 | FILE* fp; |
taurin | 0:085b2c5e3254 | 85 | |
YusukeWakuta | 11:90e2b070f337 | 86 | PwmOut kisokuServo(p26); |
YusukeWakuta | 11:90e2b070f337 | 87 | PwmOut geikakuServo(p22); |
taurin | 4:a863a092141c | 88 | |
taurin | 0:085b2c5e3254 | 89 | char soudaDatas[SOUDA_DATAS_NUM]; |
taurin | 3:8dc516be2e7e | 90 | float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM]; |
taurin | 3:8dc516be2e7e | 91 | volatile int write_datas_index = 0; |
taurin | 0:085b2c5e3254 | 92 | |
tsumagari | 30:66fa18093418 | 93 | void cadenceDataReceive(void const *argument); |
taurin | 0:085b2c5e3254 | 94 | void air_countUp(); |
taurin | 0:085b2c5e3254 | 95 | void call_calcAirSpeed(); |
taurin | 0:085b2c5e3254 | 96 | void init(); |
taurin | 0:085b2c5e3254 | 97 | void FusokukeiInit(); |
taurin | 0:085b2c5e3254 | 98 | void SdInit(); |
tsumagari | 30:66fa18093418 | 99 | //void MpuInit(); |
tsumagari | 26:50272431cd1e | 100 | //void SonarInit(); |
tsumagari | 30:66fa18093418 | 101 | void mpuProcessing(void const *argument); |
tsumagari | 30:66fa18093418 | 102 | void sonarCalc(void const *argument); |
tsumagari | 30:66fa18093418 | 103 | void DataReceiveFromSouda(void const *argument); |
tsumagari | 21:8802034b7810 | 104 | void SDprintf(void const *argument); |
taurin | 0:085b2c5e3254 | 105 | void WriteDatas(); |
tsumagari | 26:50272431cd1e | 106 | //float calcAttackAngle(); |
tsumagari | 26:50272431cd1e | 107 | //float calcKXdeg(float x); |
taurin | 0:085b2c5e3254 | 108 | |
tsumagari | 30:66fa18093418 | 109 | void cadenceDataReceive(void const *argument){ |
tsumagari | 26:50272431cd1e | 110 | while(1){ |
tsumagari | 26:50272431cd1e | 111 | cadence.readData(); |
tsumagari | 26:50272431cd1e | 112 | Thread::wait(0.01); |
tsumagari | 26:50272431cd1e | 113 | } |
tsumagari | 15:6966299bea4c | 114 | } |
tsumagari | 15:6966299bea4c | 115 | |
taurin | 0:085b2c5e3254 | 116 | void air_countUp(){ |
taurin | 0:085b2c5e3254 | 117 | air_kaitensu++; |
taurin | 0:085b2c5e3254 | 118 | } |
taurin | 0:085b2c5e3254 | 119 | |
taurin | 0:085b2c5e3254 | 120 | void call_calcAirSpeed(){ |
taurin | 0:085b2c5e3254 | 121 | air.calcAirSpeed(air_kaitensu); |
taurin | 0:085b2c5e3254 | 122 | air_kaitensu = 0; |
tsumagari | 29:2da9b8d03c0b | 123 | led3 = !led3; |
taurin | 0:085b2c5e3254 | 124 | } |
taurin | 0:085b2c5e3254 | 125 | |
tsumagari | 26:50272431cd1e | 126 | //void sonarInterruptStart(){ |
tsumagari | 26:50272431cd1e | 127 | // sonarTimer.start(); |
tsumagari | 26:50272431cd1e | 128 | // led3 = 1; |
tsumagari | 26:50272431cd1e | 129 | //} |
tsumagari | 26:50272431cd1e | 130 | //void sonarInterruptStop(){ |
tsumagari | 26:50272431cd1e | 131 | // sonarTimer.stop(); |
tsumagari | 26:50272431cd1e | 132 | // led3 = 0; |
tsumagari | 26:50272431cd1e | 133 | // sonarDistTime = sonarTimer.read_us(); |
tsumagari | 26:50272431cd1e | 134 | // sonarTimer.reset(); |
tsumagari | 26:50272431cd1e | 135 | //} |
tsumagari | 26:50272431cd1e | 136 | |
taurin | 0:085b2c5e3254 | 137 | void init(){ |
tsumagari | 26:50272431cd1e | 138 | //--------------------------------------(resetInterrupt init) |
tsumagari | 26:50272431cd1e | 139 | resetPin.rise(resetInterrupt); |
tsumagari | 26:50272431cd1e | 140 | resetPin.mode(PullDown); |
tsumagari | 26:50272431cd1e | 141 | //----------------------------------------------------------- |
tsumagari | 26:50272431cd1e | 142 | twe.baud(115200); |
YusukeWakuta | 19:fa3f9ba17af8 | 143 | Android.baud(9600); |
tsumagari | 15:6966299bea4c | 144 | soudaSerial.baud(9600); |
taurin | 5:1b3c8e5ee99e | 145 | kisokuServo.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 5:1b3c8e5ee99e | 146 | geikakuServo.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 0:085b2c5e3254 | 147 | FusokukeiInit(); |
tsumagari | 26:50272431cd1e | 148 | // MpuInit(); //mpuProcessing内で |
tsumagari | 26:50272431cd1e | 149 | // SonarInit(); |
tsumagari | 26:50272431cd1e | 150 | // SdInit();//SDprintf 内で |
taurin | 0:085b2c5e3254 | 151 | } |
taurin | 0:085b2c5e3254 | 152 | |
taurin | 0:085b2c5e3254 | 153 | void FusokukeiInit(){ |
tsumagari | 29:2da9b8d03c0b | 154 | FusokukeiPin.rise(&air_countUp); |
taurin | 0:085b2c5e3254 | 155 | FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME); |
taurin | 0:085b2c5e3254 | 156 | } |
taurin | 0:085b2c5e3254 | 157 | |
taurin | 4:a863a092141c | 158 | double calcPulse(int deg){ |
taurin | 4:a863a092141c | 159 | return (0.0006+(deg/180.0)*(0.00235-0.00045)); |
taurin | 4:a863a092141c | 160 | } |
taurin | 4:a863a092141c | 161 | |
taurin | 0:085b2c5e3254 | 162 | void SdInit(){ |
taurin | 0:085b2c5e3254 | 163 | mkdir("/sd/mydir", 0777); |
taurin | 0:085b2c5e3254 | 164 | fp = fopen("/sd/mydir/sdtest2.csv", "w"); |
taurin | 0:085b2c5e3254 | 165 | if(fp == NULL) { |
taurin | 0:085b2c5e3254 | 166 | error("Could not open file for write\n"); |
taurin | 0:085b2c5e3254 | 167 | } |
taurin | 0:085b2c5e3254 | 168 | fprintf(fp, "Hello fun SD Card World!\n\r"); |
taurin | 0:085b2c5e3254 | 169 | fclose(fp); |
taurin | 0:085b2c5e3254 | 170 | } |
tsumagari | 29:2da9b8d03c0b | 171 | |
tsumagari | 30:66fa18093418 | 172 | //void MpuInit(){ |
tsumagari | 29:2da9b8d03c0b | 173 | // i2c.frequency(400000); // use fast (400 kHz) I2C |
tsumagari | 29:2da9b8d03c0b | 174 | // t.start(); |
tsumagari | 29:2da9b8d03c0b | 175 | // uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 |
tsumagari | 29:2da9b8d03c0b | 176 | // if (whoami == 0x68) { // WHO_AM_I should always be 0x68 |
tsumagari | 29:2da9b8d03c0b | 177 | // Thread::wait(1); |
tsumagari | 29:2da9b8d03c0b | 178 | // mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values |
tsumagari | 29:2da9b8d03c0b | 179 | // Thread::wait(1); |
tsumagari | 29:2da9b8d03c0b | 180 | // if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { |
tsumagari | 29:2da9b8d03c0b | 181 | // mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration |
tsumagari | 29:2da9b8d03c0b | 182 | // mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
tsumagari | 29:2da9b8d03c0b | 183 | // mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature |
tsumagari | 29:2da9b8d03c0b | 184 | // Thread::wait(2); |
tsumagari | 29:2da9b8d03c0b | 185 | // } else { |
tsumagari | 29:2da9b8d03c0b | 186 | // } |
tsumagari | 29:2da9b8d03c0b | 187 | // } else { |
tsumagari | 29:2da9b8d03c0b | 188 | // pc.printf("MPU6050 not ready...\n\r"); |
tsumagari | 29:2da9b8d03c0b | 189 | // //pc.printf("out\n\r"); // Loop forever if communication doesn't happen |
tsumagari | 29:2da9b8d03c0b | 190 | // } |
tsumagari | 30:66fa18093418 | 191 | // using namespace MPU6050DMP6; |
tsumagari | 30:66fa18093418 | 192 | // setup(); |
tsumagari | 30:66fa18093418 | 193 | // do{ |
tsumagari | 30:66fa18093418 | 194 | // for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i]; |
tsumagari | 30:66fa18093418 | 195 | // getYPR(); |
tsumagari | 30:66fa18093418 | 196 | // Thread::wait(0.01); |
tsumagari | 30:66fa18093418 | 197 | // }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) ); |
tsumagari | 30:66fa18093418 | 198 | // for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i]; |
tsumagari | 30:66fa18093418 | 199 | //} |
taurin | 0:085b2c5e3254 | 200 | |
tsumagari | 26:50272431cd1e | 201 | //void SonarInit(){ |
tsumagari | 26:50272431cd1e | 202 | // sonarPin.rise(&sonarInterruptStart); |
tsumagari | 26:50272431cd1e | 203 | // sonarPin.fall(&sonarInterruptStop); |
tsumagari | 26:50272431cd1e | 204 | //} |
tsumagari | 26:50272431cd1e | 205 | |
tsumagari | 29:2da9b8d03c0b | 206 | //void mpuProcessing(void const *argument){ |
tsumagari | 29:2da9b8d03c0b | 207 | // MpuInit(); |
tsumagari | 29:2da9b8d03c0b | 208 | // while(1){ |
tsumagari | 29:2da9b8d03c0b | 209 | // Thread::wait(0.1); |
tsumagari | 29:2da9b8d03c0b | 210 | // if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt |
tsumagari | 29:2da9b8d03c0b | 211 | // mpu6050.readAccelData(accelCount); // Read the x/y/z adc values |
tsumagari | 29:2da9b8d03c0b | 212 | // mpu6050.getAres(); |
tsumagari | 29:2da9b8d03c0b | 213 | // ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
tsumagari | 29:2da9b8d03c0b | 214 | // ay = (float)accelCount[1]*aRes - accelBias[1]; |
tsumagari | 29:2da9b8d03c0b | 215 | // az = (float)accelCount[2]*aRes - accelBias[2]; |
tsumagari | 29:2da9b8d03c0b | 216 | // mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values |
tsumagari | 29:2da9b8d03c0b | 217 | // mpu6050.getGres(); |
tsumagari | 29:2da9b8d03c0b | 218 | // gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set |
tsumagari | 29:2da9b8d03c0b | 219 | // gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; |
tsumagari | 29:2da9b8d03c0b | 220 | // gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; |
tsumagari | 29:2da9b8d03c0b | 221 | // tempCount = mpu6050.readTempData(); // Read the x/y/z adc values |
tsumagari | 29:2da9b8d03c0b | 222 | // temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade |
tsumagari | 29:2da9b8d03c0b | 223 | // } |
tsumagari | 29:2da9b8d03c0b | 224 | // Now = t.read_us(); |
tsumagari | 29:2da9b8d03c0b | 225 | // deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
tsumagari | 29:2da9b8d03c0b | 226 | // lastUpdate = Now; |
tsumagari | 29:2da9b8d03c0b | 227 | // sum += deltat; |
tsumagari | 29:2da9b8d03c0b | 228 | // sumCount++; |
tsumagari | 29:2da9b8d03c0b | 229 | // if(lastUpdate - firstUpdate > 10000000.0f) { |
tsumagari | 29:2da9b8d03c0b | 230 | // beta = 0.04; // decrease filter gain after stabilized |
tsumagari | 29:2da9b8d03c0b | 231 | // zeta = 0.015; // increasey bias drift gain after stabilized |
tsumagari | 29:2da9b8d03c0b | 232 | // } |
tsumagari | 29:2da9b8d03c0b | 233 | // mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); |
tsumagari | 29:2da9b8d03c0b | 234 | // delt_t = t.read_ms() - count; |
tsumagari | 29:2da9b8d03c0b | 235 | // if (delt_t > MPU_DELT_MIN) { |
tsumagari | 29:2da9b8d03c0b | 236 | // yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); |
tsumagari | 29:2da9b8d03c0b | 237 | // pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
tsumagari | 29:2da9b8d03c0b | 238 | // roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); |
tsumagari | 29:2da9b8d03c0b | 239 | // pitch *= 180.0f / PI; |
tsumagari | 29:2da9b8d03c0b | 240 | // yaw *= 180.0f / PI; |
tsumagari | 29:2da9b8d03c0b | 241 | // roll *= 180.0f / PI; |
tsumagari | 29:2da9b8d03c0b | 242 | // myled= !myled; |
tsumagari | 29:2da9b8d03c0b | 243 | // count = t.read_ms(); |
tsumagari | 29:2da9b8d03c0b | 244 | // sum = 0; |
tsumagari | 29:2da9b8d03c0b | 245 | // sumCount = 0; |
tsumagari | 29:2da9b8d03c0b | 246 | // } |
tsumagari | 29:2da9b8d03c0b | 247 | // } |
tsumagari | 29:2da9b8d03c0b | 248 | //} |
tsumagari | 26:50272431cd1e | 249 | void mpuProcessing(void const *argument){ |
tsumagari | 29:2da9b8d03c0b | 250 | using namespace MPU6050DMP6; |
tsumagari | 29:2da9b8d03c0b | 251 | setup(); |
tsumagari | 30:66fa18093418 | 252 | // do{ |
tsumagari | 30:66fa18093418 | 253 | // for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i]; |
tsumagari | 29:2da9b8d03c0b | 254 | getYPR(); |
tsumagari | 30:66fa18093418 | 255 | // Thread::wait(0.05); |
tsumagari | 30:66fa18093418 | 256 | // }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) ); |
tsumagari | 29:2da9b8d03c0b | 257 | for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i]; |
tsumagari | 29:2da9b8d03c0b | 258 | // MpuInit(); |
tsumagari | 26:50272431cd1e | 259 | while(1){ |
tsumagari | 29:2da9b8d03c0b | 260 | getYPR(); |
tsumagari | 29:2da9b8d03c0b | 261 | yaw = (ypr[0] - offset_ypr[0]) *180/M_PI; |
tsumagari | 29:2da9b8d03c0b | 262 | pitch = (ypr[1] - offset_ypr[1]) *180/M_PI; |
tsumagari | 29:2da9b8d03c0b | 263 | roll = (ypr[2] - offset_ypr[2]) *180/M_PI; |
tsumagari | 30:66fa18093418 | 264 | Thread::wait(0.1); |
tsumagari | 30:66fa18093418 | 265 | } |
tsumagari | 30:66fa18093418 | 266 | } |
tsumagari | 30:66fa18093418 | 267 | |
tsumagari | 30:66fa18093418 | 268 | void sonarCalc(void const *argument){ |
tsumagari | 30:66fa18093418 | 269 | // return sonarDistTime*0.018624 - 13.511; |
tsumagari | 30:66fa18093418 | 270 | while(1){ |
tsumagari | 30:66fa18093418 | 271 | sonarV = 0; |
tsumagari | 30:66fa18093418 | 272 | for(int i =0; i<20; i++){ |
tsumagari | 30:66fa18093418 | 273 | sonarV += sonarPin.read(); |
tsumagari | 30:66fa18093418 | 274 | Thread::wait(0.01); |
tsumagari | 30:66fa18093418 | 275 | // wait(0.01); |
tsumagari | 30:66fa18093418 | 276 | } |
tsumagari | 30:66fa18093418 | 277 | sonarDist = (sonarV/20)*2062.5; |
tsumagari | 29:2da9b8d03c0b | 278 | Thread::wait(0.01); |
tsumagari | 30:66fa18093418 | 279 | // wait(0.01); |
tsumagari | 26:50272431cd1e | 280 | } |
tsumagari | 26:50272431cd1e | 281 | } |
tsumagari | 26:50272431cd1e | 282 | |
tsumagari | 26:50272431cd1e | 283 | void DataReceiveFromSouda(void const *argument){ |
tsumagari | 26:50272431cd1e | 284 | while(1){ |
tsumagari | 26:50272431cd1e | 285 | led2 = !led2; |
tsumagari | 26:50272431cd1e | 286 | for(int i = 0; i < SOUDA_DATAS_NUM; i++){ |
tsumagari | 26:50272431cd1e | 287 | if(soudaSerial.readable()) { |
tsumagari | 26:50272431cd1e | 288 | // ssMutex.lock(); |
tsumagari | 26:50272431cd1e | 289 | soudaDatas[i] = (char)soudaSerial.getc(); |
tsumagari | 26:50272431cd1e | 290 | // ssMutex.unlock(); |
tsumagari | 26:50272431cd1e | 291 | if(soudaDatas[i]==';') i=-1; |
tsumagari | 26:50272431cd1e | 292 | }else i--; |
tsumagari | 26:50272431cd1e | 293 | } |
taurin | 0:085b2c5e3254 | 294 | } |
taurin | 0:085b2c5e3254 | 295 | } |
taurin | 0:085b2c5e3254 | 296 | |
tsumagari | 21:8802034b7810 | 297 | void SDprintf(void const *argument){ |
tsumagari | 23:e59afb2044df | 298 | SdInit(); |
tsumagari | 22:5cbebf097600 | 299 | while(1){ |
tsumagari | 23:e59afb2044df | 300 | if(write_datas_index == SD_WRITE_NUM-1){ |
tsumagari | 26:50272431cd1e | 301 | // led4 = !led4; |
tsumagari | 23:e59afb2044df | 302 | fp = fopen("/sd/mydir/sdtest.csv", "a"); |
tsumagari | 23:e59afb2044df | 303 | if(fp == NULL) { |
tsumagari | 23:e59afb2044df | 304 | error("Could not open file for write\n"); |
tsumagari | 23:e59afb2044df | 305 | } |
tsumagari | 23:e59afb2044df | 306 | for(int i = 0; i < SD_WRITE_NUM; i++){ |
tsumagari | 23:e59afb2044df | 307 | for(int j = 0; j < WRITE_DATAS_NUM; j++){ |
tsumagari | 23:e59afb2044df | 308 | fprintf(fp,"%f,", writeDatas[i][j]); |
tsumagari | 23:e59afb2044df | 309 | } |
tsumagari | 23:e59afb2044df | 310 | } |
tsumagari | 23:e59afb2044df | 311 | fprintf(fp,"\n\r"); |
tsumagari | 23:e59afb2044df | 312 | fclose(fp); |
tsumagari | 23:e59afb2044df | 313 | write_datas_index=0; |
tsumagari | 21:8802034b7810 | 314 | } |
tsumagari | 26:50272431cd1e | 315 | Thread::wait(0.01); |
tsumagari | 22:5cbebf097600 | 316 | } |
taurin | 3:8dc516be2e7e | 317 | } |
taurin | 3:8dc516be2e7e | 318 | |
taurin | 3:8dc516be2e7e | 319 | void WriteDatas(){ |
taurin | 8:31e07f6ed0f7 | 320 | int i; |
tsumagari | 26:50272431cd1e | 321 | for(i = 0; i < 6; i++){ //翼端のmpu |
tsumagari | 26:50272431cd1e | 322 | if(!(i%2)){ |
tsumagari | 26:50272431cd1e | 323 | writeDatas[write_datas_index][i] = (int)( (soudaDatas[i*2]<<8) + soudaDatas[(i+1)*2] ); |
tsumagari | 26:50272431cd1e | 324 | }else{ |
tsumagari | 26:50272431cd1e | 325 | writeDatas[write_datas_index][i] = (int)( (soudaDatas[i*2-1]<<8) + soudaDatas[i*2+1] ); |
tsumagari | 26:50272431cd1e | 326 | } |
tsumagari | 26:50272431cd1e | 327 | // writeDatas[write_datas_index][i] = i; |
taurin | 8:31e07f6ed0f7 | 328 | } |
tsumagari | 26:50272431cd1e | 329 | for(i = 6; i < 12; i++){//翼端のV,操舵 |
tsumagari | 26:50272431cd1e | 330 | //writeDatas[write_datas_index][i] = 0.0; |
tsumagari | 26:50272431cd1e | 331 | writeDatas[write_datas_index][i] = (float)soudaDatas[i+6]; |
tsumagari | 26:50272431cd1e | 332 | } |
taurin | 8:31e07f6ed0f7 | 333 | writeDatas[write_datas_index][i++] = pitch; |
taurin | 8:31e07f6ed0f7 | 334 | writeDatas[write_datas_index][i++] = roll; |
taurin | 8:31e07f6ed0f7 | 335 | writeDatas[write_datas_index][i++] = yaw; |
taurin | 8:31e07f6ed0f7 | 336 | writeDatas[write_datas_index][i++] = airSpeed; |
tsumagari | 29:2da9b8d03c0b | 337 | writeDatas[write_datas_index][i++] = sonarDist; |
tsumagari | 29:2da9b8d03c0b | 338 | writeDatas[write_datas_index][i++] = cadence.cadence; |
taurin | 8:31e07f6ed0f7 | 339 | //writeDatas[write_datas_index][i++] = writeTimer.read(); |
tsumagari | 26:50272431cd1e | 340 | for(i = 0; i < WRITE_DATAS_NUM; i++){ |
tsumagari | 26:50272431cd1e | 341 | pc.printf("%f ", writeDatas[write_datas_index][i]); |
taurin | 9:95eb0bbdc2a9 | 342 | // twe.printf("%f,", writeDatas[write_datas_index][i]); |
tsumagari | 26:50272431cd1e | 343 | } |
tsumagari | 26:50272431cd1e | 344 | pc.printf("\n\r"); |
taurin | 9:95eb0bbdc2a9 | 345 | // twe.printf("\n\r"); |
tsumagari | 23:e59afb2044df | 346 | if(write_datas_index < SD_WRITE_NUM-1){ |
taurin | 8:31e07f6ed0f7 | 347 | write_datas_index++; |
taurin | 8:31e07f6ed0f7 | 348 | } |
taurin | 9:95eb0bbdc2a9 | 349 | for(int i = 0; i < SOUDA_DATAS_NUM; i++){ |
tsumagari | 26:50272431cd1e | 350 | // pc.printf("%i ",soudaDatas[i]); |
tsumagari | 26:50272431cd1e | 351 | ssMutex.lock(); |
YusukeWakuta | 10:1389f938f5f7 | 352 | twe.printf("%i,",soudaDatas[i]); |
tsumagari | 26:50272431cd1e | 353 | ssMutex.unlock(); |
taurin | 9:95eb0bbdc2a9 | 354 | } |
tsumagari | 29:2da9b8d03c0b | 355 | |
tsumagari | 29:2da9b8d03c0b | 356 | if(Android.writeable()){ |
tsumagari | 29:2da9b8d03c0b | 357 | Android.printf("%f,%f,%f",airSpeed,roll,0); |
tsumagari | 29:2da9b8d03c0b | 358 | } |
tsumagari | 29:2da9b8d03c0b | 359 | |
tsumagari | 26:50272431cd1e | 360 | ssMutex.lock(); |
YusukeWakuta | 10:1389f938f5f7 | 361 | twe.printf("%f,%f,%f,",pitch,roll,yaw); |
tsumagari | 26:50272431cd1e | 362 | // twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); |
YusukeWakuta | 10:1389f938f5f7 | 363 | twe.printf("%f,\r\n",airSpeed); |
tsumagari | 26:50272431cd1e | 364 | ssMutex.unlock(); |
tsumagari | 26:50272431cd1e | 365 | // pc.printf("%f,%f,%f\n\r",pitch,roll,yaw); |
taurin | 9:95eb0bbdc2a9 | 366 | //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); |
tsumagari | 26:50272431cd1e | 367 | // pc.printf("%f\n\r",airSpeed); |
taurin | 9:95eb0bbdc2a9 | 368 | //SDprintf(); |
tsumagari | 23:e59afb2044df | 369 | pc.printf("%d\n\r",write_datas_index); |
taurin | 9:95eb0bbdc2a9 | 370 | } |
taurin | 9:95eb0bbdc2a9 | 371 | |
taurin | 9:95eb0bbdc2a9 | 372 | void WriteDatasF(){ |
taurin | 9:95eb0bbdc2a9 | 373 | pc.printf("airSpeed:%f\n\r",airSpeed); |
taurin | 0:085b2c5e3254 | 374 | } |
tsumagari | 29:2da9b8d03c0b | 375 | |
tsumagari | 26:50272431cd1e | 376 | //float calcKXdeg(float x){ |
tsumagari | 26:50272431cd1e | 377 | // return -310.54*x+156.65; |
tsumagari | 26:50272431cd1e | 378 | //} |
taurin | 0:085b2c5e3254 | 379 | |
tsumagari | 26:50272431cd1e | 380 | //float calcAttackAngle(){ |
tsumagari | 26:50272431cd1e | 381 | // return pitch-calcKXdeg(kx_Z.read()); |
tsumagari | 26:50272431cd1e | 382 | //} |
taurin | 0:085b2c5e3254 | 383 | |
taurin | 0:085b2c5e3254 | 384 | void RollAlarm(){ |
taurin | 0:085b2c5e3254 | 385 | if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){ |
taurin | 0:085b2c5e3254 | 386 | RollAlarmL = 1; |
taurin | 0:085b2c5e3254 | 387 | } |
taurin | 0:085b2c5e3254 | 388 | else{ |
taurin | 0:085b2c5e3254 | 389 | RollAlarmL = 0; |
taurin | 0:085b2c5e3254 | 390 | } |
taurin | 0:085b2c5e3254 | 391 | if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){ |
taurin | 0:085b2c5e3254 | 392 | RollAlarmR = 1; |
taurin | 0:085b2c5e3254 | 393 | } |
taurin | 0:085b2c5e3254 | 394 | else{ |
taurin | 0:085b2c5e3254 | 395 | RollAlarmR = 0; |
taurin | 0:085b2c5e3254 | 396 | } |
taurin | 0:085b2c5e3254 | 397 | } |
taurin | 0:085b2c5e3254 | 398 | |
tsumagari | 26:50272431cd1e | 399 | |
taurin | 4:a863a092141c | 400 | |
tsumagari | 29:2da9b8d03c0b | 401 | int main(){ |
tsumagari | 23:e59afb2044df | 402 | Thread sd_thread(&SDprintf);//必ずmain内で |
tsumagari | 26:50272431cd1e | 403 | Thread sonar_thread(&sonarCalc); |
tsumagari | 26:50272431cd1e | 404 | Thread mpu_thread(&mpuProcessing); |
tsumagari | 26:50272431cd1e | 405 | Thread soudaSerial_thread(&DataReceiveFromSouda); |
tsumagari | 26:50272431cd1e | 406 | Thread cadence_thread(&cadenceDataReceive); |
taurin | 0:085b2c5e3254 | 407 | init(); |
taurin | 0:085b2c5e3254 | 408 | while(1){ |
taurin | 9:95eb0bbdc2a9 | 409 | pc.printf("test\n\r"); |
taurin | 8:31e07f6ed0f7 | 410 | RollAlarm(); |
tsumagari | 26:50272431cd1e | 411 | // DataReceiveFromSouda(); |
tsumagari | 15:6966299bea4c | 412 | // cadence.readData(); |
taurin | 3:8dc516be2e7e | 413 | WriteDatas(); |
tsumagari | 29:2da9b8d03c0b | 414 | // mpuProcessing(); |
taurin | 0:085b2c5e3254 | 415 | } |
taurin | 0:085b2c5e3254 | 416 | } |