Revision 1:4108360238c0, committed 2020-08-26

Comitter:

mfurukawa

Date:

Wed Aug 26 19:59:03 2020 +0000

Parent:

0:67dece35504d

Child:

2:83ea259e8ce5

Commit message:

under const. binary recieveing

Changed in this revision

--- a/new_serial.cpp	Wed Aug 26 09:00:08 2020 +0000
+++ b/new_serial.cpp	Wed Aug 26 19:59:03 2020 +0000
@@ -1,3 +1,8 @@
+// Created by Tomoki Hirayama
+//
+// Modified by Masahiro Furukawa
+// Aug 27, 2020
+
 #include "mbed.h"
 
 PwmOut servo0(p21);//θ0に対応するピン
@@ -7,6 +12,10 @@
 PwmOut servo4(p25);//θ4に対応するピン
 PwmOut servo5(p26);//θ5に対応するピン
 
+char rbuf[4 * 6];
+float fret[6];
+const int size_of_float = 4;
+
 //name.baud(9600);
 //MG996Rのほう
 
@@ -22,15 +31,13 @@
 //きちんと値が受け取れているかチェックするための初期化
 void num_ini()
 {
-    for (int i = 0; i < 30; i++)
-    {
+    for (int i = 0; i < 30; i++) {
         num[i] = 9999;
     }
 }
 bool num_check(int n[])
 {
-    for (int i = 0; i < 24; i++)
-    {
+    for (int i = 0; i < 24; i++) {
         if (n[i] == 9999)
             return -1;
     }
@@ -40,35 +47,38 @@
 int ctoi(char c)
 {
     //1文字の数字（char型）を数値（int型）に変換
-    if ('0' <= c && c <= '9')
-    {
+    if ('0' <= c && c <= '9') {
         return (c - '0');
-    }
-    else
-    {
+    } else {
         return -1;
     }
 }
 
 //角度[°]から入力パルス幅[us]に変換する関数
-int cal_input0(float arg) {
+int cal_input0(float arg)
+{
     return 1475 + int(10.48 * arg);
 }
-int cal_input1(float arg) {
+int cal_input1(float arg)
+{
     //return 1520 + 10.2467 * (-30.0 + arg);
     return 1218 + int(10.2467 * arg);
 }
-int cal_input2(float arg) {
+int cal_input2(float arg)
+{
     //return 853 + 10.59 * (30.0 + arg);
     return 1306 + 10.59 * arg;
 }
-int cal_input3(float arg) {
+int cal_input3(float arg)
+{
     return 1224 + int(8.556 * arg);
 }
-int cal_input4(float arg) {
+int cal_input4(float arg)
+{
     return 1460 + int(10.556 * arg);
 }
-int cal_input5(float arg) {
+int cal_input5(float arg)
+{
     return 1922 + int(10.556 * arg);
 }
 
@@ -89,7 +99,7 @@
     //eOut = 1000 * num[20] + 100 * num[19] + 10 * num[18] + 1 * num[17];//
     //fOut = 1000 * num[24] + 100 * num[23] + 10 * num[22] + 1 * num[21];//
 
-       //送られてくるのは整数2桁＋小数3桁ではなく
+    //送られてくるのは整数2桁＋小数3桁ではなく
     //1000倍されたなことに注意
     aOut = 10.0 * num[4]  + 1.0 * num[3]  + 0.1 * num[2] + 0.01 * num[1] + 0.001 * num[0] - 60.0;//
     bOut = 10.0 * num[9]  + 1.0 * num[8]  + 0.1 * num[7] + 0.01 * num[6] + 0.001 * num[5] - 60.0;//
@@ -126,11 +136,40 @@
 }
 
 //動く関数．総まとめ
-void move(){
+void move()
+{
     cal_Out();
     cal_pw();
     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, unsigned 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];
@@ -149,47 +188,57 @@
 
     ///受け取り準備開始
     //pc.printf("serial starts\n");
-    
+
     //シリアル通信をスタートする
     //受け取り配列numの初期化
     num_ini();
-    
-    
-   //while (1)
-    for(int counter=0;counter<100;counter++)
-    {
-        
-        //pc.gets(buf, 31);         // 31文字受け取る
-        for (int i = 0; i < 30; i++)
-        {
-            ch=pc.getc();//一文字ずつ受け取る
-            num[i] = ctoi(ch);//数字を数値に変換
+
+    for(int i=0; i<6; i++)
+        fret[i] = 0.0f;
+
+
+    //while (1)
+    for(int counter=0; counter<100; 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++)
+//        {
+//            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;
+//        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();
-        
+
     }
     //保存用の部分，使わないならこめんとあうと
     FILE* fp;
-     fp = fopen("/local/Output.csv", "w");
-     if(fp==NULL)
-     {
-           //pc.printf("error");
-      }
+    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++)
-    {
+    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);