Diff: Serialservo.cpp

Revision:

2:da6abd6afe26

Parent:

1:4caf52a715c2

Child:

3:42687d677641

--- a/Serialservo.cpp	Mon Nov 05 09:09:16 2018 +0000
+++ b/Serialservo.cpp	Fri Nov 23 08:38:33 2018 +0000
@@ -5,51 +5,62 @@
   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-Serialservo::Serialservo(PinName TX, PinName RX) : ser(TX, RX){
+Serialservo::Serialservo(PinName TX, PinName RX, PinName Dout) : ser(TX, RX), switching(Dout){
 }
 
+//初期化関数
 void Serialservo::init(){
     ser.baud(115200);
     ser.format(8, Serial::Even, 1);
+    ser.attach(NULL, Serial::TxIrq);
+    switching = 0;
+    tx_data[0] = 0;
+    tx_data[1] = 0;
+    tx_data[2] = 0;
 }
 
+//データ送信完了待ち関数
+void Serialservo::flush(){
+    ser.attach(NULL, Serial::TxIrq);
+    switching = 0;//受信モードに切替
+}
 
-void Serialservo::stretch(char id,int value){
-    unsigned char h,l,head;
-    int temp1, temp2;
-    head = 0xC0 | id;
-    temp1 = 0x01;
-    temp2 = value;
-    h = (unsigned char)temp1;
-    l = (unsigned char)temp2;
-    ser.putc(head);
-    ser.putc(h);
-    ser.putc(l);
+//データ送信関数
+void Serialservo::transmission(){
+    switching = 1;
+    ser.putc(tx_data[0]);
+    ser.putc(tx_data[1]);
+    ser.putc(tx_data[2]);
+    ser.attach(flush, Serial::TxIrq);
 }
 
-void Serialservo::speed(char id,int value){
-    unsigned char h,l,head;
-    int temp1, temp2;
-    head = 0xC0 | id;
-    temp1 = 0x02;
-    temp2 = value;
-    h = (unsigned char)temp1;
-    l = (unsigned char)temp2;
-    ser.putc(head);
-    ser.putc(h);
-    ser.putc(l);
+//ポジション設定
+void Serialservo::move(char id,int pos){
+    pos = (int)map((int)pos, 0, 270, 3500, 11500);
+    int temp1 = (value & 0x3F80) >> 7;
+    int temp2 = value & 0x7f;
+    tx_data[0] = ICS_POS_CMD | id;
+    tx_data[1] = (unsigned char)temp1;
+    tx_data[2] = (unsigned char)temp2;
+    transmission();//送信
 }
 
-void Serialservo::move(char id,int value){
-    value = (int)map((int)value, 270, 0, 3500, 11500);
-    unsigned char h,l,head;
-    int temp1, temp2;
-    temp1 = (value & 0x3F80) >> 7;
-    temp2 = value & 0x7f;
-    head = 0x80 | id;
-    h = (unsigned char)temp1;
-    l = (unsigned char)temp2;
-    ser.putc(head);
-    ser.putc(h);
-    ser.putc(l);
-}
\ No newline at end of file
+//保持トルク設定
+void Serialservo::stretch(char id,int str){
+    int temp1 = SUB_ST_COMMND;
+    int temp2 = str;
+    tx_data[0] = ICS_PARA_WRITE_COMMND | id;
+    tx_data[1] = (unsigned char)temp1;
+    tx_data[2] = (unsigned char)temp2;
+    transmission();//送信
+}
+
+//スピード設定
+void Serialservo::speed(char id,int spe){
+    int temp1 = SUB_SP_COMMND;
+    int temp2 = spe;
+    tx_data[0] = ICS_PARA_WRITE_COMMND | id;
+    tx_data[1] = (unsigned char)temp1;
+    tx_data[2] = (unsigned char)temp2;
+    transmission();//送信
+}