Serialservo.cpp

00001 #include "mbed.h"
00002 #include "Serialservo.h"
00003 
00004 long Serialservo::map(long x, long in_min, long in_max, long out_min, long out_max){
00005   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00006 }
00007 
00008 Serialservo::Serialservo(PinName TX, PinName RX, PinName Dout) : ser(TX, RX), switching(Dout){
00009 }
00010 
00011 //初期化関数
00012 void Serialservo::init(){
00013     ser.baud(115200);
00014     ser.format(8, Serial::Even, 1);
00015     //ser.attach(NULL, Serial::TxIrq);
00016     switching = 1;
00017     tx_data[0] = 0;
00018     tx_data[1] = 0;
00019     tx_data[2] = 0;
00020 }
00021 
00022 //データ送信完了待ち関数
00023 void Serialservo::flush(){
00024     //wait_ms(0.1);
00025     ser.attach(NULL, Serial::TxIrq);
00026     switching = 0;//受信モードに切替
00027 }
00028 
00029 //データ送信関数
00030 void Serialservo::transmission(){
00031     switching = 1;//送信モードに切り替え
00032     ser.putc(tx_data[0]);
00033     ser.putc(tx_data[1]);
00034     ser.putc(tx_data[2]);
00035     wait(0.005);
00036     //ser.attach(this,&Serialservo::flush, Serial::TxIrq);
00037 }
00038 
00039 //ポジション設定
00040 void Serialservo::move(char id,int pos){
00041     pos = (int)map((int)pos, 0, 270, 3500, 11500);
00042     int temp1 = (pos & 0x3F80) >> 7;
00043     int temp2 = pos & 0x7f;
00044     tx_data[0] = ICS_POS_CMD | id;
00045     tx_data[1] = (unsigned char)temp1;
00046     tx_data[2] = (unsigned char)temp2;
00047     transmission();//送信
00048 }
00049 
00050 //保持トルク設定
00051 void Serialservo::stretch(char id,int str){
00052     int temp1 = SUB_ST_COMMND;
00053     int temp2 = str;
00054     tx_data[0] = ICS_PARA_WRITE_COMMND | id;
00055     tx_data[1] = (unsigned char)temp1;
00056     tx_data[2] = (unsigned char)temp2;
00057     transmission();//送信
00058 }
00059 
00060 //スピード設定
00061 void Serialservo::speed(char id,int spe){
00062     int temp1 = SUB_SP_COMMND;
00063     int temp2 = spe;
00064     tx_data[0] = ICS_PARA_WRITE_COMMND | id;
00065     tx_data[1] = (unsigned char)temp1;
00066     tx_data[2] = (unsigned char)temp2;
00067     transmission();//送信
00068 }