File content as of revision 1:4108360238c0:

// Created by Tomoki Hirayama
//
// Modified by Masahiro Furukawa
// Aug 27, 2020

#include "mbed.h"

PwmOut servo0(p21);//θ0に対応するピン
PwmOut servo1(p22);//θ1に対応するピン
PwmOut servo2(p23);//θ2に対応するピン
PwmOut servo3(p24);//θ3に対応するピン
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のほう

Serial pc(USBTX, USBRX);
LocalFileSystem local("local");

float aOut, bOut, cOut, dOut, eOut, fOut;//それぞれの角度
int num[30];//数字格納場所，基本的に1桁の数字しか入らない
int pw0, pw1, pw2, pw3, pw4, pw5;//出力パルス幅



//きちんと値が受け取れているかチェックするための初期化
void num_ini()
{
    for (int i = 0; i < 30; i++) {
        num[i] = 9999;
    }
}
bool num_check(int n[])
{
    for (int i = 0; i < 24; i++) {
        if (n[i] == 9999)
            return -1;
    }
    return 1;
}
//char型で受け取った文字をint型に返す
int ctoi(char c)
{
    //1文字の数字（char型）を数値（int型）に変換
    if ('0' <= c && c <= '9') {
        return (c - '0');
    } else {
        return -1;
    }
}

//角度[°]から入力パルス幅[us]に変換する関数
int cal_input0(float arg)
{
    return 1475 + int(10.48 * arg);
}
int cal_input1(float arg)
{
    //return 1520 + 10.2467 * (-30.0 + arg);
    return 1218 + int(10.2467 * arg);
}
int cal_input2(float arg)
{
    //return 853 + 10.59 * (30.0 + arg);
    return 1306 + 10.59 * arg;
}
int cal_input3(float arg)
{
    return 1224 + int(8.556 * arg);
}
int cal_input4(float arg)
{
    return 1460 + int(10.556 * arg);
}
int cal_input5(float arg)
{
    return 1922 + int(10.556 * arg);
}


//受け取った値を変換
void cal_Out()
{
    //aOut = 1000 * num[3] + 100 * num[2] + 10 * num[1] + 1 * num[0];//
    //bOut = 1000 * num[7] + 100 * num[6] + 10 * num[5] + 1 * num[4];//
    //cOut = 1000 * num[11] + 100 * num[10] + 10 * num[9] + 1 * num[8];//
    //dOut = 1000 * num[15] + 100 * num[14] + 10 * num[13] + 1 * num[12];//
    //eOut = 1000 * num[19] + 100 * num[18] + 10 * num[17] + 1 * num[16];//
    //fOut = 1000 * num[23] + 100 * num[22] + 10 * num[21] + 1 * num[20];//
    //aOut = 1000 * num[4] + 100 * num[3] + 10 * num[2] + 1 * num[1];//
    //bOut = 1000 * num[8] + 100 * num[7] + 10 * num[6] + 1 * num[5];//
    //cOut = 1000 * num[12] + 100 * num[11] + 10 * num[10] + 1 * num[9];//
    //dOut = 1000 * num[16] + 100 * num[15] + 10 * num[14] + 1 * num[13];//
    //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桁ではなく
    //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;//
    cOut = 10.0 * num[14] + 1.0 * num[13] + 0.1 * num[12] + 0.01 * num[11] + 0.001 * num[10] - 60.0;//
    dOut = 10.0 * num[19] + 1.0 * num[18] + 0.1 * num[17] + 0.01 * num[16] + 0.001 * num[15] - 60.0;//
    eOut = 10.0 * num[24] + 1.0 * num[23] + 0.1 * num[22] + 0.01 * num[21] + 0.001 * num[20] - 60.0;//
    fOut = 10.0 * num[29] + 1.0 * num[28] + 0.1 * num[27] + 0.01 * num[26] + 0.001 * num[25] - 60.0;//

    //aOut = 10.0 * num[28] + 1.0 * num[27] + 0.1 * num[26] + 0.01 * num[25] + 0.001 * num[24] - 60.0;//
    //bOut = 10.0 * num[3]  + 1.0 * num[2]  + 0.1 * num[1]  + 0.01 * num[0]  + 0.001 * num[29] - 60.0;//
    //cOut = 10.0 * num[8]  + 1.0 * num[7]  + 0.1 * num[6]  + 0.01 * num[5]  + 0.001 * num[4]  - 60.0;//
    //dOut = 10.0 * num[13] + 1.0 * num[12] + 0.1 * num[11] + 0.01 * num[10] + 0.001 * num[9]  - 60.0;//
    //eOut = 10.0 * num[18] + 1.0 * num[17] + 0.1 * num[16] + 0.01 * num[15] + 0.001 * num[14] - 60.0;//
    //fOut = 10.0 * num[23] + 1.0 * num[22] + 0.1 * num[21] + 0.01 * num[20] + 0.001 * num[19] - 60.0;//

}
void cal_pw()
{
    pw0 = cal_input0(aOut);
    pw1 = cal_input1(bOut);
    pw2 = cal_input2(cOut);
    pw3 = cal_input3(dOut);
    pw4 = cal_input4(eOut);
    pw5 = cal_input5(fOut);
}
void send_servo()
{
    servo0.pulsewidth_us(pw0);//パルス変調幅を1625usで入力
    servo1.pulsewidth_us(pw1);//パルス変調幅を1420usで入力
    servo2.pulsewidth_us(pw2); //パルス変調幅を 632usで入力
    servo3.pulsewidth_us(pw3);//パルス変調幅を1310usで入力
    servo4.pulsewidth_us(pw4);//パルス変調幅を1368usで入力
    servo5.pulsewidth_us(pw5);//パルス変調幅を1500usで入力
}

//動く関数．総まとめ
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];
    float w = 60.0;//========制御周波数===========
    float PWMperiod = 1.0 / w;    //PWM周期の計算
    char ch;
    //===============ボーレート=============
    pc.baud(115200);
    //各サーボの設定
    servo0.period(PWMperiod);
    servo1.period(PWMperiod);
    servo2.period(PWMperiod);
    servo3.period(PWMperiod);
    servo4.period(PWMperiod);
    servo5.period(PWMperiod);

    ///受け取り準備開始
    //pc.printf("serial starts\n");

    //シリアル通信をスタートする
    //受け取り配列numの初期化
    num_ini();

    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;
        //動かしたら初期化する
        num_ini();

    }
    //保存用の部分，使わないならこめんとあうと
    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);

}