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();