Masahiro Furukawa / Mbed 2 deprecated new_serial

under const

Dependencies:   mbed mbed-rtos

new_serial.cpp@2:83ea259e8ce5, 2020-08-26 (annotated)

Committer:
mfurukawa
Date:
Wed Aug 26 22:04:49 2020 +0000
Revision:
2:83ea259e8ce5
Parent:
1:4108360238c0
Child:
3:a7df2c55da1a
under const;

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 2:83ea259e8ce5 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 2:83ea259e8ce5 20 char rbuf[4 * 6 + 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 2:83ea259e8ce5 28 #define NUM_OF_LOG_LINES 60
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);
hiramath 0:67dece35504d 185 //各サーボの設定
hiramath 0:67dece35504d 186 servo0.period(PWMperiod);
hiramath 0:67dece35504d 187 servo1.period(PWMperiod);
hiramath 0:67dece35504d 188 servo2.period(PWMperiod);
hiramath 0:67dece35504d 189 servo3.period(PWMperiod);
hiramath 0:67dece35504d 190 servo4.period(PWMperiod);
hiramath 0:67dece35504d 191 servo5.period(PWMperiod);
hiramath 0:67dece35504d 192
hiramath 0:67dece35504d 193 ///受け取り準備開始
hiramath 0:67dece35504d 194 //pc.printf("serial starts\n");
mfurukawa 1:4108360238c0 195
hiramath 0:67dece35504d 196 //シリアル通信をスタートする
hiramath 0:67dece35504d 197 //受け取り配列numの初期化
hiramath 0:67dece35504d 198 num_ini();
mfurukawa 1:4108360238c0 199
mfurukawa 2:83ea259e8ce5 200 float fret;
mfurukawa 2:83ea259e8ce5 201 while(1) {
mfurukawa 2:83ea259e8ce5 202 int counter = 0;
mfurukawa 2:83ea259e8ce5 203 while (counter < NUM_OF_LOG_LINES) {
mfurukawa 2:83ea259e8ce5 204
mfurukawa 2:83ea259e8ce5 205 led1 = 1;
mfurukawa 2:83ea259e8ce5 206 if(pc.readable()) {
mfurukawa 2:83ea259e8ce5 207 // receive 24 bytes + null character
mfurukawa 2:83ea259e8ce5 208 // https://os.mbed.com/questions/77081/Why-does-he-get-only-3-characters-when-I/
mfurukawa 2:83ea259e8ce5 209 pc.gets(rbuf, 4+1);
mfurukawa 2:83ea259e8ce5 210 // ( 4 bytes -> one float ) x 6dof
mfurukawa 2:83ea259e8ce5 211 for(int i=0; i<6; i++) {
mfurukawa 2:83ea259e8ce5 212 byte2float(&fret, &rbuf[i * size_of_float]);
mfurukawa 2:83ea259e8ce5 213 output[counter][i] = fret;
mfurukawa 2:83ea259e8ce5 214 output[counter][5] = 3.14159;
mfurukawa 2:83ea259e8ce5 215 led2 = 1;
mfurukawa 2:83ea259e8ce5 216 }
mfurukawa 2:83ea259e8ce5 217 counter++;
mfurukawa 2:83ea259e8ce5 218 }
mfurukawa 1:4108360238c0 219
mfurukawa 2:83ea259e8ce5 220 //for (int i = 0; i < 30; i++)
mfurukawa 1:4108360238c0 221 // {
mfurukawa 1:4108360238c0 222 // ch=pc.getc();//一文字ずつ受け取る
mfurukawa 1:4108360238c0 223 // num[i] = ctoi(ch);//数字を数値に変換
mfurukawa 1:4108360238c0 224 // }
mfurukawa 1:4108360238c0 225 //
mfurukawa 2:83ea259e8ce5 226 //動かす関数
mfurukawa 2:83ea259e8ce5 227 // move();
mfurukawa 1:4108360238c0 228
mfurukawa 2:83ea259e8ce5 229 //csvファイル書き込みのために配列に保存
mfurukawa 1:4108360238c0 230 // output[counter][0]=aOut;
mfurukawa 1:4108360238c0 231 // output[counter][1]=bOut;
mfurukawa 1:4108360238c0 232 // output[counter][2]=cOut;
mfurukawa 1:4108360238c0 233 // output[counter][3]=dOut;
mfurukawa 1:4108360238c0 234 // output[counter][4]=eOut;
mfurukawa 1:4108360238c0 235 // output[counter][5]=fOut;
mfurukawa 2:83ea259e8ce5 236 //動かしたら初期化する
mfurukawa 2:83ea259e8ce5 237 // num_ini();
mfurukawa 1:4108360238c0 238
mfurukawa 2:83ea259e8ce5 239 }
mfurukawa 2:83ea259e8ce5 240
mfurukawa 2:83ea259e8ce5 241 led3 = 1;
mfurukawa 2:83ea259e8ce5 242 //保存用の部分，使わないならこめんとあうと
mfurukawa 2:83ea259e8ce5 243 FILE* fp;
mfurukawa 2:83ea259e8ce5 244 fp = fopen("/local/Output.csv", "w");
mfurukawa 2:83ea259e8ce5 245 wait(0.1);
mfurukawa 2:83ea259e8ce5 246 if(fp!=NULL) {
mfurukawa 2:83ea259e8ce5 247 fprintf(fp, "aOut,bOut,cOut,dOut,eOut,fOut\n");
mfurukawa 2:83ea259e8ce5 248 for(int j =0; j<NUM_OF_LOG_LINES; j++) {
mfurukawa 2:83ea259e8ce5 249 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]);
mfurukawa 2:83ea259e8ce5 250 }
mfurukawa 2:83ea259e8ce5 251 }
mfurukawa 2:83ea259e8ce5 252 led4 = 1;
mfurukawa 2:83ea259e8ce5 253 wait(0.1);
mfurukawa 2:83ea259e8ce5 254 fclose(fp);
mfurukawa 2:83ea259e8ce5 255
mfurukawa 2:83ea259e8ce5 256 led1 = 0;
mfurukawa 2:83ea259e8ce5 257 led2 = 0;
mfurukawa 2:83ea259e8ce5 258 led3 = 0;
mfurukawa 2:83ea259e8ce5 259 led4 = 0;
hiramath 0:67dece35504d 260 }
hiramath 0:67dece35504d 261
hiramath 0:67dece35504d 262 }