Masahiro Furukawa
/
new_serial
under const
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?
User | Revision | Line number | New 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 | } |