Masahiro Furukawa / Mbed 2 deprecated new_serial

under const

Dependencies:   mbed mbed-rtos

new_serial.cpp@5:7f031c7e4694, 2020-08-27 (annotated)

Committer:
mfurukawa
Date:
Thu Aug 27 14:04:49 2020 +0000
Revision:
5:7f031c7e4694
Parent:
4:066d90d485d3 
finished binary communication (PC->mbed 4bytes x 6 dof @ 60fps)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mfurukawa 1:4108360238c0 1 // Created by Tomoki Hirayama
mfurukawa 1:4108360238c0 2 //
mfurukawa 1:4108360238c0 3 // Modified by Masahiro Furukawa
mfurukawa 1:4108360238c0 4 // Aug 27, 2020
mfurukawa 1:4108360238c0 5
hiramath 0:67dece35504d 6 #include "mbed.h"
mfurukawa 2:83ea259e8ce5 7 DigitalOut led1(LED1);
mfurukawa 2:83ea259e8ce5 8 DigitalOut led2(LED2);
mfurukawa 2:83ea259e8ce5 9 DigitalOut led3(LED3);
mfurukawa 2:83ea259e8ce5 10 DigitalOut led4(LED4);
mfurukawa 3:a7df2c55da1a 11
hiramath 0:67dece35504d 12
hiramath 0:67dece35504d 13 PwmOut servo0(p21);//θ0に対応するピン
hiramath 0:67dece35504d 14 PwmOut servo1(p22);//θ1に対応するピン
hiramath 0:67dece35504d 15 PwmOut servo2(p23);//θ2に対応するピン
hiramath 0:67dece35504d 16 PwmOut servo3(p24);//θ3に対応するピン
hiramath 0:67dece35504d 17 PwmOut servo4(p25);//θ4に対応するピン
hiramath 0:67dece35504d 18 PwmOut servo5(p26);//θ5に対応するピン
hiramath 0:67dece35504d 19
mfurukawa 1:4108360238c0 20
hiramath 0:67dece35504d 21 //name.baud(9600);
hiramath 0:67dece35504d 22 //MG996Rのほう
hiramath 0:67dece35504d 23
hiramath 0:67dece35504d 24 Serial pc(USBTX, USBRX);
hiramath 0:67dece35504d 25 LocalFileSystem local("local");
mfurukawa 3:a7df2c55da1a 26 #define NUM_OF_LOG_LINES 5
hiramath 0:67dece35504d 27
hiramath 0:67dece35504d 28 float aOut, bOut, cOut, dOut, eOut, fOut;//それぞれの角度
hiramath 0:67dece35504d 29 int num[30];//数字格納場所，基本的に1桁の数字しか入らない
hiramath 0:67dece35504d 30 int pw0, pw1, pw2, pw3, pw4, pw5;//出力パルス幅
hiramath 0:67dece35504d 31
hiramath 0:67dece35504d 32
hiramath 0:67dece35504d 33
hiramath 0:67dece35504d 34 //きちんと値が受け取れているかチェックするための初期化
hiramath 0:67dece35504d 35 void num_ini()
hiramath 0:67dece35504d 36 {
mfurukawa 1:4108360238c0 37 for (int i = 0; i < 30; i++) {
hiramath 0:67dece35504d 38 num[i] = 9999;
hiramath 0:67dece35504d 39 }
hiramath 0:67dece35504d 40 }
hiramath 0:67dece35504d 41 bool num_check(int n[])
hiramath 0:67dece35504d 42 {
mfurukawa 1:4108360238c0 43 for (int i = 0; i < 24; i++) {
hiramath 0:67dece35504d 44 if (n[i] == 9999)
hiramath 0:67dece35504d 45 return -1;
hiramath 0:67dece35504d 46 }
hiramath 0:67dece35504d 47 return 1;
hiramath 0:67dece35504d 48 }
hiramath 0:67dece35504d 49 //char型で受け取った文字をint型に返す
hiramath 0:67dece35504d 50 int ctoi(char c)
hiramath 0:67dece35504d 51 {
hiramath 0:67dece35504d 52 //1文字の数字（char型）を数値（int型）に変換
mfurukawa 1:4108360238c0 53 if ('0' <= c && c <= '9') {
hiramath 0:67dece35504d 54 return (c - '0');
mfurukawa 1:4108360238c0 55 } else {
hiramath 0:67dece35504d 56 return -1;
hiramath 0:67dece35504d 57 }
hiramath 0:67dece35504d 58 }
hiramath 0:67dece35504d 59
hiramath 0:67dece35504d 60 //角度[°]から入力パルス幅[us]に変換する関数
mfurukawa 1:4108360238c0 61 int cal_input0(float arg)
mfurukawa 1:4108360238c0 62 {
hiramath 0:67dece35504d 63 return 1475 + int(10.48 * arg);
hiramath 0:67dece35504d 64 }
mfurukawa 1:4108360238c0 65 int cal_input1(float arg)
mfurukawa 1:4108360238c0 66 {
hiramath 0:67dece35504d 67 //return 1520 + 10.2467 * (-30.0 + arg);
hiramath 0:67dece35504d 68 return 1218 + int(10.2467 * arg);
hiramath 0:67dece35504d 69 }
mfurukawa 1:4108360238c0 70 int cal_input2(float arg)
mfurukawa 1:4108360238c0 71 {
hiramath 0:67dece35504d 72 //return 853 + 10.59 * (30.0 + arg);
hiramath 0:67dece35504d 73 return 1306 + 10.59 * arg;
hiramath 0:67dece35504d 74 }
mfurukawa 1:4108360238c0 75 int cal_input3(float arg)
mfurukawa 1:4108360238c0 76 {
hiramath 0:67dece35504d 77 return 1224 + int(8.556 * arg);
hiramath 0:67dece35504d 78 }
mfurukawa 1:4108360238c0 79 int cal_input4(float arg)
mfurukawa 1:4108360238c0 80 {
hiramath 0:67dece35504d 81 return 1460 + int(10.556 * arg);
hiramath 0:67dece35504d 82 }
mfurukawa 1:4108360238c0 83 int cal_input5(float arg)
mfurukawa 1:4108360238c0 84 {
hiramath 0:67dece35504d 85 return 1922 + int(10.556 * arg);
hiramath 0:67dece35504d 86 }
hiramath 0:67dece35504d 87
hiramath 0:67dece35504d 88
hiramath 0:67dece35504d 89 //受け取った値を変換
hiramath 0:67dece35504d 90 void cal_Out()
hiramath 0:67dece35504d 91 {
hiramath 0:67dece35504d 92 //aOut = 1000 * num[3] + 100 * num[2] + 10 * num[1] + 1 * num[0];//
hiramath 0:67dece35504d 93 //bOut = 1000 * num[7] + 100 * num[6] + 10 * num[5] + 1 * num[4];//
hiramath 0:67dece35504d 94 //cOut = 1000 * num[11] + 100 * num[10] + 10 * num[9] + 1 * num[8];//
hiramath 0:67dece35504d 95 //dOut = 1000 * num[15] + 100 * num[14] + 10 * num[13] + 1 * num[12];//
hiramath 0:67dece35504d 96 //eOut = 1000 * num[19] + 100 * num[18] + 10 * num[17] + 1 * num[16];//
hiramath 0:67dece35504d 97 //fOut = 1000 * num[23] + 100 * num[22] + 10 * num[21] + 1 * num[20];//
hiramath 0:67dece35504d 98 //aOut = 1000 * num[4] + 100 * num[3] + 10 * num[2] + 1 * num[1];//
hiramath 0:67dece35504d 99 //bOut = 1000 * num[8] + 100 * num[7] + 10 * num[6] + 1 * num[5];//
hiramath 0:67dece35504d 100 //cOut = 1000 * num[12] + 100 * num[11] + 10 * num[10] + 1 * num[9];//
hiramath 0:67dece35504d 101 //dOut = 1000 * num[16] + 100 * num[15] + 10 * num[14] + 1 * num[13];//
hiramath 0:67dece35504d 102 //eOut = 1000 * num[20] + 100 * num[19] + 10 * num[18] + 1 * num[17];//
hiramath 0:67dece35504d 103 //fOut = 1000 * num[24] + 100 * num[23] + 10 * num[22] + 1 * num[21];//
hiramath 0:67dece35504d 104
mfurukawa 1:4108360238c0 105 //送られてくるのは整数2桁＋小数3桁ではなく
hiramath 0:67dece35504d 106 //1000倍されたなことに注意
hiramath 0:67dece35504d 107 aOut = 10.0 * num[4] + 1.0 * num[3] + 0.1 * num[2] + 0.01 * num[1] + 0.001 * num[0] - 60.0;//
hiramath 0:67dece35504d 108 bOut = 10.0 * num[9] + 1.0 * num[8] + 0.1 * num[7] + 0.01 * num[6] + 0.001 * num[5] - 60.0;//
hiramath 0:67dece35504d 109 cOut = 10.0 * num[14] + 1.0 * num[13] + 0.1 * num[12] + 0.01 * num[11] + 0.001 * num[10] - 60.0;//
hiramath 0:67dece35504d 110 dOut = 10.0 * num[19] + 1.0 * num[18] + 0.1 * num[17] + 0.01 * num[16] + 0.001 * num[15] - 60.0;//
hiramath 0:67dece35504d 111 eOut = 10.0 * num[24] + 1.0 * num[23] + 0.1 * num[22] + 0.01 * num[21] + 0.001 * num[20] - 60.0;//
hiramath 0:67dece35504d 112 fOut = 10.0 * num[29] + 1.0 * num[28] + 0.1 * num[27] + 0.01 * num[26] + 0.001 * num[25] - 60.0;//
hiramath 0:67dece35504d 113
hiramath 0:67dece35504d 114 //aOut = 10.0 * num[28] + 1.0 * num[27] + 0.1 * num[26] + 0.01 * num[25] + 0.001 * num[24] - 60.0;//
hiramath 0:67dece35504d 115 //bOut = 10.0 * num[3] + 1.0 * num[2] + 0.1 * num[1] + 0.01 * num[0] + 0.001 * num[29] - 60.0;//
hiramath 0:67dece35504d 116 //cOut = 10.0 * num[8] + 1.0 * num[7] + 0.1 * num[6] + 0.01 * num[5] + 0.001 * num[4] - 60.0;//
hiramath 0:67dece35504d 117 //dOut = 10.0 * num[13] + 1.0 * num[12] + 0.1 * num[11] + 0.01 * num[10] + 0.001 * num[9] - 60.0;//
hiramath 0:67dece35504d 118 //eOut = 10.0 * num[18] + 1.0 * num[17] + 0.1 * num[16] + 0.01 * num[15] + 0.001 * num[14] - 60.0;//
hiramath 0:67dece35504d 119 //fOut = 10.0 * num[23] + 1.0 * num[22] + 0.1 * num[21] + 0.01 * num[20] + 0.001 * num[19] - 60.0;//
hiramath 0:67dece35504d 120
hiramath 0:67dece35504d 121 }
hiramath 0:67dece35504d 122 void cal_pw()
hiramath 0:67dece35504d 123 {
hiramath 0:67dece35504d 124 pw0 = cal_input0(aOut);
hiramath 0:67dece35504d 125 pw1 = cal_input1(bOut);
hiramath 0:67dece35504d 126 pw2 = cal_input2(cOut);
hiramath 0:67dece35504d 127 pw3 = cal_input3(dOut);
hiramath 0:67dece35504d 128 pw4 = cal_input4(eOut);
hiramath 0:67dece35504d 129 pw5 = cal_input5(fOut);
hiramath 0:67dece35504d 130 }
hiramath 0:67dece35504d 131 void send_servo()
hiramath 0:67dece35504d 132 {
hiramath 0:67dece35504d 133 servo0.pulsewidth_us(pw0);//パルス変調幅を1625usで入力
hiramath 0:67dece35504d 134 servo1.pulsewidth_us(pw1);//パルス変調幅を1420usで入力
hiramath 0:67dece35504d 135 servo2.pulsewidth_us(pw2); //パルス変調幅を 632usで入力
hiramath 0:67dece35504d 136 servo3.pulsewidth_us(pw3);//パルス変調幅を1310usで入力
hiramath 0:67dece35504d 137 servo4.pulsewidth_us(pw4);//パルス変調幅を1368usで入力
hiramath 0:67dece35504d 138 servo5.pulsewidth_us(pw5);//パルス変調幅を1500usで入力
hiramath 0:67dece35504d 139 }
hiramath 0:67dece35504d 140
hiramath 0:67dece35504d 141 //動く関数．総まとめ
mfurukawa 1:4108360238c0 142 void move()
mfurukawa 1:4108360238c0 143 {
hiramath 0:67dece35504d 144 cal_Out();
hiramath 0:67dece35504d 145 cal_pw();
hiramath 0:67dece35504d 146 send_servo();
hiramath 0:67dece35504d 147 }
mfurukawa 1:4108360238c0 148
hiramath 0:67dece35504d 149 int main()
hiramath 0:67dece35504d 150 {
hiramath 0:67dece35504d 151 float w = 60.0;//========制御周波数===========
hiramath 0:67dece35504d 152 float PWMperiod = 1.0 / w; //PWM周期の計算
hiramath 0:67dece35504d 153 //===============ボーレート=============
mfurukawa 2:83ea259e8ce5 154 pc.baud(921600);
mfurukawa 3:a7df2c55da1a 155 pc.format(8, Serial::None, 1);
mfurukawa 5:7f031c7e4694 156
hiramath 0:67dece35504d 157 //各サーボの設定
hiramath 0:67dece35504d 158 servo0.period(PWMperiod);
hiramath 0:67dece35504d 159 servo1.period(PWMperiod);
hiramath 0:67dece35504d 160 servo2.period(PWMperiod);
hiramath 0:67dece35504d 161 servo3.period(PWMperiod);
hiramath 0:67dece35504d 162 servo4.period(PWMperiod);
hiramath 0:67dece35504d 163 servo5.period(PWMperiod);
hiramath 0:67dece35504d 164
hiramath 0:67dece35504d 165 num_ini();
mfurukawa 1:4108360238c0 166
mfurukawa 5:7f031c7e4694 167 char rbuf[4];
mfurukawa 5:7f031c7e4694 168
mfurukawa 2:83ea259e8ce5 169 while(1) {
mfurukawa 3:a7df2c55da1a 170
mfurukawa 3:a7df2c55da1a 171 led1 = 0;
mfurukawa 3:a7df2c55da1a 172 led2 = 0;
mfurukawa 3:a7df2c55da1a 173 led3 = 0;
mfurukawa 3:a7df2c55da1a 174 led4 = 0;
mfurukawa 5:7f031c7e4694 175 if(pc.readable()) {
mfurukawa 5:7f031c7e4694 176 if (pc.getc() != '*') continue;
mfurukawa 5:7f031c7e4694 177 if (pc.getc() != '+') continue;
mfurukawa 5:7f031c7e4694 178 for(int i=0; i<6; i++) {
mfurukawa 5:7f031c7e4694 179 for(int b=0; b<4; b++)
mfurukawa 5:7f031c7e4694 180 rbuf[b] = pc.getc();
hiramath 0:67dece35504d 181
mfurukawa 5:7f031c7e4694 182 // ( 4 bytes -> one float )
mfurukawa 5:7f031c7e4694 183 float const* f = reinterpret_cast<float const*>(rbuf);
mfurukawa 5:7f031c7e4694 184
mfurukawa 5:7f031c7e4694 185 if(*f == 5.3f)
mfurukawa 5:7f031c7e4694 186 led1 = 1;
mfurukawa 5:7f031c7e4694 187 if(*f == 5.2f)
mfurukawa 5:7f031c7e4694 188 led2 = 1;
mfurukawa 5:7f031c7e4694 189 if(*f == -0.3f)
mfurukawa 5:7f031c7e4694 190 led3 = 1;
mfurukawa 5:7f031c7e4694 191 if(*f == 40.0f)
mfurukawa 5:7f031c7e4694 192 led4 = 1;
mfurukawa 5:7f031c7e4694 193 }
mfurukawa 5:7f031c7e4694 194 }
mfurukawa 5:7f031c7e4694 195
mfurukawa 5:7f031c7e4694 196 //
mfurukawa 5:7f031c7e4694 197 //動かす関数
mfurukawa 5:7f031c7e4694 198 // move();
mfurukawa 5:7f031c7e4694 199 //動かしたら初期化する
mfurukawa 5:7f031c7e4694 200 // num_ini();
mfurukawa 5:7f031c7e4694 201
mfurukawa 5:7f031c7e4694 202 }
hiramath 0:67dece35504d 203 }