2018年度計器mbed用プログラム
Dependencies: BufferedSoftSerial2 INA226_ver1 mbed-rtos mbed SDFileSystem-RTOS
Fork of keiki2017 by
Diff: main.cpp
- Branch:
- SDandCadenceThread
- Revision:
- 75:2ac3d145942f
- Parent:
- 73:2122c87cac9b
diff -r 2122c87cac9b -r 2ac3d145942f main.cpp --- a/main.cpp Fri Jun 16 14:33:37 2017 +0000 +++ b/main.cpp Sat Jun 24 14:11:19 2017 +0000 @@ -10,8 +10,8 @@ #define SOUDA_DATAS_NUM 28 //(yokutan 7 + input 7)*2 #define YOKUTAN_DATAS_NUM 14 -#define WRITE_DATAS_NUM 34 // souda_datas_num + 6( rpy, airspeed, height, cadence) -#define SD_WRITE_NUM 20 +#define WRITE_DATAS_NUM 25 // time + yokutan_datas_num+ 4(input) + 6( rpy, airspeed, height, cadence) +#define SD_WRITE_NUM 40 #define MPU_LOOP_TIME 0.01 #define AIR_LOOP_TIME 0.01//(0.002005) #define WRITE_DATAS_LOOP_TIME 1 @@ -20,17 +20,23 @@ #define MPU_DELT_MIN 250 #define INIT_SERVO_PERIOD_MS 20 +#define DEBUG(...) { pc.printf(__VA_ARGS__); } + //-----------------------------------(resetInterrupt def) extern "C" void mbed_reset(); InterruptIn resetPin(p25); -Timer resetTimeCount; +//Timer resetTimeCount; +Timer mainTimer; void resetInterrupt() { + int start = mainTimer.read(); while(resetPin) { - resetTimeCount.start(); - if(resetTimeCount.read()>3) mbed_reset(); +// resetTimeCount.start(); +// if(resetTimeCount.read()>3) mbed_reset(); + if(mainTimer.read()-start > 3) mbed_reset(); } - resetTimeCount.reset(); +// led3 = 0; +// resetTimeCount.reset(); } //------------------------------------------------------- @@ -110,10 +116,12 @@ air_kaitensu++; // led3 = !led3; } - +int cnt = 0; void call_calcAirSpeed() { air.calcAirSpeed(air_kaitensu); +// if(!(cnt%10))pc.printf("airspeed:%f\tair_kaitensu:%d\n",airSpeed,air_kaitensu); +// cnt++; air_kaitensu = 0; } @@ -132,9 +140,9 @@ void sonarCalc() { sonarV = 0; - for(int i = 0; i<20; i++) { + for(int i = 0; i<10; i++) { sonarV += sonarPin.read(); - wait(0.01); + Thread::wait(5); } sonarDist = (sonarV/20)*2064.5;// volt*3.3*1000/1.6 (電圧/距離:3.2mV/2cm) } @@ -191,6 +199,7 @@ void init() { pc.printf("(BUILD:[" __DATE__ "/" __TIME__ "])\n\r"); + mainTimer.start(); //--------------------------------------(resetInterrupt init) resetPin.rise(resetInterrupt); resetPin.mode(PullDown); @@ -321,7 +330,7 @@ drugL = soudaDatas[26]- '0'; inputL = atof(cErebonL); - pc.printf("erebonR:%5.5f, drugR:%d erebonL:%5.5f drugL:%d",inputR,drugR,inputL,drugL); +// pc.printf("erebonR:%5.5f, drugR:%d erebonL:%5.5f drugL:%d",inputR,drugR,inputL,drugL); // pc.printf("erebonR:%s, drugR:%d erebonL:%s drugL:%d",cErebonR,drugR,cErebonL,drugL); }//if // }//while(1) @@ -367,11 +376,16 @@ void WriteDatas() { - int i; - for(i = 0; i < SOUDA_DATAS_NUM; i++) { + int i=0; + writeDatas[write_datas_index][i++] = mainTimer.read(); + for(int j = 0; j < YOKUTAN_DATAS_NUM; i++,j++) { //writeDatas[write_datas_index][i] = 0.0; - writeDatas[write_datas_index][i] = (float)soudaDatas[i]; + writeDatas[write_datas_index][i] = (float)soudaDatas[j]; } + writeDatas[write_datas_index][i++] = inputR; + writeDatas[write_datas_index][i++] = drugR; + writeDatas[write_datas_index][i++] = inputL; + writeDatas[write_datas_index][i++] = drugL; writeDatas[write_datas_index][i++] = pitch; writeDatas[write_datas_index][i++] = roll; writeDatas[write_datas_index][i++] = yaw; @@ -379,11 +393,11 @@ writeDatas[write_datas_index][i++] = sonarDist; writeDatas[write_datas_index][i++] = cadenceResult;//cadence_twe.cadence; //writeDatas[write_datas_index][i++] = writeTimer.read(); - //for(i = 0; i < WRITE_DATAS_NUM; i++){ -// ////pc.printf("%f ", writeDatas[write_datas_index][i]); + for(i = 0; i < WRITE_DATAS_NUM; i++){ + DEBUG("%f ", writeDatas[write_datas_index][i]); // twe.printf("%f,", writeDatas[write_datas_index][i]); -// } -// //pc.printf("\n\r"); + } + DEBUG("\n\r"); // twe.printf("\n\r"); if(write_datas_index == SD_WRITE_NUM-1) { // SDprintf(); @@ -391,7 +405,7 @@ } else { write_datas_index++; } - char sbuf[128]; + char sbuf[YOKUTAN_DATAS_NUM*2 + 5]; int p=0; // twe.printf("con,"); p += sprintf(sbuf,"con,"); @@ -429,7 +443,7 @@ // soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1], // pitch,roll,yaw, // airSpeed,sonarDist,cadenceResult); - pc.printf("cadence:%5.5f\n\r",cadenceResult); +// pc.printf("cadence:%5.5f\n\r",cadenceResult); // for(int i = 0; i < strlen(cadence_twe.myBuff); i++){ // ////pc.printf("%c",*(cadence_twe.myBuff+i)); @@ -438,21 +452,11 @@ if(android.writeable()) { // android.printf("%f,%f,%f,",pitch,roll,yaw); // android.printf("%f,%f,\r\n",airSpeed,sonarDist); - android.printf("%4.2f,%4.2f,%4.2f,\n,",roll,airSpeed,cadenceResult);//cadence_twe.cadence); -// led2 = !led2; + android.printf("%4.2f,%4.2f,%4.2f,\n,",roll,airSpeed,cadenceResult); } // SDprintf(); } -void WriteDatasF() -{ - //pc.printf("airSpeed:%f\n\r",airSpeed); -} - -//float calcKXdeg(float x){ -// return -310.54*x+156.65; -//} - void RollAlarm() { if((roll < -ROLL_L_MAX_DEG ) && (roll > ROLL_L_MAX_DEG-180)) { @@ -474,14 +478,14 @@ Thread SD_thread(&SDprintf); // Thread soudaSerial_thread(&DataReceiveFromSouda); init(); - int VCcounter = 0; +// int VCcounter = 0; while(1) { - if(VCcounter%20 == 0 ) { - if( VCmonitor.getVoltage(&V) == 0) { - pc.printf("e:%f\n",V); - } - } - VCcounter++; +// if(VCcounter%20 == 0 ) { +// if( VCmonitor.getVoltage(&V) == 0) { +// pc.printf("e:%f\n",V); +// } +// } +// VCcounter++; // updateCadence(V,mgPin.read() * 3.3,mgPin2.read() * 3.3,isFirstCadenceFlag); //pc.printf("test\n\r");