test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
KondoServoLibrary_old/KondoServo.cpp
- Committer:
- shimizuta
- Date:
- 2019-02-08
- Revision:
- 2:a92568bdeb5c
File content as of revision 2:a92568bdeb5c:
#include "KondoServo.h" /*KondoServo::KondoServo():master(tx,rx) { }*/ KondoServo::~KondoServo() { } KondoServo::KondoServo(PinName txPin,PinName rxPin, int quantity, unsigned int baud):master(txPin,rxPin) { baudrate = baud; master.baud(baudrate); master.format(8,Serial::Even,1); mode = (quantity==1) ? single : multi; } void KondoServo::init(int baud) { baudrate = baud; master.baud(baudrate); master.format(8,Serial::Even,1); } void KondoServo::init() { master.baud(baudrate); master.format(8,Serial::Even,1); } int KondoServo::set_degree(int id,float degree) { u8 pos_h,pos_l; long pos = 8000 * degree / 270 + 3500; // (11500-3500)÷270 = 29.62 ≒30 pos_h = pos / 128; //上位7bit pos_l = pos % 128; //下位7bit master.putc(0x80+id);// ID master.putc(pos_h); master.putc(pos_l); /* int ret1 = master.getc() - 128; int ret2 = master.getc(); int ret3 = master.getc(); return ((ret2*128+ret3)-3500)*270/8000; */ return 0; } void KondoServo::setID(u8 id) { if(mode == single) { master.putc(0xE0+id); master.putc(0x01); master.putc(0x01); master.putc(0x01); } } u8 KondoServo::readID() { if(mode == single) { master.putc(0xFF); master.putc(0x00); master.putc(0x00); master.putc(0x00); u8 readID = master.getc() - 224; return readID; } else { return 0xff; } } void KondoServo::setSpeed(int id, u8 speed) { master.putc(0xC0+id); master.putc(0x02); master.putc(speed); } float KondoServo::readSpeed(int id) { master.putc(0xA0); master.putc(0x02); /*int ret1 = master.getc(); int ret2 = master.getc(); int ret3 = master.getc(); int ret4 = master.getc(); int ret5 = master.getc(); int ret6 = master.getc(); printf("%d\n%d\n%d\n%d\n%d\n%d\n",ret1,ret2,ret3,ret4,ret5,ret6); */ wait_ms(10); for(int i=0; i<10; i++) { //wait_ms(10); int ret = master.getc(); printf("%d\n",ret); } /*int ret=0; while(ret != 0x20+id){ ret = master.getc(); printf("%d\n",ret); }*/ //return (float)(((ret5*128+ret6)-3500)*270/8000); return 0; }