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