ケンタ ミヤザキ
Dependents: Serialservo_01 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more
Serialservo.cpp@3:42687d677641, 2018-11-24 (annotated)
- Committer:
- kenken0721
- Date:
- Sat Nov 24 02:27:41 2018 +0000
- Revision:
- 3:42687d677641
- Parent:
- 2:da6abd6afe26
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| kenken0721 | 0:2d468b5749f9 | 1 | #include "mbed.h" |
| kenken0721 | 0:2d468b5749f9 | 2 | #include "Serialservo.h" |
| kenken0721 | 0:2d468b5749f9 | 3 | |
| kenken0721 | 0:2d468b5749f9 | 4 | long Serialservo::map(long x, long in_min, long in_max, long out_min, long out_max){ |
| kenken0721 | 0:2d468b5749f9 | 5 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
| kenken0721 | 0:2d468b5749f9 | 6 | } |
| kenken0721 | 0:2d468b5749f9 | 7 | |
| kenken0721 | 2:da6abd6afe26 | 8 | Serialservo::Serialservo(PinName TX, PinName RX, PinName Dout) : ser(TX, RX), switching(Dout){ |
| kenken0721 | 0:2d468b5749f9 | 9 | } |
| kenken0721 | 0:2d468b5749f9 | 10 | |
| kenken0721 | 2:da6abd6afe26 | 11 | //初期化関数 |
| kenken0721 | 1:4caf52a715c2 | 12 | void Serialservo::init(){ |
| kenken0721 | 0:2d468b5749f9 | 13 | ser.baud(115200); |
| kenken0721 | 0:2d468b5749f9 | 14 | ser.format(8, Serial::Even, 1); |
| kenken0721 | 3:42687d677641 | 15 | //ser.attach(NULL, Serial::TxIrq); |
| kenken0721 | 3:42687d677641 | 16 | switching = 1; |
| kenken0721 | 2:da6abd6afe26 | 17 | tx_data[0] = 0; |
| kenken0721 | 2:da6abd6afe26 | 18 | tx_data[1] = 0; |
| kenken0721 | 2:da6abd6afe26 | 19 | tx_data[2] = 0; |
| kenken0721 | 0:2d468b5749f9 | 20 | } |
| kenken0721 | 0:2d468b5749f9 | 21 | |
| kenken0721 | 2:da6abd6afe26 | 22 | //データ送信完了待ち関数 |
| kenken0721 | 2:da6abd6afe26 | 23 | void Serialservo::flush(){ |
| kenken0721 | 3:42687d677641 | 24 | //wait_ms(0.1); |
| kenken0721 | 2:da6abd6afe26 | 25 | ser.attach(NULL, Serial::TxIrq); |
| kenken0721 | 2:da6abd6afe26 | 26 | switching = 0;//受信モードに切替 |
| kenken0721 | 2:da6abd6afe26 | 27 | } |
| kenken0721 | 1:4caf52a715c2 | 28 | |
| kenken0721 | 2:da6abd6afe26 | 29 | //データ送信関数 |
| kenken0721 | 2:da6abd6afe26 | 30 | void Serialservo::transmission(){ |
| kenken0721 | 3:42687d677641 | 31 | switching = 1;//送信モードに切り替え |
| kenken0721 | 2:da6abd6afe26 | 32 | ser.putc(tx_data[0]); |
| kenken0721 | 2:da6abd6afe26 | 33 | ser.putc(tx_data[1]); |
| kenken0721 | 2:da6abd6afe26 | 34 | ser.putc(tx_data[2]); |
| kenken0721 | 3:42687d677641 | 35 | wait(0.005); |
| kenken0721 | 3:42687d677641 | 36 | //ser.attach(this,&Serialservo::flush, Serial::TxIrq); |
| kenken0721 | 0:2d468b5749f9 | 37 | } |
| kenken0721 | 1:4caf52a715c2 | 38 | |
| kenken0721 | 2:da6abd6afe26 | 39 | //ポジション設定 |
| kenken0721 | 2:da6abd6afe26 | 40 | void Serialservo::move(char id,int pos){ |
| kenken0721 | 2:da6abd6afe26 | 41 | pos = (int)map((int)pos, 0, 270, 3500, 11500); |
| kenken0721 | 3:42687d677641 | 42 | int temp1 = (pos & 0x3F80) >> 7; |
| kenken0721 | 3:42687d677641 | 43 | int temp2 = pos & 0x7f; |
| kenken0721 | 2:da6abd6afe26 | 44 | tx_data[0] = ICS_POS_CMD | id; |
| kenken0721 | 2:da6abd6afe26 | 45 | tx_data[1] = (unsigned char)temp1; |
| kenken0721 | 2:da6abd6afe26 | 46 | tx_data[2] = (unsigned char)temp2; |
| kenken0721 | 2:da6abd6afe26 | 47 | transmission();//送信 |
| kenken0721 | 1:4caf52a715c2 | 48 | } |
| kenken0721 | 1:4caf52a715c2 | 49 | |
| kenken0721 | 2:da6abd6afe26 | 50 | //保持トルク設定 |
| kenken0721 | 2:da6abd6afe26 | 51 | void Serialservo::stretch(char id,int str){ |
| kenken0721 | 2:da6abd6afe26 | 52 | int temp1 = SUB_ST_COMMND; |
| kenken0721 | 2:da6abd6afe26 | 53 | int temp2 = str; |
| kenken0721 | 2:da6abd6afe26 | 54 | tx_data[0] = ICS_PARA_WRITE_COMMND | id; |
| kenken0721 | 2:da6abd6afe26 | 55 | tx_data[1] = (unsigned char)temp1; |
| kenken0721 | 2:da6abd6afe26 | 56 | tx_data[2] = (unsigned char)temp2; |
| kenken0721 | 2:da6abd6afe26 | 57 | transmission();//送信 |
| kenken0721 | 2:da6abd6afe26 | 58 | } |
| kenken0721 | 2:da6abd6afe26 | 59 | |
| kenken0721 | 2:da6abd6afe26 | 60 | //スピード設定 |
| kenken0721 | 2:da6abd6afe26 | 61 | void Serialservo::speed(char id,int spe){ |
| kenken0721 | 2:da6abd6afe26 | 62 | int temp1 = SUB_SP_COMMND; |
| kenken0721 | 2:da6abd6afe26 | 63 | int temp2 = spe; |
| kenken0721 | 2:da6abd6afe26 | 64 | tx_data[0] = ICS_PARA_WRITE_COMMND | id; |
| kenken0721 | 2:da6abd6afe26 | 65 | tx_data[1] = (unsigned char)temp1; |
| kenken0721 | 2:da6abd6afe26 | 66 | tx_data[2] = (unsigned char)temp2; |
| kenken0721 | 2:da6abd6afe26 | 67 | transmission();//送信 |
| kenken0721 | 2:da6abd6afe26 | 68 | } |