Diff: new_serial.cpp

Revision:

2:83ea259e8ce5

Parent:

1:4108360238c0

Child:

3:a7df2c55da1a

diff -r 4108360238c0 -r 83ea259e8ce5 new_serial.cpp
--- a/new_serial.cpp	Wed Aug 26 19:59:03 2020 +0000
+++ b/new_serial.cpp	Wed Aug 26 22:04:49 2020 +0000
@@ -4,6 +4,11 @@
 // Aug 27, 2020
 
 #include "mbed.h"
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+ 
 
 PwmOut servo0(p21);//θ0に対応するピン
 PwmOut servo1(p22);//θ1に対応するピン
@@ -12,8 +17,7 @@
 PwmOut servo4(p25);//θ4に対応するピン
 PwmOut servo5(p26);//θ5に対応するピン
 
-char rbuf[4 * 6];
-float fret[6];
+char rbuf[4 * 6 + 1];
 const int size_of_float = 4;
 
 //name.baud(9600);
@@ -21,6 +25,7 @@
 
 Serial pc(USBTX, USBRX);
 LocalFileSystem local("local");
+#define NUM_OF_LOG_LINES 60
 
 float aOut, bOut, cOut, dOut, eOut, fOut;//それぞれの角度
 int num[30];//数字格納場所，基本的に1桁の数字しか入らない
@@ -157,7 +162,7 @@
     //  std::printf("The byte #%zu is 0x%02X\n", i, ret[i]);
     //}
 }
-void byte2float(float *f, unsigned char ret[])
+void byte2float(float *f,  char ret[])
 {
     //
     // reference : https://stackoverflow.com/questions/14018894/how-to-convert-float-to-byte-array-of-length-4-array-of-char
@@ -175,9 +180,8 @@
     float output[500][6];
     float w = 60.0;//========制御周波数===========
     float PWMperiod = 1.0 / w;    //PWM周期の計算
-    char ch;
     //===============ボーレート=============
-    pc.baud(115200);
+    pc.baud(921600);
     //各サーボの設定
     servo0.period(PWMperiod);
     servo1.period(PWMperiod);
@@ -193,55 +197,66 @@
     //受け取り配列numの初期化
     num_ini();
 
-    for(int i=0; i<6; i++)
-        fret[i] = 0.0f;
-
-
-    //while (1)
-    for(int counter=0; counter<100; counter++) {
+    float fret;
+    while(1) {
+        int counter = 0;
+        while (counter < NUM_OF_LOG_LINES) {
+            
+            led1 = 1;
+            if(pc.readable()) {
+                //  receive 24 bytes + null character
+                // https://os.mbed.com/questions/77081/Why-does-he-get-only-3-characters-when-I/
+                pc.gets(rbuf, 4+1);
+                // ( 4 bytes -> one float ) x 6dof
+                for(int i=0; i<6; i++) {
+                    byte2float(&fret, &rbuf[i * size_of_float]);
+                    output[counter][i] = fret;
+                    output[counter][5] = 3.14159;
+                    led2 = 1;
+                }
+                counter++;
+            }
 
-        if(pc.readable()) {
-            //  receive 24 bytes
-            pc.gets(rbuf, 24);
-            // ( 4 bytes -> one float ) x 6dof
-            for(int i=0; i<6; i++) {
-                byte2float(&fret[i], &rbuf[i*4]);
-                output[counter][i] = fret[i];
-            }
-        }
-
-        //for (int i = 0; i < 30; i++)
+            //for (int i = 0; i < 30; i++)
 //        {
 //            ch=pc.getc();//一文字ずつ受け取る
 //            num[i] = ctoi(ch);//数字を数値に変換
 //        }
 //
-        //動かす関数
-        move();
+            //動かす関数
+//            move();
 
-        //csvファイル書き込みのために配列に保存
+            //csvファイル書き込みのために配列に保存
 //        output[counter][0]=aOut;
 //        output[counter][1]=bOut;
 //        output[counter][2]=cOut;
 //        output[counter][3]=dOut;
 //        output[counter][4]=eOut;
 //        output[counter][5]=fOut;
-        //動かしたら初期化する
-        num_ini();
+            //動かしたら初期化する
+//            num_ini();
 
+        }
+        
+        led3 = 1;    
+        //保存用の部分，使わないならこめんとあうと
+        FILE* fp;
+        fp = fopen("/local/Output.csv", "w");
+        wait(0.1);
+        if(fp!=NULL) {
+            fprintf(fp, "aOut,bOut,cOut,dOut,eOut,fOut\n");
+            for(int j =0; j<NUM_OF_LOG_LINES; j++) {
+                fprintf(fp, "%f,%f,%f,%f,%f,%f\n", output[j][0],output[j][1],output[j][2],output[j][3],output[j][4],output[j][5]);
+            }
+        }
+        led4 = 1;  
+        wait(0.1);
+        fclose(fp);
+        
+        led1 = 0;    
+        led2 = 0;    
+        led3 = 0;    
+        led4 = 0;    
     }
-    //保存用の部分，使わないならこめんとあうと
-    FILE* fp;
-    fp = fopen("/local/Output.csv", "w");
-    if(fp==NULL) {
-        //pc.printf("error");
-    }
-    //while (1)
-    fprintf(fp, "aOut,bOut,cOut,dOut,eOut,fOut\n");
-    for(int j =0; j<500; j++) {
-        fprintf(fp, "%f,%f,%f,%f,%f,%f\n", output[j][0],output[j][1],output[j][2],output[j][3],output[j][4],output[j][5]);
-    }
-    fclose(fp);
-    //wait(0.1);
 
 }
\ No newline at end of file