maedalab / Mbed 2 deprecated binary_recieve

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos

new_serial.cpp@4:066d90d485d3, 2020-08-27 (annotated)

Committer:
mfurukawa
Date:
Thu Aug 27 05:18:24 2020 +0000
Revision:
4:066d90d485d3
Parent:
3:a7df2c55da1a
Child:
5:7f031c7e4694
final;

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 3:a7df2c55da1a 20 char rbuf[4 + 1];
mfurukawa 1:4108360238c0 21 const int size_of_float = 4;
mfurukawa 1:4108360238c0 22
hiramath 0:67dece35504d 23 //name.baud(9600);
hiramath 0:67dece35504d 24 //MG996Rのほう
hiramath 0:67dece35504d 25
hiramath 0:67dece35504d 26 Serial pc(USBTX, USBRX);
hiramath 0:67dece35504d 27 LocalFileSystem local("local");
mfurukawa 3:a7df2c55da1a 28 #define NUM_OF_LOG_LINES 5
hiramath 0:67dece35504d 29
hiramath 0:67dece35504d 30 float aOut, bOut, cOut, dOut, eOut, fOut;//それぞれの角度
hiramath 0:67dece35504d 31 int num[30];//数字格納場所，基本的に1桁の数字しか入らない
hiramath 0:67dece35504d 32 int pw0, pw1, pw2, pw3, pw4, pw5;//出力パルス幅
hiramath 0:67dece35504d 33
hiramath 0:67dece35504d 34
hiramath 0:67dece35504d 35
hiramath 0:67dece35504d 36 //きちんと値が受け取れているかチェックするための初期化
hiramath 0:67dece35504d 37 void num_ini()
hiramath 0:67dece35504d 38 {
mfurukawa 1:4108360238c0 39 for (int i = 0; i < 30; i++) {
hiramath 0:67dece35504d 40 num[i] = 9999;
hiramath 0:67dece35504d 41 }
hiramath 0:67dece35504d 42 }
hiramath 0:67dece35504d 43 bool num_check(int n[])
hiramath 0:67dece35504d 44 {
mfurukawa 1:4108360238c0 45 for (int i = 0; i < 24; i++) {
hiramath 0:67dece35504d 46 if (n[i] == 9999)
hiramath 0:67dece35504d 47 return -1;
hiramath 0:67dece35504d 48 }
hiramath 0:67dece35504d 49 return 1;
hiramath 0:67dece35504d 50 }
hiramath 0:67dece35504d 51 //char型で受け取った文字をint型に返す
hiramath 0:67dece35504d 52 int ctoi(char c)
hiramath 0:67dece35504d 53 {
hiramath 0:67dece35504d 54 //1文字の数字（char型）を数値（int型）に変換
mfurukawa 1:4108360238c0 55 if ('0' <= c && c <= '9') {
hiramath 0:67dece35504d 56 return (c - '0');
mfurukawa 1:4108360238c0 57 } else {
hiramath 0:67dece35504d 58 return -1;
hiramath 0:67dece35504d 59 }
hiramath 0:67dece35504d 60 }
hiramath 0:67dece35504d 61
hiramath 0:67dece35504d 62 //角度[°]から入力パルス幅[us]に変換する関数
mfurukawa 1:4108360238c0 63 int cal_input0(float arg)
mfurukawa 1:4108360238c0 64 {
hiramath 0:67dece35504d 65 return 1475 + int(10.48 * arg);
hiramath 0:67dece35504d 66 }
mfurukawa 1:4108360238c0 67 int cal_input1(float arg)
mfurukawa 1:4108360238c0 68 {
hiramath 0:67dece35504d 69 //return 1520 + 10.2467 * (-30.0 + arg);
hiramath 0:67dece35504d 70 return 1218 + int(10.2467 * arg);
hiramath 0:67dece35504d 71 }
mfurukawa 1:4108360238c0 72 int cal_input2(float arg)
mfurukawa 1:4108360238c0 73 {
hiramath 0:67dece35504d 74 //return 853 + 10.59 * (30.0 + arg);
hiramath 0:67dece35504d 75 return 1306 + 10.59 * arg;
hiramath 0:67dece35504d 76 }
mfurukawa 1:4108360238c0 77 int cal_input3(float arg)
mfurukawa 1:4108360238c0 78 {
hiramath 0:67dece35504d 79 return 1224 + int(8.556 * arg);
hiramath 0:67dece35504d 80 }
mfurukawa 1:4108360238c0 81 int cal_input4(float arg)
mfurukawa 1:4108360238c0 82 {
hiramath 0:67dece35504d 83 return 1460 + int(10.556 * arg);
hiramath 0:67dece35504d 84 }
mfurukawa 1:4108360238c0 85 int cal_input5(float arg)
mfurukawa 1:4108360238c0 86 {
hiramath 0:67dece35504d 87 return 1922 + int(10.556 * arg);
hiramath 0:67dece35504d 88 }
hiramath 0:67dece35504d 89
hiramath 0:67dece35504d 90
hiramath 0:67dece35504d 91 //受け取った値を変換
hiramath 0:67dece35504d 92 void cal_Out()
hiramath 0:67dece35504d 93 {
hiramath 0:67dece35504d 94 //aOut = 1000 * num[3] + 100 * num[2] + 10 * num[1] + 1 * num[0];//
hiramath 0:67dece35504d 95 //bOut = 1000 * num[7] + 100 * num[6] + 10 * num[5] + 1 * num[4];//
hiramath 0:67dece35504d 96 //cOut = 1000 * num[11] + 100 * num[10] + 10 * num[9] + 1 * num[8];//
hiramath 0:67dece35504d 97 //dOut = 1000 * num[15] + 100 * num[14] + 10 * num[13] + 1 * num[12];//
hiramath 0:67dece35504d 98 //eOut = 1000 * num[19] + 100 * num[18] + 10 * num[17] + 1 * num[16];//
hiramath 0:67dece35504d 99 //fOut = 1000 * num[23] + 100 * num[22] + 10 * num[21] + 1 * num[20];//
hiramath 0:67dece35504d 100 //aOut = 1000 * num[4] + 100 * num[3] + 10 * num[2] + 1 * num[1];//
hiramath 0:67dece35504d 101 //bOut = 1000 * num[8] + 100 * num[7] + 10 * num[6] + 1 * num[5];//
hiramath 0:67dece35504d 102 //cOut = 1000 * num[12] + 100 * num[11] + 10 * num[10] + 1 * num[9];//
hiramath 0:67dece35504d 103 //dOut = 1000 * num[16] + 100 * num[15] + 10 * num[14] + 1 * num[13];//
hiramath 0:67dece35504d 104 //eOut = 1000 * num[20] + 100 * num[19] + 10 * num[18] + 1 * num[17];//
hiramath 0:67dece35504d 105 //fOut = 1000 * num[24] + 100 * num[23] + 10 * num[22] + 1 * num[21];//
hiramath 0:67dece35504d 106
mfurukawa 1:4108360238c0 107 //送られてくるのは整数2桁＋小数3桁ではなく
hiramath 0:67dece35504d 108 //1000倍されたなことに注意
hiramath 0:67dece35504d 109 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 110 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 111 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 112 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 113 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 114 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 115
hiramath 0:67dece35504d 116 //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 117 //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 118 //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 119 //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 120 //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 121 //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 122
hiramath 0:67dece35504d 123 }
hiramath 0:67dece35504d 124 void cal_pw()
hiramath 0:67dece35504d 125 {
hiramath 0:67dece35504d 126 pw0 = cal_input0(aOut);
hiramath 0:67dece35504d 127 pw1 = cal_input1(bOut);
hiramath 0:67dece35504d 128 pw2 = cal_input2(cOut);
hiramath 0:67dece35504d 129 pw3 = cal_input3(dOut);
hiramath 0:67dece35504d 130 pw4 = cal_input4(eOut);
hiramath 0:67dece35504d 131 pw5 = cal_input5(fOut);
hiramath 0:67dece35504d 132 }
hiramath 0:67dece35504d 133 void send_servo()
hiramath 0:67dece35504d 134 {
hiramath 0:67dece35504d 135 servo0.pulsewidth_us(pw0);//パルス変調幅を1625usで入力
hiramath 0:67dece35504d 136 servo1.pulsewidth_us(pw1);//パルス変調幅を1420usで入力
hiramath 0:67dece35504d 137 servo2.pulsewidth_us(pw2); //パルス変調幅を 632usで入力
hiramath 0:67dece35504d 138 servo3.pulsewidth_us(pw3);//パルス変調幅を1310usで入力
hiramath 0:67dece35504d 139 servo4.pulsewidth_us(pw4);//パルス変調幅を1368usで入力
hiramath 0:67dece35504d 140 servo5.pulsewidth_us(pw5);//パルス変調幅を1500usで入力
hiramath 0:67dece35504d 141 }
hiramath 0:67dece35504d 142
hiramath 0:67dece35504d 143 //動く関数．総まとめ
mfurukawa 1:4108360238c0 144 void move()
mfurukawa 1:4108360238c0 145 {
hiramath 0:67dece35504d 146 cal_Out();
hiramath 0:67dece35504d 147 cal_pw();
hiramath 0:67dece35504d 148 send_servo();
hiramath 0:67dece35504d 149 }
mfurukawa 1:4108360238c0 150
mfurukawa 1:4108360238c0 151 void float2byte(unsigned char ret[], float f)
mfurukawa 1:4108360238c0 152 {
mfurukawa 1:4108360238c0 153 //
mfurukawa 1:4108360238c0 154 // reference : https://stackoverflow.com/questions/14018894/how-to-convert-float-to-byte-array-of-length-4-array-of-char
mfurukawa 1:4108360238c0 155
mfurukawa 1:4108360238c0 156 unsigned char const* p = reinterpret_cast<unsigned char const*>(&f);
mfurukawa 1:4108360238c0 157
mfurukawa 1:4108360238c0 158 memcpy(ret, p, size_of_float);
mfurukawa 1:4108360238c0 159
mfurukawa 1:4108360238c0 160 //for (std::size_t i = 0; i != size_of_float; ++i)
mfurukawa 1:4108360238c0 161 //{
mfurukawa 1:4108360238c0 162 // std::printf("The byte #%zu is 0x%02X\n", i, ret[i]);
mfurukawa 1:4108360238c0 163 //}
mfurukawa 1:4108360238c0 164 }
mfurukawa 2:83ea259e8ce5 165 void byte2float(float *f, char ret[])
mfurukawa 1:4108360238c0 166 {
mfurukawa 1:4108360238c0 167 //
mfurukawa 1:4108360238c0 168 // reference : https://stackoverflow.com/questions/14018894/how-to-convert-float-to-byte-array-of-length-4-array-of-char
mfurukawa 1:4108360238c0 169
mfurukawa 1:4108360238c0 170 char fbuf[4];
mfurukawa 1:4108360238c0 171 memcpy(fbuf, ret, 4);
mfurukawa 1:4108360238c0 172
mfurukawa 1:4108360238c0 173 float const* p = reinterpret_cast<float const*>(ret);
mfurukawa 1:4108360238c0 174 memcpy(f, p, size_of_float);
mfurukawa 1:4108360238c0 175
mfurukawa 1:4108360238c0 176 //std::printf("The value is %3.3lf\n", *f);
mfurukawa 1:4108360238c0 177 }
hiramath 0:67dece35504d 178 int main()
hiramath 0:67dece35504d 179 {
hiramath 0:67dece35504d 180 float output[500][6];
hiramath 0:67dece35504d 181 float w = 60.0;//========制御周波数===========
hiramath 0:67dece35504d 182 float PWMperiod = 1.0 / w; //PWM周期の計算
hiramath 0:67dece35504d 183 //===============ボーレート=============
mfurukawa 2:83ea259e8ce5 184 pc.baud(921600);
mfurukawa 3:a7df2c55da1a 185 pc.format(8, Serial::None, 1);
hiramath 0:67dece35504d 186 //各サーボの設定
hiramath 0:67dece35504d 187 servo0.period(PWMperiod);
hiramath 0:67dece35504d 188 servo1.period(PWMperiod);
hiramath 0:67dece35504d 189 servo2.period(PWMperiod);
hiramath 0:67dece35504d 190 servo3.period(PWMperiod);
hiramath 0:67dece35504d 191 servo4.period(PWMperiod);
hiramath 0:67dece35504d 192 servo5.period(PWMperiod);
hiramath 0:67dece35504d 193
hiramath 0:67dece35504d 194 ///受け取り準備開始
hiramath 0:67dece35504d 195 //pc.printf("serial starts\n");
mfurukawa 1:4108360238c0 196
hiramath 0:67dece35504d 197 //シリアル通信をスタートする
hiramath 0:67dece35504d 198 //受け取り配列numの初期化
hiramath 0:67dece35504d 199 num_ini();
mfurukawa 1:4108360238c0 200
mfurukawa 2:83ea259e8ce5 201 float fret;
mfurukawa 3:a7df2c55da1a 202 char c;
mfurukawa 3:a7df2c55da1a 203
mfurukawa 2:83ea259e8ce5 204 while(1) {
mfurukawa 2:83ea259e8ce5 205 int counter = 0;
mfurukawa 2:83ea259e8ce5 206 while (counter < NUM_OF_LOG_LINES) {
mfurukawa 3:a7df2c55da1a 207
mfurukawa 2:83ea259e8ce5 208 led1 = 1;
mfurukawa 2:83ea259e8ce5 209 if(pc.readable()) {
mfurukawa 3:a7df2c55da1a 210 c = pc.getc();
mfurukawa 4:066d90d485d3 211 if (c != '1')
mfurukawa 4:066d90d485d3 212 continue;
mfurukawa 4:066d90d485d3 213 c = pc.getc();
mfurukawa 4:066d90d485d3 214 if (c != '2')
mfurukawa 3:a7df2c55da1a 215 continue;
mfurukawa 4:066d90d485d3 216 c = pc.getc();
mfurukawa 4:066d90d485d3 217 if (c != '3')
mfurukawa 4:066d90d485d3 218 continue;
mfurukawa 4:066d90d485d3 219
mfurukawa 2:83ea259e8ce5 220 for(int i=0; i<6; i++) {
mfurukawa 4:066d90d485d3 221 led1 = 0;
mfurukawa 3:a7df2c55da1a 222 for(int b=0; b<4; b++)
mfurukawa 3:a7df2c55da1a 223 rbuf[b] = pc.getc();
mfurukawa 3:a7df2c55da1a 224
mfurukawa 4:066d90d485d3 225 led1 = 1;
mfurukawa 3:a7df2c55da1a 226 byte2float(&fret, rbuf);
mfurukawa 4:066d90d485d3 227
mfurukawa 4:066d90d485d3 228 // ( 4 bytes -> one float )
mfurukawa 4:066d90d485d3 229 output[counter][i] = fret;
mfurukawa 4:066d90d485d3 230 }
mfurukawa 3:a7df2c55da1a 231
mfurukawa 4:066d90d485d3 232 led2 = 1;
mfurukawa 4:066d90d485d3 233 //c = pc.getc();
mfurukawa 4:066d90d485d3 234 //if (c != NULL) continue;
mfurukawa 4:066d90d485d3 235
mfurukawa 4:066d90d485d3 236 output[counter][5] = 3.14159;
mfurukawa 4:066d90d485d3 237 led3 = 1;
mfurukawa 2:83ea259e8ce5 238 counter++;
mfurukawa 2:83ea259e8ce5 239 }
mfurukawa 1:4108360238c0 240
mfurukawa 2:83ea259e8ce5 241 //for (int i = 0; i < 30; i++)
mfurukawa 1:4108360238c0 242 // {
mfurukawa 1:4108360238c0 243 // ch=pc.getc();//一文字ずつ受け取る
mfurukawa 1:4108360238c0 244 // num[i] = ctoi(ch);//数字を数値に変換
mfurukawa 1:4108360238c0 245 // }
mfurukawa 1:4108360238c0 246 //
mfurukawa 2:83ea259e8ce5 247 //動かす関数
mfurukawa 2:83ea259e8ce5 248 // move();
mfurukawa 1:4108360238c0 249
mfurukawa 2:83ea259e8ce5 250 //csvファイル書き込みのために配列に保存
mfurukawa 1:4108360238c0 251 // output[counter][0]=aOut;
mfurukawa 1:4108360238c0 252 // output[counter][1]=bOut;
mfurukawa 1:4108360238c0 253 // output[counter][2]=cOut;
mfurukawa 1:4108360238c0 254 // output[counter][3]=dOut;
mfurukawa 1:4108360238c0 255 // output[counter][4]=eOut;
mfurukawa 1:4108360238c0 256 // output[counter][5]=fOut;
mfurukawa 2:83ea259e8ce5 257 //動かしたら初期化する
mfurukawa 2:83ea259e8ce5 258 // num_ini();
mfurukawa 1:4108360238c0 259
mfurukawa 2:83ea259e8ce5 260 }
mfurukawa 3:a7df2c55da1a 261
mfurukawa 3:a7df2c55da1a 262 led3 = 1;
mfurukawa 2:83ea259e8ce5 263 //保存用の部分，使わないならこめんとあうと
mfurukawa 2:83ea259e8ce5 264 FILE* fp;
mfurukawa 2:83ea259e8ce5 265 fp = fopen("/local/Output.csv", "w");
mfurukawa 2:83ea259e8ce5 266 wait(0.1);
mfurukawa 2:83ea259e8ce5 267 if(fp!=NULL) {
mfurukawa 2:83ea259e8ce5 268 fprintf(fp, "aOut,bOut,cOut,dOut,eOut,fOut\n");
mfurukawa 2:83ea259e8ce5 269 for(int j =0; j<NUM_OF_LOG_LINES; j++) {
mfurukawa 4:066d90d485d3 270 fprintf(fp, "%f,%f,%f,%f,%f,%f\n",
mfurukawa 4:066d90d485d3 271 output[j][0],
mfurukawa 4:066d90d485d3 272 output[j][1],
mfurukawa 4:066d90d485d3 273 output[j][2],
mfurukawa 4:066d90d485d3 274 output[j][3],
mfurukawa 4:066d90d485d3 275 output[j][4],
mfurukawa 4:066d90d485d3 276 output[j][5]);
mfurukawa 2:83ea259e8ce5 277 }
mfurukawa 2:83ea259e8ce5 278 }
mfurukawa 3:a7df2c55da1a 279 led4 = 1;
mfurukawa 2:83ea259e8ce5 280 wait(0.1);
mfurukawa 2:83ea259e8ce5 281 fclose(fp);
mfurukawa 3:a7df2c55da1a 282
mfurukawa 3:a7df2c55da1a 283 led1 = 0;
mfurukawa 3:a7df2c55da1a 284 led2 = 0;
mfurukawa 3:a7df2c55da1a 285 led3 = 0;
mfurukawa 3:a7df2c55da1a 286 led4 = 0;
hiramath 0:67dece35504d 287 }
hiramath 0:67dece35504d 288
hiramath 0:67dece35504d 289 }