SerialServo.cpp

00001 #include "mbed.h"
00002 #include "SerialServo.h"
00003 //#include "Servo.h"
00004 
00005 SerialServo::SerialServo(PinName TX, PinName RX) : ser(TX, RX){
00006 }
00007 
00008 //初期化関数
00009 void SerialServo::init(){
00010     ser.baud(115200);
00011     ser.format(8, Serial::Even, 1);
00012     tx_data[0] = 0;
00013     tx_data[1] = 0;
00014     tx_data[2] = 0;
00015 }
00016 
00017 
00018 //データ送信関数
00019 void SerialServo::transmission(){
00020     ser.putc(tx_data[0]);
00021     ser.putc(tx_data[1]);
00022     ser.putc(tx_data[2]);
00023     //wait_us(1000);
00024 }
00025 
00026 //ポジション設定
00027 void SerialServo::move(char id,int pos){
00028     int temp1 = (pos & 0x3F80) >> 7;
00029     int temp2 = pos & 0x7f;
00030     tx_data[0] = ICS_POS_CMD | id;
00031     tx_data[1] = (unsigned char)temp1;
00032     tx_data[2] = (unsigned char)temp2;
00033     transmission();//送信
00034 }
00035 
00036 void SerialServo::speed(char id,int spe){
00037     int temp1 = SUB_SP_COMMND;
00038     int temp2 = spe;
00039     tx_data[0] = ICS_PARA_WRITE_COMMND | id;
00040     tx_data[1] = (unsigned char)temp1;
00041     tx_data[2] = (unsigned char)temp2;
00042     transmission();//送信
00043 }
00044 
00045