ケンタ ミヤザキ
Dependents: Serialservo_01 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more
Serialservo.cpp@2:da6abd6afe26, 2018-11-23 (annotated)
- Committer:
- kenken0721
- Date:
- Fri Nov 23 08:38:33 2018 +0000
- Revision:
- 2:da6abd6afe26
- Parent:
- 1:4caf52a715c2
- Child:
- 3:42687d677641
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 | 2:da6abd6afe26 | 15 | ser.attach(NULL, Serial::TxIrq); |
| kenken0721 | 2:da6abd6afe26 | 16 | switching = 0; |
| 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 | 2:da6abd6afe26 | 24 | ser.attach(NULL, Serial::TxIrq); |
| kenken0721 | 2:da6abd6afe26 | 25 | switching = 0;//受信モードに切替 |
| kenken0721 | 2:da6abd6afe26 | 26 | } |
| kenken0721 | 1:4caf52a715c2 | 27 | |
| kenken0721 | 2:da6abd6afe26 | 28 | //データ送信関数 |
| kenken0721 | 2:da6abd6afe26 | 29 | void Serialservo::transmission(){ |
| kenken0721 | 2:da6abd6afe26 | 30 | switching = 1; |
| kenken0721 | 2:da6abd6afe26 | 31 | ser.putc(tx_data[0]); |
| kenken0721 | 2:da6abd6afe26 | 32 | ser.putc(tx_data[1]); |
| kenken0721 | 2:da6abd6afe26 | 33 | ser.putc(tx_data[2]); |
| kenken0721 | 2:da6abd6afe26 | 34 | ser.attach(flush, Serial::TxIrq); |
| kenken0721 | 0:2d468b5749f9 | 35 | } |
| kenken0721 | 1:4caf52a715c2 | 36 | |
| kenken0721 | 2:da6abd6afe26 | 37 | //ポジション設定 |
| kenken0721 | 2:da6abd6afe26 | 38 | void Serialservo::move(char id,int pos){ |
| kenken0721 | 2:da6abd6afe26 | 39 | pos = (int)map((int)pos, 0, 270, 3500, 11500); |
| kenken0721 | 2:da6abd6afe26 | 40 | int temp1 = (value & 0x3F80) >> 7; |
| kenken0721 | 2:da6abd6afe26 | 41 | int temp2 = value & 0x7f; |
| kenken0721 | 2:da6abd6afe26 | 42 | tx_data[0] = ICS_POS_CMD | id; |
| kenken0721 | 2:da6abd6afe26 | 43 | tx_data[1] = (unsigned char)temp1; |
| kenken0721 | 2:da6abd6afe26 | 44 | tx_data[2] = (unsigned char)temp2; |
| kenken0721 | 2:da6abd6afe26 | 45 | transmission();//送信 |
| kenken0721 | 1:4caf52a715c2 | 46 | } |
| kenken0721 | 1:4caf52a715c2 | 47 | |
| kenken0721 | 2:da6abd6afe26 | 48 | //保持トルク設定 |
| kenken0721 | 2:da6abd6afe26 | 49 | void Serialservo::stretch(char id,int str){ |
| kenken0721 | 2:da6abd6afe26 | 50 | int temp1 = SUB_ST_COMMND; |
| kenken0721 | 2:da6abd6afe26 | 51 | int temp2 = str; |
| kenken0721 | 2:da6abd6afe26 | 52 | tx_data[0] = ICS_PARA_WRITE_COMMND | id; |
| kenken0721 | 2:da6abd6afe26 | 53 | tx_data[1] = (unsigned char)temp1; |
| kenken0721 | 2:da6abd6afe26 | 54 | tx_data[2] = (unsigned char)temp2; |
| kenken0721 | 2:da6abd6afe26 | 55 | transmission();//送信 |
| kenken0721 | 2:da6abd6afe26 | 56 | } |
| kenken0721 | 2:da6abd6afe26 | 57 | |
| kenken0721 | 2:da6abd6afe26 | 58 | //スピード設定 |
| kenken0721 | 2:da6abd6afe26 | 59 | void Serialservo::speed(char id,int spe){ |
| kenken0721 | 2:da6abd6afe26 | 60 | int temp1 = SUB_SP_COMMND; |
| kenken0721 | 2:da6abd6afe26 | 61 | int temp2 = spe; |
| kenken0721 | 2:da6abd6afe26 | 62 | tx_data[0] = ICS_PARA_WRITE_COMMND | id; |
| kenken0721 | 2:da6abd6afe26 | 63 | tx_data[1] = (unsigned char)temp1; |
| kenken0721 | 2:da6abd6afe26 | 64 | tx_data[2] = (unsigned char)temp2; |
| kenken0721 | 2:da6abd6afe26 | 65 | transmission();//送信 |
| kenken0721 | 2:da6abd6afe26 | 66 | } |