Dependents: Serialservo_01 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more
Serialservo.cpp@1:4caf52a715c2, 2018-11-05 (annotated)
- Committer:
- kenken0721
- Date:
- Mon Nov 05 09:09:16 2018 +0000
- Revision:
- 1:4caf52a715c2
- Parent:
- 0:2d468b5749f9
- Child:
- 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 | 0:2d468b5749f9 | 8 | Serialservo::Serialservo(PinName TX, PinName RX) : ser(TX, RX){ |
kenken0721 | 0:2d468b5749f9 | 9 | } |
kenken0721 | 0:2d468b5749f9 | 10 | |
kenken0721 | 1:4caf52a715c2 | 11 | void Serialservo::init(){ |
kenken0721 | 0:2d468b5749f9 | 12 | ser.baud(115200); |
kenken0721 | 0:2d468b5749f9 | 13 | ser.format(8, Serial::Even, 1); |
kenken0721 | 0:2d468b5749f9 | 14 | } |
kenken0721 | 0:2d468b5749f9 | 15 | |
kenken0721 | 1:4caf52a715c2 | 16 | |
kenken0721 | 1:4caf52a715c2 | 17 | void Serialservo::stretch(char id,int value){ |
kenken0721 | 0:2d468b5749f9 | 18 | unsigned char h,l,head; |
kenken0721 | 0:2d468b5749f9 | 19 | int temp1, temp2; |
kenken0721 | 1:4caf52a715c2 | 20 | head = 0xC0 | id; |
kenken0721 | 1:4caf52a715c2 | 21 | temp1 = 0x01; |
kenken0721 | 1:4caf52a715c2 | 22 | temp2 = value; |
kenken0721 | 0:2d468b5749f9 | 23 | h = (unsigned char)temp1; |
kenken0721 | 0:2d468b5749f9 | 24 | l = (unsigned char)temp2; |
kenken0721 | 0:2d468b5749f9 | 25 | ser.putc(head); |
kenken0721 | 0:2d468b5749f9 | 26 | ser.putc(h); |
kenken0721 | 0:2d468b5749f9 | 27 | ser.putc(l); |
kenken0721 | 0:2d468b5749f9 | 28 | } |
kenken0721 | 1:4caf52a715c2 | 29 | |
kenken0721 | 1:4caf52a715c2 | 30 | void Serialservo::speed(char id,int value){ |
kenken0721 | 1:4caf52a715c2 | 31 | unsigned char h,l,head; |
kenken0721 | 1:4caf52a715c2 | 32 | int temp1, temp2; |
kenken0721 | 1:4caf52a715c2 | 33 | head = 0xC0 | id; |
kenken0721 | 1:4caf52a715c2 | 34 | temp1 = 0x02; |
kenken0721 | 1:4caf52a715c2 | 35 | temp2 = value; |
kenken0721 | 1:4caf52a715c2 | 36 | h = (unsigned char)temp1; |
kenken0721 | 1:4caf52a715c2 | 37 | l = (unsigned char)temp2; |
kenken0721 | 1:4caf52a715c2 | 38 | ser.putc(head); |
kenken0721 | 1:4caf52a715c2 | 39 | ser.putc(h); |
kenken0721 | 1:4caf52a715c2 | 40 | ser.putc(l); |
kenken0721 | 1:4caf52a715c2 | 41 | } |
kenken0721 | 1:4caf52a715c2 | 42 | |
kenken0721 | 1:4caf52a715c2 | 43 | void Serialservo::move(char id,int value){ |
kenken0721 | 1:4caf52a715c2 | 44 | value = (int)map((int)value, 270, 0, 3500, 11500); |
kenken0721 | 1:4caf52a715c2 | 45 | unsigned char h,l,head; |
kenken0721 | 1:4caf52a715c2 | 46 | int temp1, temp2; |
kenken0721 | 1:4caf52a715c2 | 47 | temp1 = (value & 0x3F80) >> 7; |
kenken0721 | 1:4caf52a715c2 | 48 | temp2 = value & 0x7f; |
kenken0721 | 1:4caf52a715c2 | 49 | head = 0x80 | id; |
kenken0721 | 1:4caf52a715c2 | 50 | h = (unsigned char)temp1; |
kenken0721 | 1:4caf52a715c2 | 51 | l = (unsigned char)temp2; |
kenken0721 | 1:4caf52a715c2 | 52 | ser.putc(head); |
kenken0721 | 1:4caf52a715c2 | 53 | ser.putc(h); |
kenken0721 | 1:4caf52a715c2 | 54 | ser.putc(l); |
kenken0721 | 1:4caf52a715c2 | 55 | } |