Diff: SerialServo.cpp

Revision:

0:e7ccd59b1ab3

Child:

1:a07940935629

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialServo.cpp	Sat Feb 16 08:23:51 2019 +0000
@@ -0,0 +1,35 @@
+#include "mbed.h"
+#include "SerialServo.h"
+//#include "Servo.h"
+
+SerialServo::SerialServo(PinName TX, PinName RX) : ser(TX, RX){
+}
+
+//初期化関数
+void SerialServo::init(){
+    ser.baud(115200);
+    ser.format(8, Serial::Even, 1);
+    tx_data[0] = 0;
+    tx_data[1] = 0;
+    tx_data[2] = 0;
+}
+
+
+//データ送信関数
+void SerialServo::transmission(){
+    ser.putc(tx_data[0]);
+    ser.putc(tx_data[1]);
+    ser.putc(tx_data[2]);
+    wait(0.005);
+}
+
+//ポジション設定
+void SerialServo::move(char id,int pos){
+    int temp1 = (pos & 0x3F80) >> 7;
+    int temp2 = pos & 0x7f;
+    tx_data[0] = ICS_POS_CMD | id;
+    tx_data[1] = (unsigned char)temp1;
+    tx_data[2] = (unsigned char)temp2;
+    transmission();//送信
+}
+