test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
KondoServo.cpp
00001 #include "KondoServo.h" 00002 00003 /*KondoServo::KondoServo():master(tx,rx) 00004 { 00005 }*/ 00006 KondoServo::~KondoServo() 00007 { 00008 } 00009 00010 00011 KondoServo::KondoServo(PinName txPin,PinName rxPin, int quantity, unsigned int baud):master(txPin,rxPin) 00012 { 00013 baudrate = baud; 00014 master.baud(baudrate); 00015 master.format(8,Serial::Even,1); 00016 mode = (quantity==1) ? single : multi; 00017 } 00018 00019 void KondoServo::init(int baud) 00020 { 00021 baudrate = baud; 00022 master.baud(baudrate); 00023 master.format(8,Serial::Even,1); 00024 } 00025 void KondoServo::init() 00026 { 00027 master.baud(baudrate); 00028 master.format(8,Serial::Even,1); 00029 } 00030 int KondoServo::set_degree(int id,float degree) 00031 { 00032 u8 pos_h,pos_l; 00033 long pos = 8000 * degree / 270 + 3500; // (11500-3500)÷270 = 29.62 ≒30 00034 pos_h = pos / 128; //上位7bit 00035 pos_l = pos % 128; //下位7bit 00036 master.putc(0x80+id);// ID 00037 master.putc(pos_h); 00038 master.putc(pos_l); 00039 /* 00040 int ret1 = master.getc() - 128; 00041 int ret2 = master.getc(); 00042 int ret3 = master.getc(); 00043 00044 return ((ret2*128+ret3)-3500)*270/8000; 00045 */ 00046 return 0; 00047 } 00048 void KondoServo::setID(u8 id) 00049 { 00050 if(mode == single) { 00051 master.putc(0xE0+id); 00052 master.putc(0x01); 00053 master.putc(0x01); 00054 master.putc(0x01); 00055 } 00056 } 00057 u8 KondoServo::readID() 00058 { 00059 if(mode == single) { 00060 master.putc(0xFF); 00061 master.putc(0x00); 00062 master.putc(0x00); 00063 master.putc(0x00); 00064 u8 readID = master.getc() - 224; 00065 return readID; 00066 } else { 00067 return 0xff; 00068 } 00069 } 00070 00071 void KondoServo::setSpeed(int id, u8 speed) 00072 { 00073 master.putc(0xC0+id); 00074 master.putc(0x02); 00075 master.putc(speed); 00076 } 00077 00078 float KondoServo::readSpeed(int id) 00079 { 00080 master.putc(0xA0); 00081 master.putc(0x02); 00082 00083 /*int ret1 = master.getc(); 00084 int ret2 = master.getc(); 00085 int ret3 = master.getc(); 00086 int ret4 = master.getc(); 00087 int ret5 = master.getc(); 00088 int ret6 = master.getc(); 00089 00090 printf("%d\n%d\n%d\n%d\n%d\n%d\n",ret1,ret2,ret3,ret4,ret5,ret6); 00091 */ 00092 00093 wait_ms(10); 00094 00095 for(int i=0; i<10; i++) { 00096 //wait_ms(10); 00097 int ret = master.getc(); 00098 printf("%d\n",ret); 00099 } 00100 00101 /*int ret=0; 00102 while(ret != 0x20+id){ 00103 ret = master.getc(); 00104 printf("%d\n",ret); 00105 }*/ 00106 //return (float)(((ret5*128+ret6)-3500)*270/8000); 00107 return 0; 00108 }
Generated on Thu Jul 14 2022 13:15:26 by 1.7.2