test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
KondoServoLibrary_old/KondoServo.cpp@50:36741e8ab197, 2019-03-11 (annotated)
- Committer:
- shimizuta
- Date:
- Mon Mar 11 10:38:07 2019 +0000
- Revision:
- 50:36741e8ab197
- Parent:
- 2:a92568bdeb5c
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 2:a92568bdeb5c | 1 | #include "KondoServo.h" |
shimizuta | 2:a92568bdeb5c | 2 | |
shimizuta | 2:a92568bdeb5c | 3 | /*KondoServo::KondoServo():master(tx,rx) |
shimizuta | 2:a92568bdeb5c | 4 | { |
shimizuta | 2:a92568bdeb5c | 5 | }*/ |
shimizuta | 2:a92568bdeb5c | 6 | KondoServo::~KondoServo() |
shimizuta | 2:a92568bdeb5c | 7 | { |
shimizuta | 2:a92568bdeb5c | 8 | } |
shimizuta | 2:a92568bdeb5c | 9 | |
shimizuta | 2:a92568bdeb5c | 10 | |
shimizuta | 2:a92568bdeb5c | 11 | KondoServo::KondoServo(PinName txPin,PinName rxPin, int quantity, unsigned int baud):master(txPin,rxPin) |
shimizuta | 2:a92568bdeb5c | 12 | { |
shimizuta | 2:a92568bdeb5c | 13 | baudrate = baud; |
shimizuta | 2:a92568bdeb5c | 14 | master.baud(baudrate); |
shimizuta | 2:a92568bdeb5c | 15 | master.format(8,Serial::Even,1); |
shimizuta | 2:a92568bdeb5c | 16 | mode = (quantity==1) ? single : multi; |
shimizuta | 2:a92568bdeb5c | 17 | } |
shimizuta | 2:a92568bdeb5c | 18 | |
shimizuta | 2:a92568bdeb5c | 19 | void KondoServo::init(int baud) |
shimizuta | 2:a92568bdeb5c | 20 | { |
shimizuta | 2:a92568bdeb5c | 21 | baudrate = baud; |
shimizuta | 2:a92568bdeb5c | 22 | master.baud(baudrate); |
shimizuta | 2:a92568bdeb5c | 23 | master.format(8,Serial::Even,1); |
shimizuta | 2:a92568bdeb5c | 24 | } |
shimizuta | 2:a92568bdeb5c | 25 | void KondoServo::init() |
shimizuta | 2:a92568bdeb5c | 26 | { |
shimizuta | 2:a92568bdeb5c | 27 | master.baud(baudrate); |
shimizuta | 2:a92568bdeb5c | 28 | master.format(8,Serial::Even,1); |
shimizuta | 2:a92568bdeb5c | 29 | } |
shimizuta | 2:a92568bdeb5c | 30 | int KondoServo::set_degree(int id,float degree) |
shimizuta | 2:a92568bdeb5c | 31 | { |
shimizuta | 2:a92568bdeb5c | 32 | u8 pos_h,pos_l; |
shimizuta | 2:a92568bdeb5c | 33 | long pos = 8000 * degree / 270 + 3500; // (11500-3500)÷270 = 29.62 ≒30 |
shimizuta | 2:a92568bdeb5c | 34 | pos_h = pos / 128; //上位7bit |
shimizuta | 2:a92568bdeb5c | 35 | pos_l = pos % 128; //下位7bit |
shimizuta | 2:a92568bdeb5c | 36 | master.putc(0x80+id);// ID |
shimizuta | 2:a92568bdeb5c | 37 | master.putc(pos_h); |
shimizuta | 2:a92568bdeb5c | 38 | master.putc(pos_l); |
shimizuta | 2:a92568bdeb5c | 39 | /* |
shimizuta | 2:a92568bdeb5c | 40 | int ret1 = master.getc() - 128; |
shimizuta | 2:a92568bdeb5c | 41 | int ret2 = master.getc(); |
shimizuta | 2:a92568bdeb5c | 42 | int ret3 = master.getc(); |
shimizuta | 2:a92568bdeb5c | 43 | |
shimizuta | 2:a92568bdeb5c | 44 | return ((ret2*128+ret3)-3500)*270/8000; |
shimizuta | 2:a92568bdeb5c | 45 | */ |
shimizuta | 2:a92568bdeb5c | 46 | return 0; |
shimizuta | 2:a92568bdeb5c | 47 | } |
shimizuta | 2:a92568bdeb5c | 48 | void KondoServo::setID(u8 id) |
shimizuta | 2:a92568bdeb5c | 49 | { |
shimizuta | 2:a92568bdeb5c | 50 | if(mode == single) { |
shimizuta | 2:a92568bdeb5c | 51 | master.putc(0xE0+id); |
shimizuta | 2:a92568bdeb5c | 52 | master.putc(0x01); |
shimizuta | 2:a92568bdeb5c | 53 | master.putc(0x01); |
shimizuta | 2:a92568bdeb5c | 54 | master.putc(0x01); |
shimizuta | 2:a92568bdeb5c | 55 | } |
shimizuta | 2:a92568bdeb5c | 56 | } |
shimizuta | 2:a92568bdeb5c | 57 | u8 KondoServo::readID() |
shimizuta | 2:a92568bdeb5c | 58 | { |
shimizuta | 2:a92568bdeb5c | 59 | if(mode == single) { |
shimizuta | 2:a92568bdeb5c | 60 | master.putc(0xFF); |
shimizuta | 2:a92568bdeb5c | 61 | master.putc(0x00); |
shimizuta | 2:a92568bdeb5c | 62 | master.putc(0x00); |
shimizuta | 2:a92568bdeb5c | 63 | master.putc(0x00); |
shimizuta | 2:a92568bdeb5c | 64 | u8 readID = master.getc() - 224; |
shimizuta | 2:a92568bdeb5c | 65 | return readID; |
shimizuta | 2:a92568bdeb5c | 66 | } else { |
shimizuta | 2:a92568bdeb5c | 67 | return 0xff; |
shimizuta | 2:a92568bdeb5c | 68 | } |
shimizuta | 2:a92568bdeb5c | 69 | } |
shimizuta | 2:a92568bdeb5c | 70 | |
shimizuta | 2:a92568bdeb5c | 71 | void KondoServo::setSpeed(int id, u8 speed) |
shimizuta | 2:a92568bdeb5c | 72 | { |
shimizuta | 2:a92568bdeb5c | 73 | master.putc(0xC0+id); |
shimizuta | 2:a92568bdeb5c | 74 | master.putc(0x02); |
shimizuta | 2:a92568bdeb5c | 75 | master.putc(speed); |
shimizuta | 2:a92568bdeb5c | 76 | } |
shimizuta | 2:a92568bdeb5c | 77 | |
shimizuta | 2:a92568bdeb5c | 78 | float KondoServo::readSpeed(int id) |
shimizuta | 2:a92568bdeb5c | 79 | { |
shimizuta | 2:a92568bdeb5c | 80 | master.putc(0xA0); |
shimizuta | 2:a92568bdeb5c | 81 | master.putc(0x02); |
shimizuta | 2:a92568bdeb5c | 82 | |
shimizuta | 2:a92568bdeb5c | 83 | /*int ret1 = master.getc(); |
shimizuta | 2:a92568bdeb5c | 84 | int ret2 = master.getc(); |
shimizuta | 2:a92568bdeb5c | 85 | int ret3 = master.getc(); |
shimizuta | 2:a92568bdeb5c | 86 | int ret4 = master.getc(); |
shimizuta | 2:a92568bdeb5c | 87 | int ret5 = master.getc(); |
shimizuta | 2:a92568bdeb5c | 88 | int ret6 = master.getc(); |
shimizuta | 2:a92568bdeb5c | 89 | |
shimizuta | 2:a92568bdeb5c | 90 | printf("%d\n%d\n%d\n%d\n%d\n%d\n",ret1,ret2,ret3,ret4,ret5,ret6); |
shimizuta | 2:a92568bdeb5c | 91 | */ |
shimizuta | 2:a92568bdeb5c | 92 | |
shimizuta | 2:a92568bdeb5c | 93 | wait_ms(10); |
shimizuta | 2:a92568bdeb5c | 94 | |
shimizuta | 2:a92568bdeb5c | 95 | for(int i=0; i<10; i++) { |
shimizuta | 2:a92568bdeb5c | 96 | //wait_ms(10); |
shimizuta | 2:a92568bdeb5c | 97 | int ret = master.getc(); |
shimizuta | 2:a92568bdeb5c | 98 | printf("%d\n",ret); |
shimizuta | 2:a92568bdeb5c | 99 | } |
shimizuta | 2:a92568bdeb5c | 100 | |
shimizuta | 2:a92568bdeb5c | 101 | /*int ret=0; |
shimizuta | 2:a92568bdeb5c | 102 | while(ret != 0x20+id){ |
shimizuta | 2:a92568bdeb5c | 103 | ret = master.getc(); |
shimizuta | 2:a92568bdeb5c | 104 | printf("%d\n",ret); |
shimizuta | 2:a92568bdeb5c | 105 | }*/ |
shimizuta | 2:a92568bdeb5c | 106 | //return (float)(((ret5*128+ret6)-3500)*270/8000); |
shimizuta | 2:a92568bdeb5c | 107 | return 0; |
shimizuta | 2:a92568bdeb5c | 108 | } |