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 }