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 void 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 void KondoServo::setID(u8 id)
00047 {
00048     if(mode == single) {
00049         master.putc(0xE0+id);
00050         master.putc(0x01);
00051         master.putc(0x01);
00052         master.putc(0x01);
00053     }
00054 }
00055 u8 KondoServo::readID()
00056 {
00057     if(mode == single) {
00058         master.putc(0xFF);
00059         master.putc(0x00);
00060         master.putc(0x00);
00061         master.putc(0x00);
00062         u8 readID = master.getc() - 224;
00063         return readID;
00064     } else {
00065         return 0xff;
00066     }
00067 }
00068 
00069 void KondoServo::setSpeed(int id, u8 speed)
00070 {
00071     master.putc(0xC0+id);
00072     master.putc(0x02);
00073     master.putc(speed);
00074 }
00075 
00076 float KondoServo::readSpeed(int id)
00077 {
00078     master.putc(0xA0);
00079     master.putc(0x02);
00080 
00081     /*int ret1 = master.getc();
00082     int ret2 = master.getc();
00083     int ret3 = master.getc();
00084     int ret4 = master.getc();
00085     int ret5 = master.getc();
00086     int ret6 = master.getc();
00087 
00088     printf("%d\n%d\n%d\n%d\n%d\n%d\n",ret1,ret2,ret3,ret4,ret5,ret6);
00089     */
00090 
00091     wait_ms(10);
00092 
00093     for(int i=0; i<10; i++) {
00094         //wait_ms(10);
00095         int ret = master.getc();
00096         printf("%d\n",ret);
00097     }
00098 
00099     /*int ret=0;
00100     while(ret != 0x20+id){
00101         ret = master.getc();
00102         printf("%d\n",ret);
00103     }*/
00104     //return (float)(((ret5*128+ret6)-3500)*270/8000);
00105     return 0;
00106 }