Diff: new_serial.cpp

Revision:

5:7f031c7e4694

Parent:

4:066d90d485d3

diff -r 066d90d485d3 -r 7f031c7e4694 new_serial.cpp
--- a/new_serial.cpp	Thu Aug 27 05:18:24 2020 +0000
+++ b/new_serial.cpp	Thu Aug 27 14:04:49 2020 +0000
@@ -17,8 +17,6 @@
 PwmOut servo4(p25);//θ4に対応するピン
 PwmOut servo5(p26);//θ5に対応するピン
 
-char rbuf[4 + 1];
-const int size_of_float = 4;
 
 //name.baud(9600);
 //MG996Rのほう
@@ -148,41 +146,14 @@
     send_servo();
 }
 
-void float2byte(unsigned char ret[], float f)
-{
-    //
-    // reference : https://stackoverflow.com/questions/14018894/how-to-convert-float-to-byte-array-of-length-4-array-of-char
-
-    unsigned char const* p = reinterpret_cast<unsigned char const*>(&f);
-
-    memcpy(ret, p, size_of_float);
-
-    //for (std::size_t i = 0; i != size_of_float; ++i)
-    //{
-    //  std::printf("The byte #%zu is 0x%02X\n", i, ret[i]);
-    //}
-}
-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
-
-    char fbuf[4];
-    memcpy(fbuf, ret, 4);
-
-    float const* p = reinterpret_cast<float const*>(ret);
-    memcpy(f, p, size_of_float);
-
-    //std::printf("The value is %3.3lf\n", *f);
-}
 int main()
 {
-    float output[500][6];
     float w = 60.0;//========制御周波数===========
     float PWMperiod = 1.0 / w;    //PWM周期の計算
     //===============ボーレート=============
     pc.baud(921600);
     pc.format(8, Serial::None, 1);
+    
     //各サーボの設定
     servo0.period(PWMperiod);
     servo1.period(PWMperiod);
@@ -191,99 +162,42 @@
     servo4.period(PWMperiod);
     servo5.period(PWMperiod);
 
-    ///受け取り準備開始
-    //pc.printf("serial starts\n");
-
-    //シリアル通信をスタートする
-    //受け取り配列numの初期化
     num_ini();
 
-    float fret;
-    char c;
-
+    char rbuf[4];
+    
     while(1) {
-        int counter = 0;
-        while (counter < NUM_OF_LOG_LINES) {
-
-            led1 = 1;
-            if(pc.readable()) {
-                c = pc.getc();
-                if (c != '1')
-                    continue;
-                c = pc.getc();
-                if (c != '2')
-                    continue;
-                c = pc.getc();
-                if (c != '3')
-                    continue;
-
-                for(int i=0; i<6; i++) {
-                    led1 = 0;
-                    for(int b=0; b<4; b++)
-                        rbuf[b] = pc.getc();
-
-                    led1 = 1;
-                    byte2float(&fret, rbuf);
-
-                    // ( 4 bytes -> one float )
-                    output[counter][i] = fret;
-                }
-
-                led2 = 1;
-                //c = pc.getc();
-                //if (c != NULL)               continue;
-
-                output[counter][5] = 3.14159;
-                led3 = 1;
-                counter++;
-            }
-
-            //for (int i = 0; i < 30; i++)
-//        {
-//            ch=pc.getc();//一文字ずつ受け取る
-//            num[i] = ctoi(ch);//数字を数値に変換
-//        }
-//
-            //動かす関数
-//            move();
-
-            //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();
-
-        }
-
-        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;
-    }
+        if(pc.readable()) {
+            if (pc.getc() != '*') continue;
+            if (pc.getc() != '+') continue;
+            for(int i=0; i<6; i++) {
+                for(int b=0; b<4; b++)
+                    rbuf[b] = pc.getc();
 
+                // ( 4 bytes -> one float )
+                float const* f = reinterpret_cast<float const*>(rbuf);
+                
+                if(*f == 5.3f)
+                    led1 = 1;
+                if(*f == 5.2f)
+                    led2 = 1;
+                if(*f == -0.3f)
+                    led3 = 1;
+                if(*f == 40.0f)
+                    led4 = 1;
+            }
+        }
+
+//
+        //動かす関数
+//            move();
+        //動かしたら初期化する
+//            num_ini();
+
+    }
 }
\ No newline at end of file