2018年度計器mbed用プログラム
Dependencies: BufferedSoftSerial2 INA226_ver1 mbed-rtos mbed SDFileSystem-RTOS
Fork of keiki2017 by
Diff: main.cpp
- Branch:
- Thread-gyogetsuMPU
- Revision:
- 44:c51b60c74fef
- Parent:
- 38:32f483b0a77f
- Child:
- 45:73ac4054ea23
--- a/main.cpp Thu Mar 09 13:36:24 2017 +0000 +++ b/main.cpp Sun Mar 12 16:39:58 2017 +0000 @@ -21,8 +21,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(); } @@ -81,42 +82,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); @@ -124,22 +132,24 @@ twe.baud(19200);//BufferedSoftSerialでは19200が上限。twelite側でもBPS無効化が必要 //writeTimer.start(); FusokukeiInit(); -// MpuInit(); +// MpuInit(); //writeDatasTicker.attach(&WriteDatas,1); // cadenceUpdateTicker.attach(&updateCadence, 1); - + //-----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 @@ -156,17 +166,13 @@ } } else { //pc.printf("out\n\r"); // Loop forever if communication doesn't happen - } + } } -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(); @@ -208,25 +214,27 @@ }//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) } -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; @@ -247,80 +255,57 @@ // } // else{ // write_datas_index++; -// } - for(int i = 0; i < SOUDA_DATAS_NUM; i++){ +// } + for(int i = 0; i < SOUDA_DATAS_NUM; i++) { pc.printf("%i ",soudaDatas[i]); twe.printf("%i,",soudaDatas[i]); // android.printf("%i,",soudaDatas[i]); } //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,%f,%f,%f\r\n",pitch,roll,yaw,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",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); pc.printf("%f,%f,%s\n\r",airSpeed,sonarDist,cadence_twe.myBuff); pc.printf(cadence_twe.strData.c_str()); pc.putc(cadence_twe.strV[0]); - 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); } -//float calcKXdeg(float x){ -// return -310.54*x+156.65; -//} - -//float calcAttackAngle(){ -// 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(){ - //kisokuServo.pulsewidth(calcPulse(airSpeed*10)); -// kisokuServo.pulsewidth(calcPulse(9*airSpeed)); - if(pitch<0){ -// geikakuServo.pulsewidth(calcPulse(0)); - } - else{ -// geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0))); - } - //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();