2018年度計器mbed用プログラム
Dependencies: BufferedSoftSerial2 INA226_ver1 mbed-rtos mbed SDFileSystem-RTOS
Fork of keiki2017 by
Diff: main.cpp
- Branch:
- Thread-gyogetsuMPU
- Revision:
- 46:c649987c4d84
- Parent:
- 42:73c3862e4c12
- Child:
- 47:b3c81142cad6
- Child:
- 48:0c9f3a057f79
--- a/main.cpp Sat Mar 18 14:09:42 2017 +0000 +++ b/main.cpp Fri Mar 31 07:33:34 2017 +0000 @@ -7,7 +7,8 @@ #include "BufferedSoftSerial.h" //#include "SDFileSystem.h" -#define SOUDA_DATAS_NUM 24 //(yokutan 7 + input 5)*2 +#define SOUDA_DATAS_NUM 28 //(yokutan 7 + input 7)*2 +#define YOKUTAN_DATAS_NUM 14 #define WRITE_DATAS_NUM 30 // souda_datas_num + 6( rpy, airspeed, height, cadence) #define SD_WRITE_NUM 20 #define MPU_LOOP_TIME 0.01 @@ -22,8 +23,9 @@ extern "C" void mbed_reset(); InterruptIn resetPin(p25); Timer resetTimeCount; -void resetInterrupt(){ - while(resetPin){ +void resetInterrupt() +{ + while(resetPin) { resetTimeCount.start(); if(resetTimeCount.read()>3) mbed_reset(); } @@ -87,42 +89,49 @@ float calcAttackAngle(); float calcKXdeg(float x); -void air_countUp(){ +void air_countUp() +{ air_kaitensu++; } -void call_calcAirSpeed(){ +void call_calcAirSpeed() +{ air.calcAirSpeed(air_kaitensu); air_kaitensu = 0; } -void sonarInterruptStart(){ +void sonarInterruptStart() +{ // sonarTimer.start(); } -void sonarInterruptStop(){ +void sonarInterruptStop() +{ // sonarTimer.stop(); // sonarDistTime = sonarTimer.read_us(); // sonarTimer.reset(); // sonarDist = sonarDistTime*0.018624 - 13.511; } -void sonarCalc(){ +void sonarCalc() +{ sonarV = 0; - for(int i = 0; i<20; i++){ + for(int i = 0; i<20; i++) { sonarV += sonarPin.read(); wait(0.01); } sonarDist = (sonarV/20)*2064.5;// volt*3.3*1000/1.6 (電圧/距離:3.2mV/2cm) } -void updateCadence(/*void const *arg*/){ +void updateCadence(/*void const *arg*/) +{ // while(1){ - cadence_twe.readData(); + cadence_twe.readData(); // Thread::wait(5); // } } -void init(){ +void init() +{ //--------------------------------------(resetInterrupt init) resetPin.rise(resetInterrupt); resetPin.mode(PullDown); @@ -131,22 +140,24 @@ //writeTimer.start(); FusokukeiInit(); // SdInit(); -// MpuInit(); +// MpuInit(); //writeDatasTicker.attach(&WriteDatas,1); // cadenceUpdateTicker.attach(&updateCadence, 0.2); - + //-----for InterruptMode of sonar---------------------------- // sonarPin.rise(&sonarInterruptStart); // sonarPin.fall(&sonarInterruptStop); //----------------------------------------------------------- } -void FusokukeiInit(){ +void FusokukeiInit() +{ FusokukeiPin.rise(air_countUp); FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME); } -void MpuInit(){ +void MpuInit() +{ i2c.frequency(400000); // use fast (400 kHz) I2C t.start(); uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 @@ -163,17 +174,19 @@ } } else { //pc.printf("out\n\r"); // Loop forever if communication doesn't happen - } + } } -double calcPulse(int deg){ +double calcPulse(int deg) +{ return (0.0006+(deg/180.0)*(0.00235-0.00045)); } -void mpuProcessing(void const *arg){ +void mpuProcessing(void const *arg) +{ MpuInit(); - while(1){ + while(1) { if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt mpu6050.readAccelData(accelCount); // Read the x/y/z adc values mpu6050.getAres(); @@ -215,18 +228,19 @@ }//while(1) } -void DataReceiveFromSouda(/*void const *arg*/){ +void DataReceiveFromSouda(/*void const *arg*/) +{ // while(1){ - if(soudaSerial.readable()){ + if(soudaSerial.readable()) { led2 = !led2; char c = soudaSerial.getc(); - while( c != ';' ){ + while( c != ';' ) { c = soudaSerial.getc(); } - for(int i = 0; i < SOUDA_DATAS_NUM; i++){ + for(int i = 0; i < SOUDA_DATAS_NUM; i++) { soudaDatas[i] = soudaSerial.getc(); } - }//if + }//if // }//while(1) } // @@ -254,10 +268,11 @@ // fclose(fp); //} -void WriteDatas(){ +void WriteDatas() +{ int i; - for(i = 0; i < SOUDA_DATAS_NUM; i++){ - //writeDatas[write_datas_index][i] = 0.0; + for(i = 0; i < SOUDA_DATAS_NUM; i++) { + //writeDatas[write_datas_index][i] = 0.0; writeDatas[write_datas_index][i] = (float)soudaDatas[i]; } writeDatas[write_datas_index][i++] = pitch; @@ -273,39 +288,49 @@ // } // pc.printf("\n\r"); // twe.printf("\n\r"); - if(write_datas_index == SD_WRITE_NUM-1){ + if(write_datas_index == SD_WRITE_NUM-1) { // SDprintf(); write_datas_index=0; - } - else{ + } else { write_datas_index++; - } - for(int i = 0; i < SOUDA_DATAS_NUM; i++){ + } + for(int i = 0; i <YOKUTAN_DATAS_NUM ; i++) { pc.printf("%i ",soudaDatas[i]); twe.printf("%i,",soudaDatas[i]); // android.printf("%i,",soudaDatas[i]); } + twe.printf("%d,%i,%d,%i,",soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1]); + pc.printf("%d,%i,%d,%i,",soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1]); + /* + 送信文字列 + 0-13翼端データ + 14-17 R erebon + 18 R DRUG + 19-22 L erebon + 23 LDRUG + */ //pc.printf("\n\r"); twe.printf("%f,%f,%f,",pitch,roll,yaw); - twe.printf("%f,%f,%f\r\n",airSpeed,sonarDist,cadence_twe.cadence); + twe.printf("%f,%f,%f\n",airSpeed,sonarDist,cadence_twe.cadence); pc.printf("%f,%f,%f\n\r",pitch,roll,yaw); //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); pc.printf("%f,%f,%f\n\r",airSpeed,sonarDist,cadence_twe.cadence); // for(int i = 0; i < strlen(cadence_twe.myBuff); i++){ // pc.printf("%c",*(cadence_twe.myBuff+i)); // } - if(android.writeable()){ + if(android.writeable()) { // for(int i = 0; i<SOUDA_DATAS_NUM; i++){ // android.printf("%i,",soudaDatas[i]); // } // android.printf("%f,%f,%f,",pitch,roll,yaw); -// android.printf("%f,%f,\r\n",airSpeed,sonarDist); +// android.printf("%f,%f,\r\n",airSpeed,sonarDist); android.printf("%f,%f,test\n\r",roll,airSpeed); } // SDprintf(); } -void WriteDatasF(){ +void WriteDatasF() +{ pc.printf("airSpeed:%f\n\r",airSpeed); } @@ -317,43 +342,43 @@ // return pitch-calcKXdeg(kx_Z.read()); //} -void RollAlarm(){ - if((roll < -ROLL_L_MAX_DEG ) && (roll > ROLL_L_MAX_DEG-180)){ +void RollAlarm() +{ + if((roll < -ROLL_L_MAX_DEG ) && (roll > ROLL_L_MAX_DEG-180)) { RollAlarmL = 1; - } - else{ + } else { RollAlarmL = 0; } - - if((roll > ROLL_R_MAX_DEG) && (roll < 180-ROLL_R_MAX_DEG)){ + + if((roll > ROLL_R_MAX_DEG) && (roll < 180-ROLL_R_MAX_DEG)) { RollAlarmR = 1; - } - else{ + } else { RollAlarmR = 0; } } -void WriteServo(){ +void WriteServo() +{ //kisokuServo.pulsewidth(calcPulse(airSpeed*10)); // kisokuServo.pulsewidth(calcPulse(9*airSpeed)); - if(pitch<0){ + if(pitch<0) { // geikakuServo.pulsewidth(calcPulse(0)); - } - else{ + } else { // geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0))); } - //pc.printf("a:%f",airSpeed); - //pc.printf("p:%f\r\n",pitch); + ////pc.printf("a:%f",airSpeed); + pc.printf("p:%f\r\n",pitch); //kisokuServo.pulsewidth(calcPulse(0)); //geikakuServo.pulsewidth(calcPulse(0)); } -int main(){ +int main() +{ // Thread cadence_thread(&updateCadence); Thread mpu_thread(&mpuProcessing); // Thread soudaSerial_thread(&DataReceiveFromSouda); init(); - while(1){ + while(1) { pc.printf("test\n\r"); // mpuProcessing(); sonarCalc();