Masahiro Furukawa
/
new_serial
under const
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?
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 | 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 | } |