KrsServo.cpp

00001 #include "mbed.h"
00002 #include "KrsServo.h"
00003 #include "SerialHalfDuplex.h"
00004 
00005 //通信失敗した場合の試行回数
00006 #define MAXTRY 5
00007 
00008 KrsServo::KrsServo(PinName tx,PinName rx,int id,int baudrate):ics(tx,rx)
00009 {
00010     ics.baud(baudrate);
00011     _servoID = id;
00012 }
00013 
00014 bool KrsServo::write(unsigned char *txData,int txLen,unsigned char *rxData,int rxLen)
00015 {
00016     int rxSize;//受信した数
00017 
00018     for (int i = 0; i< txLen; i++) {
00019         ics.putc(txData[i]);
00020     }
00021     wait(0.05);
00022     for (int i = 0; i< rxLen; i++) {
00023         //if(ics.readable()) {
00024             rxData[i]= ics.getc();
00025             rxSize++;
00026         //}
00027     }
00028 
00029     //受信数確認
00030     if (rxSize != rxLen) {
00031         return false;
00032     }
00033     return true;
00034 }
00035 
00036 int KrsServo::setPosition(int postion)
00037 {
00038     //コマンドの成否
00039     bool exit_status;
00040 
00041     int txLen = 3;
00042     unsigned char txData[txLen];
00043     int rxLen = 3;
00044     unsigned char rxData[rxLen];
00045     int data;
00046 
00047     txData[0] = 0x80 | _servoID;
00048     txData[1] = (unsigned char)(postion >> 7 & 0x7F);
00049     txData[2] = (unsigned char)(postion & 0x7F);
00050 
00051     exit_status= write(txData,txLen,rxData,rxLen);
00052 
00053     data = (int)(rxData[1] & 0x7F);
00054     data = (data << 7) + (int)rxData[2];
00055 
00056     return data;
00057 }
00058 
00059 int KrsServo::setFree()
00060 {
00061     return setPosition(0);
00062 }
00063 
00064 int KrsServo::setAngle(float deg)
00065 {
00066     return setPosition(deg2pos(deg));
00067 }
00068 
00069 int KrsServo::getPosition()
00070 {
00071     //コマンドの成否
00072     bool exit_status;
00073 
00074     int txLen = 3;
00075     unsigned char txData[txLen];
00076     int rxLen = 3;
00077     unsigned char rxData[rxLen];
00078     int data;
00079 
00080     exit_status = write(txData,txLen,rxData,rxLen);
00081 
00082     data = (int)(rxData[2] & 0x7F);
00083     data = (data << 7) + (int)rxData[3];
00084 
00085     return data;
00086 }
00087 
00088 int KrsServo::setSpeed(unsigned int speed)
00089 {
00090     bool exit_status;
00091 
00092     int txLen = 3;
00093     unsigned char txData[txLen];
00094     int rxLen = 3;
00095     unsigned char rxData[rxLen];
00096     int data;
00097     
00098     txData[0] = 0xC0 + _servoID;      // CMD
00099     txData[1] = 0x02;           // SC スピード
00100     txData[2] = speed;            // スピード
00101 
00102     exit_status = write(txData,txLen,rxData,rxLen);
00103 
00104     data = rxData[2];
00105 
00106     return data;
00107 }
00108 
00109 float KrsServo::pos2deg(int pos)
00110 {
00111     pos = pos - 7500;
00112     float deg = pos  / 29.633;
00113 
00114     return deg;
00115 }
00116 
00117 int KrsServo::deg2pos(float deg)
00118 {
00119 
00120     int pos = deg * 29.633;
00121     pos = pos + 7500;
00122     return pos;
00123 }