Diff: new_serial.cpp

Revision:

6:81bc6ebb1115

Parent:

5:7f031c7e4694

--- a/new_serial.cpp	Thu Aug 27 14:04:49 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,203 +0,0 @@
-// Created by Tomoki Hirayama
-//
-// Modified by Masahiro Furukawa
-// 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に対応するピン
-PwmOut servo2(p23);//θ2に対応するピン
-PwmOut servo3(p24);//θ3に対応するピン
-PwmOut servo4(p25);//θ4に対応するピン
-PwmOut servo5(p26);//θ5に対応するピン
-
-
-//name.baud(9600);
-//MG996Rのほう
-
-Serial pc(USBTX, USBRX);
-LocalFileSystem local("local");
-#define NUM_OF_LOG_LINES 5
-
-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();
-}
-
-int main()
-{
-    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);
-    servo2.period(PWMperiod);
-    servo3.period(PWMperiod);
-    servo4.period(PWMperiod);
-    servo5.period(PWMperiod);
-
-    num_ini();
-
-    char rbuf[4];
-    
-    while(1) {
-
-        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