Masahiro Furukawa / 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@0:67dece35504d, 2020-08-26 (annotated)

Committer:
hiramath
Date:
Wed Aug 26 09:00:08 2020 +0000
Revision:
0:67dece35504d
Child:
1:4108360238c0
Serial reciever

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hiramath 0:67dece35504d 1 #include "mbed.h"
hiramath 0:67dece35504d 2
hiramath 0:67dece35504d 3 PwmOut servo0(p21);//θ0に対応するピン
hiramath 0:67dece35504d 4 PwmOut servo1(p22);//θ1に対応するピン
hiramath 0:67dece35504d 5 PwmOut servo2(p23);//θ2に対応するピン
hiramath 0:67dece35504d 6 PwmOut servo3(p24);//θ3に対応するピン
hiramath 0:67dece35504d 7 PwmOut servo4(p25);//θ4に対応するピン
hiramath 0:67dece35504d 8 PwmOut servo5(p26);//θ5に対応するピン
hiramath 0:67dece35504d 9
hiramath 0:67dece35504d 10 //name.baud(9600);
hiramath 0:67dece35504d 11 //MG996Rのほう
hiramath 0:67dece35504d 12
hiramath 0:67dece35504d 13 Serial pc(USBTX, USBRX);
hiramath 0:67dece35504d 14 LocalFileSystem local("local");
hiramath 0:67dece35504d 15
hiramath 0:67dece35504d 16 float aOut, bOut, cOut, dOut, eOut, fOut;//それぞれの角度
hiramath 0:67dece35504d 17 int num[30];//数字格納場所，基本的に1桁の数字しか入らない
hiramath 0:67dece35504d 18 int pw0, pw1, pw2, pw3, pw4, pw5;//出力パルス幅
hiramath 0:67dece35504d 19
hiramath 0:67dece35504d 20
hiramath 0:67dece35504d 21
hiramath 0:67dece35504d 22 //きちんと値が受け取れているかチェックするための初期化
hiramath 0:67dece35504d 23 void num_ini()
hiramath 0:67dece35504d 24 {
hiramath 0:67dece35504d 25 for (int i = 0; i < 30; i++)
hiramath 0:67dece35504d 26 {
hiramath 0:67dece35504d 27 num[i] = 9999;
hiramath 0:67dece35504d 28 }
hiramath 0:67dece35504d 29 }
hiramath 0:67dece35504d 30 bool num_check(int n[])
hiramath 0:67dece35504d 31 {
hiramath 0:67dece35504d 32 for (int i = 0; i < 24; i++)
hiramath 0:67dece35504d 33 {
hiramath 0:67dece35504d 34 if (n[i] == 9999)
hiramath 0:67dece35504d 35 return -1;
hiramath 0:67dece35504d 36 }
hiramath 0:67dece35504d 37 return 1;
hiramath 0:67dece35504d 38 }
hiramath 0:67dece35504d 39 //char型で受け取った文字をint型に返す
hiramath 0:67dece35504d 40 int ctoi(char c)
hiramath 0:67dece35504d 41 {
hiramath 0:67dece35504d 42 //1文字の数字（char型）を数値（int型）に変換
hiramath 0:67dece35504d 43 if ('0' <= c && c <= '9')
hiramath 0:67dece35504d 44 {
hiramath 0:67dece35504d 45 return (c - '0');
hiramath 0:67dece35504d 46 }
hiramath 0:67dece35504d 47 else
hiramath 0:67dece35504d 48 {
hiramath 0:67dece35504d 49 return -1;
hiramath 0:67dece35504d 50 }
hiramath 0:67dece35504d 51 }
hiramath 0:67dece35504d 52
hiramath 0:67dece35504d 53 //角度[°]から入力パルス幅[us]に変換する関数
hiramath 0:67dece35504d 54 int cal_input0(float arg) {
hiramath 0:67dece35504d 55 return 1475 + int(10.48 * arg);
hiramath 0:67dece35504d 56 }
hiramath 0:67dece35504d 57 int cal_input1(float arg) {
hiramath 0:67dece35504d 58 //return 1520 + 10.2467 * (-30.0 + arg);
hiramath 0:67dece35504d 59 return 1218 + int(10.2467 * arg);
hiramath 0:67dece35504d 60 }
hiramath 0:67dece35504d 61 int cal_input2(float arg) {
hiramath 0:67dece35504d 62 //return 853 + 10.59 * (30.0 + arg);
hiramath 0:67dece35504d 63 return 1306 + 10.59 * arg;
hiramath 0:67dece35504d 64 }
hiramath 0:67dece35504d 65 int cal_input3(float arg) {
hiramath 0:67dece35504d 66 return 1224 + int(8.556 * arg);
hiramath 0:67dece35504d 67 }
hiramath 0:67dece35504d 68 int cal_input4(float arg) {
hiramath 0:67dece35504d 69 return 1460 + int(10.556 * arg);
hiramath 0:67dece35504d 70 }
hiramath 0:67dece35504d 71 int cal_input5(float arg) {
hiramath 0:67dece35504d 72 return 1922 + int(10.556 * arg);
hiramath 0:67dece35504d 73 }
hiramath 0:67dece35504d 74
hiramath 0:67dece35504d 75
hiramath 0:67dece35504d 76 //受け取った値を変換
hiramath 0:67dece35504d 77 void cal_Out()
hiramath 0:67dece35504d 78 {
hiramath 0:67dece35504d 79 //aOut = 1000 * num[3] + 100 * num[2] + 10 * num[1] + 1 * num[0];//
hiramath 0:67dece35504d 80 //bOut = 1000 * num[7] + 100 * num[6] + 10 * num[5] + 1 * num[4];//
hiramath 0:67dece35504d 81 //cOut = 1000 * num[11] + 100 * num[10] + 10 * num[9] + 1 * num[8];//
hiramath 0:67dece35504d 82 //dOut = 1000 * num[15] + 100 * num[14] + 10 * num[13] + 1 * num[12];//
hiramath 0:67dece35504d 83 //eOut = 1000 * num[19] + 100 * num[18] + 10 * num[17] + 1 * num[16];//
hiramath 0:67dece35504d 84 //fOut = 1000 * num[23] + 100 * num[22] + 10 * num[21] + 1 * num[20];//
hiramath 0:67dece35504d 85 //aOut = 1000 * num[4] + 100 * num[3] + 10 * num[2] + 1 * num[1];//
hiramath 0:67dece35504d 86 //bOut = 1000 * num[8] + 100 * num[7] + 10 * num[6] + 1 * num[5];//
hiramath 0:67dece35504d 87 //cOut = 1000 * num[12] + 100 * num[11] + 10 * num[10] + 1 * num[9];//
hiramath 0:67dece35504d 88 //dOut = 1000 * num[16] + 100 * num[15] + 10 * num[14] + 1 * num[13];//
hiramath 0:67dece35504d 89 //eOut = 1000 * num[20] + 100 * num[19] + 10 * num[18] + 1 * num[17];//
hiramath 0:67dece35504d 90 //fOut = 1000 * num[24] + 100 * num[23] + 10 * num[22] + 1 * num[21];//
hiramath 0:67dece35504d 91
hiramath 0:67dece35504d 92 //送られてくるのは整数2桁＋小数3桁ではなく
hiramath 0:67dece35504d 93 //1000倍されたなことに注意
hiramath 0:67dece35504d 94 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 95 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 96 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 97 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 98 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 99 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 100
hiramath 0:67dece35504d 101 //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 102 //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 103 //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 104 //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 105 //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 106 //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 107
hiramath 0:67dece35504d 108 }
hiramath 0:67dece35504d 109 void cal_pw()
hiramath 0:67dece35504d 110 {
hiramath 0:67dece35504d 111 pw0 = cal_input0(aOut);
hiramath 0:67dece35504d 112 pw1 = cal_input1(bOut);
hiramath 0:67dece35504d 113 pw2 = cal_input2(cOut);
hiramath 0:67dece35504d 114 pw3 = cal_input3(dOut);
hiramath 0:67dece35504d 115 pw4 = cal_input4(eOut);
hiramath 0:67dece35504d 116 pw5 = cal_input5(fOut);
hiramath 0:67dece35504d 117 }
hiramath 0:67dece35504d 118 void send_servo()
hiramath 0:67dece35504d 119 {
hiramath 0:67dece35504d 120 servo0.pulsewidth_us(pw0);//パルス変調幅を1625usで入力
hiramath 0:67dece35504d 121 servo1.pulsewidth_us(pw1);//パルス変調幅を1420usで入力
hiramath 0:67dece35504d 122 servo2.pulsewidth_us(pw2); //パルス変調幅を 632usで入力
hiramath 0:67dece35504d 123 servo3.pulsewidth_us(pw3);//パルス変調幅を1310usで入力
hiramath 0:67dece35504d 124 servo4.pulsewidth_us(pw4);//パルス変調幅を1368usで入力
hiramath 0:67dece35504d 125 servo5.pulsewidth_us(pw5);//パルス変調幅を1500usで入力
hiramath 0:67dece35504d 126 }
hiramath 0:67dece35504d 127
hiramath 0:67dece35504d 128 //動く関数．総まとめ
hiramath 0:67dece35504d 129 void move(){
hiramath 0:67dece35504d 130 cal_Out();
hiramath 0:67dece35504d 131 cal_pw();
hiramath 0:67dece35504d 132 send_servo();
hiramath 0:67dece35504d 133 }
hiramath 0:67dece35504d 134 int main()
hiramath 0:67dece35504d 135 {
hiramath 0:67dece35504d 136 float output[500][6];
hiramath 0:67dece35504d 137 float w = 60.0;//========制御周波数===========
hiramath 0:67dece35504d 138 float PWMperiod = 1.0 / w; //PWM周期の計算
hiramath 0:67dece35504d 139 char ch;
hiramath 0:67dece35504d 140 //===============ボーレート=============
hiramath 0:67dece35504d 141 pc.baud(115200);
hiramath 0:67dece35504d 142 //各サーボの設定
hiramath 0:67dece35504d 143 servo0.period(PWMperiod);
hiramath 0:67dece35504d 144 servo1.period(PWMperiod);
hiramath 0:67dece35504d 145 servo2.period(PWMperiod);
hiramath 0:67dece35504d 146 servo3.period(PWMperiod);
hiramath 0:67dece35504d 147 servo4.period(PWMperiod);
hiramath 0:67dece35504d 148 servo5.period(PWMperiod);
hiramath 0:67dece35504d 149
hiramath 0:67dece35504d 150 ///受け取り準備開始
hiramath 0:67dece35504d 151 //pc.printf("serial starts\n");
hiramath 0:67dece35504d 152
hiramath 0:67dece35504d 153 //シリアル通信をスタートする
hiramath 0:67dece35504d 154 //受け取り配列numの初期化
hiramath 0:67dece35504d 155 num_ini();
hiramath 0:67dece35504d 156
hiramath 0:67dece35504d 157
hiramath 0:67dece35504d 158 //while (1)
hiramath 0:67dece35504d 159 for(int counter=0;counter<100;counter++)
hiramath 0:67dece35504d 160 {
hiramath 0:67dece35504d 161
hiramath 0:67dece35504d 162 //pc.gets(buf, 31); // 31文字受け取る
hiramath 0:67dece35504d 163 for (int i = 0; i < 30; i++)
hiramath 0:67dece35504d 164 {
hiramath 0:67dece35504d 165 ch=pc.getc();//一文字ずつ受け取る
hiramath 0:67dece35504d 166 num[i] = ctoi(ch);//数字を数値に変換
hiramath 0:67dece35504d 167 }
hiramath 0:67dece35504d 168 //動かす関数
hiramath 0:67dece35504d 169 move();
hiramath 0:67dece35504d 170
hiramath 0:67dece35504d 171 //csvファイル書き込みのために配列に保存
hiramath 0:67dece35504d 172 output[counter][0]=aOut;
hiramath 0:67dece35504d 173 output[counter][1]=bOut;
hiramath 0:67dece35504d 174 output[counter][2]=cOut;
hiramath 0:67dece35504d 175 output[counter][3]=dOut;
hiramath 0:67dece35504d 176 output[counter][4]=eOut;
hiramath 0:67dece35504d 177 output[counter][5]=fOut;
hiramath 0:67dece35504d 178 //動かしたら初期化する
hiramath 0:67dece35504d 179 num_ini();
hiramath 0:67dece35504d 180
hiramath 0:67dece35504d 181 }
hiramath 0:67dece35504d 182 //保存用の部分，使わないならこめんとあうと
hiramath 0:67dece35504d 183 FILE* fp;
hiramath 0:67dece35504d 184 fp = fopen("/local/Output.csv", "w");
hiramath 0:67dece35504d 185 if(fp==NULL)
hiramath 0:67dece35504d 186 {
hiramath 0:67dece35504d 187 //pc.printf("error");
hiramath 0:67dece35504d 188 }
hiramath 0:67dece35504d 189 //while (1)
hiramath 0:67dece35504d 190 fprintf(fp, "aOut,bOut,cOut,dOut,eOut,fOut\n");
hiramath 0:67dece35504d 191 for(int j =0;j<500;j++)
hiramath 0:67dece35504d 192 {
hiramath 0:67dece35504d 193 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]);
hiramath 0:67dece35504d 194 }
hiramath 0:67dece35504d 195 fclose(fp);
hiramath 0:67dece35504d 196 //wait(0.1);
hiramath 0:67dece35504d 197
hiramath 0:67dece35504d 198 }